在Mac上使用Dronekit与SITL做飞行程序的模拟调试

背景

无人机的项目快要中期答辩了，为了在地面站（电脑）控制无人机，我们选择DroneKit来进行代码的书写。DroneKit是一个专门用于控制无人机的Python库，使用这个API我们可以更方便的使用MAVLink来给无人机发送指令。

关于python、DroneKit和SITL的安装可以参考这个：
DroneKit教程（一）：安装DroneKit和测试工具

在我的Mac上面，我使用parallels虚拟机装载Windows 10和Ubuntu。因为SITL是在Linux下开发的，并且我们将使用树莓派作为机载电脑，所以Ubuntu上面安装的是SITL、MAVProxy和DroneKit。为了方便编辑代码，我在Ubuntu和macOS上都装了Python。Windows系统只用于加载可视化的地面站MissionPlanner。

---

步骤

1. 运行SITL和MAVProxy：打开Ubuntu，按住control+option+t，新建一个终端，输入：

   cd ~/ardupilot/ArduCopter/
   ./ArduCopter.elf --home 30.75,103.93,580,0 --model quad

   home后面的参数分别是模拟机出生时的（纬度，经度，海拔，朝向），因为是四旋翼所以model为quad。

2. 再新建一个终端，输入：

   mavproxy.py --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14551 --out 10.211.55.6:14550

   这里面有两个out接口，第一个127.0.0.1:14551是Ubuntu的内部地址，这个地址将用作代码接口，第二个10.211.55.6:14550是Windows的IP地址。要查看Windows的IP地址，打开Windows，新建一个cmd窗口，输入：

   ipconfig

   找到对应的IPv4地址填入上方相应位置即可。

3. 打开Windows中的MissionPlanner，右上角连接方式选择UDP，点击connect连接，端口填写14550。这里的端口和上述第二个接口的端口保持一致。

这个时候模拟器和地面站都已经准备好了，就等待程序的运行了。

示例程序

本示例引自DroneKit教程（二）：控制Pixhawk示例

#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
© Copyright 2015-2016, 3D Robotics.
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto.
"""

from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative


# 通过本地的14551端口，使用UDP连接到SITL模拟器，注意端口与上文的第一个out保持一致
connection_string = '127.0.0.1:14551'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect(connection_string, wait_ready=True)

# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度
# 参数aTargetAltitude即为目标高度，单位为米
def arm_and_takeoff(aTargetAltitude):
    # 进行起飞前检查
    print("Basic pre-arm checks")
    # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器
    # 是否初始化完毕。若以上检查通过，则会返回True
    while not vehicle.is_armable:
        print(" Waiting for vehicle to initialise...")
        time.sleep(1)

    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True

    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        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 target altitude")
            break
        # 等待1s
        time.sleep(1)

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

# 设置在运动时，默认的空速为3m/s
print("Set default/target airspeed to 3")
# vehicle.airspeed变量可读可写，且读、写时的含义不同。
# 读取时，为无人机的当前空速；写入时，设定无人机在执行航点任务时的默认速度
vehicle.airspeed = 3

# 发送指令，让无人机前往第一个航点
print("Going towards first point for 30 seconds ...")
# LocationGlobalRelative是一个类，它由经纬度(WGS84)和相对于home点的高度组成
# 这条语句将创建一个位于南纬35.361354，东经149.165218，相对home点高20m的位置
point1 = LocationGlobalRelative(31.361354, 104.165218, 20)
# simple_goto函数将位置发送给无人机，生成一个目标航点
vehicle.simple_goto(point1)

# simple_goto函数只发送指令，不判断有没有到达目标航点
# 它可以被其他后续指令打断，此处延时30s，即让无人机朝向point1飞行30s
time.sleep(30)

# 发送指令，让无人机前往第二个航点
print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...")
# 与之前类似，这条语句创建了另一个相对home高20m的点
point2 = LocationGlobalRelative(31.363244, 104.168801, 20)
# simple_goto将目标航点发送给无人机，groundspeed=10设置飞行时的地速为10m/s
vehicle.simple_goto(point2, groundspeed=10)

# 与之前一样，延时30s
time.sleep(30)

# 发送"返航"指令
print("Returning to Launch")
# 返航，只需将无人机的飞行模式切换成"RTL(Return to Launch)"
# 无人机会自动返回home点的正上方，之后自动降落
vehicle.mode = VehicleMode("RTL")

# 退出之前，清除vehicle对象
print("Close vehicle object")
vehicle.close()

保存该示例程序为test.py，到相应目录。

测试程序

在SITL和MAVProxy正在运行、MissionPlanner连接正常的情况下，在Ubuntu中新建一个终端，cd到示例程序所在的目录，输入：

python test.py

运行后，可以看到MAVProxy终端返回的各种数据，MisssionPlanner中则可以更直观的观察飞机的轨迹、航向等等。

自家程序的效果图：