摘自：https://mp.weixin.qq.com/s/P1EVjYiZ_bS_Mmm9EyEX3g

Pixhawk无人机扩展教程(4)---使用Dronekit编写一个控制程序

原创 CJKK 苍穹四轴DIY 2020-02-12

上节教程使用Dronekit编写了第一个脚本，运行并读取飞控当前状态参数，详细请参考 《树莓派安装Dronekit及读取飞控数据》。有同学留言不太清楚每行脚本的意思，这节有必要先做个注释补充解释一下：

 

---

# 飞控软件版本

print "Autopilot Firmware version: %s" % vehicle.version

 

# 全球定位信息（经纬度，高度相对于平均海平面）

print "Global Location: %s" % vehicle.location.global_frame

 

# 全球定位信息（经纬度，高度相对于home点）

print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame

 

# 相对home点的位置信息（向北、向东、向下）；解锁之前返回None

print "Local Location: %s" % vehicle.location.local_frame

 

# 无人机朝向（欧拉角：roll，pitch，yaw，单位为rad，范围-π到+π）

print "Attitude: %s" % vehicle.attitude

 

# 三维速度（m/s）

print "Velocity: %s" % vehicle.velocity

 

# GPS信息

print "GPS: %s" % vehicle.gps_0

 

# 地速（m/s）

print "Groundspeed: %s" % vehicle.groundspeed

 

# 空速（m/s）

print "Airspeed: %s" % vehicle.airspeed

 

# 云台信息（得到的为当前目标的roll, pitch, yaw，而非测量值。单位为度）

print "Gimbal status: %s" % vehicle.gimbal

 

# 电池信息

print "Battery: %s" % vehicle.battery

 

# EKF（拓展卡曼滤波器）状态

print "EKF OK?: %s" % vehicle.ekf_ok

 

# 超声波或激光雷达传感器状态

print "Rangefinder: %s" % vehicle.rangefinder

 

# 无人机朝向（度）

print "Heading: %s" % vehicle.heading

 

# 是否可以解锁

print "Is Armable?: %s" % vehicle.is_armable

 

# 系统状态

print "System status: %s" % vehicle.system_status.state

 

# 当前飞行模式

print "Mode: %s" % vehicle.mode.name

 

# 解锁状态

print "Armed: %s" % vehicle.armed

 

 

---

设置属性

---

少数的属性变量可以被设置，通过设置这些属性变量，可以控制无人机的运行状态。可设置的属性变量如下：

Vehicle.home_location

Vehicle.gimbal

Vehicle.airspeed

Vehicle.groundspeed

Vehicle.mode

Vehicle.armed

vehicle.disarmed

 

设置示例：

1.锁定无人机：

vehicle.disarmed = False

 

2.切换到GUIDED模式：

vehicle.mode = VehicleMode("GUIDED")

 

3.设置航点模式下，无人机飞行的地速为3.2m/s（注意：读、写groundspeed的意义不同）

vehicle.groundspeed = 3.2

 

---

读取参数

---

参数以字典（dictionary）的形式，存储在vehicle.parameters变量中。具体参数的名称即为相应的键值（key）。

 

例如，在屏幕上显示THR_MIN参数（THR_MIN代表油门处于最低时的电机怠速，以PWM值表示）：

print "Param: %s" % vehicle.parameters['THR_MIN']

 

显示全部参数：

print "\nPrint all parameters (iterate `vehicle.parameters`):"

for key, value in vehicle.parameters.iteritems():

    print " Key:%s Value:%s" % (key,value)

 

---

设置参数

---

使用读取参数类似的方法，即可设置参数：

vehicle.parameters['THR_MIN']=100

 

以上只是截取了部分使用较多的知识点进行介绍，大家可以到官网查看详细的Dronekit教程。

 

 

---

下面我们根据官方示例编写一个程序，展示如何连接到无人机，控制无人机解锁后升空到10m，依次朝两个方向飞行30s，最后返回出发点并降落。

 

1. 笔记本端通过ssh命令连接树莓派，在test文件夹下,新建example1.py文件:

    

cd test

touch example1.py

 

可以输入 ls 命令，查看是否新建好这个文件，如下所示：

   

2.用 vim编辑文件：

sudo vim example1.py

 

将以下代码复制到文件中，中文注释也可一并复制：

---

#!/usr/bin/envpython 

# -*- coding: utf-8-*- 

 

"""

Copyright2015-2016, 3D Robotics.

simple_goto.py:GUIDED mode "simple goto" example (Copter Only)

Demonstrates how toarm and takeoff in Copter and how to navigate to points usingVehicle.simple_goto.

""" 

 

from __future__import print_function 

import time 

from dronekitimport connect, VehicleMode, LocationGlobalRelative 

 

 

# 改为当前连接的pixhawk飞控的端口 

connection_string ='/dev/ttyUSB0' 

print('Connectingto vehicle on: %s' % connection_string) 

# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle 

# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机 

vehicle =connect(connection_string, wait_ready=True,baud=921600) 

 

# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度 

# 参数aTargetAltitude即为目标高度，单位为米 

defarm_and_takeoff(aTargetAltitude): 

    # 进行起飞前检查 

    print("Basic pre-armchecks") 

    # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器 

    # 是否初始化完毕。若以上检查通过，则会返回True

    while not vehicle.is_armable: 

        print(" Waiting for vehicle toinitialise...") 

        time.sleep(1) 

 

    # 解锁无人机（电机将开始旋转） 

    print("Arming motors") 

    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机） 

    vehicle.mode =VehicleMode("GUIDED") 

    # 通过设置vehicle.armed状态变量为True，解锁无人机 

    vehicle.armed = True 

 

    # 在无人机起飞之前，确认电机已经解锁 

    while not vehicle.armed: 

        print(" Waiting forarming...") 

        time.sleep(1) 

 

    # 发送起飞指令 

    print("Taking off!") 

    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度 

   vehicle.simple_takeoff(aTargetAltitude) 

 

    # 在无人机上升到目标高度之前，阻塞程序 

    while True: 

        print(" Altitude: ",vehicle.location.global_relative_frame.alt) 

        # 当高度上升到目标高度的0.95倍时，即认为达到了目标高度，退出循环 

        #vehicle.location.global_relative_frame.alt为相对于home点的高度 

        if vehicle.location.global_relative_frame.alt>= aTargetAltitude * 0.95: 

            print("Reached targetaltitude") 

            break 

        # 等待1s 

        time.sleep(1) 

 

# 调用上面声明的arm_and_takeoff函数，目标高度10m 

arm_and_takeoff(10) 

 

# 设置在运动时，默认的空速为3m/s 

print("Setdefault/target airspeed to 3") 

# vehicle.airspeed变量可读可写，且读、写时的含义不同。 

# 读取时，为无人机的当前空速；写入时，设定无人机在执行航点任务时的默认速度 

vehicle.airspeed =3 

 

# 发送指令，让无人机前往第一个航点 

print("Goingtowards first point for 30 seconds ...") 

#LocationGlobalRelative是一个类，它由经纬度(WGS84)和相对于home点的高度组成 

# 这条语句将创建一个位于南纬35.361354，东经149.165218，相对home点高20m的位置 .注意：如要实际测试，此处的经纬度需要改成你当前测试位置的经纬度，不然飞机飞跑了！！！

point1 =LocationGlobalRelative(-35.361354, 149.165218, 20) 

# simple_goto函数将位置发送给无人机，生成一个目标航点 

vehicle.simple_goto(point1) 

 

# simple_goto函数只发送指令，不判断有没有到达目标航点 

# 它可以被其他后续指令打断，此处延时30s，即让无人机朝向point1飞行30s 

time.sleep(30) 

 

# 发送指令，让无人机前往第二个航点 

print("Goingtowards second point for 30 seconds (groundspeed set to 10 m/s) ...") 

# 与之前类似，这条语句创建了另一个相对home高20m的点。 注意：同理，如要实际测试，此处的经纬度也需要改成你当前测试位置的经纬度！！！

point2 =LocationGlobalRelative(-35.363244, 149.168801, 20) 

# simple_goto将目标航点发送给无人机，groundspeed=10设置飞行时的地速为10m/s 

vehicle.simple_goto(point2,groundspeed=10) 

 

# 与之前一样，延时30s 

time.sleep(30) 

 

# 发送"返航"指令 

print("Returningto Launch") 

# 返航，只需将无人机的飞行模式切换成"RTL(Return to Launch)" 

# 无人机会自动返回home点的正上方，之后自动降落 

vehicle.mode =VehicleMode("RTL") 

 

# 退出之前，清除vehicle对象 

print("Closevehicle object") 

vehicle.close() 

 

 

 

---

以上代码编辑保存好以后，执行 python example1.py很大程度上是无法正常运行的，主要有以下几个原因：

1. 如果是在室内，GPS无法定位，也就无法切换到Guided模式(引导)，起飞前的自检是不能通过的，就像如下这个样子，会一直等待解锁：

    

   结束进程请使用 CTRL+C。

 

 

 

2.注意：如果要在室外测试，请务必修改航点1和航点2的经纬度数据，示例里面的经纬度在堪培拉，否则飞机一去不复返。。。

 

做到这里肯定有同学会问：既然在室内又不能测试，那怎么知道执行效果如何？无人机执行动作是否和设计的初衷一致？怎样安全的验证代码的正确性？要解决这些实际问题，就需要用到SITL在环仿真。下节教程我们就介绍这些内容。

疫情期间大家务必少出门，勤洗手，多通风！也是难得有闲时间可以宅在家里学习一下以上教程。希望疫情尽快结束，回归正常状态。请大家继续关注！

 

 

 

---