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

RXDlwE

上节教程使用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，最后返回出发点并降落。

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

cd testtouch 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很大程度上是无法正常运行的，主要有以下几个原因：

• 
  如果是在室内，GPS无法定位，也就无法切换到Guided模式(引导)，起飞前的自检是不能通过的，就像如下这个样子，会一直等待解锁：
  
  
  结束进程请使用 CTRL+C。
  

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

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

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

---