DroneKit教程（五）：使用自定义MAVLink指令

DroneKit的实质是通过发送和接受MAVLink消息，向飞控发送控制指令、从飞控获取各种状态信息。DroneKit的所有内置功能都是基于MAVLink实现的。DroneKit内置了simple_takeoff和simple_goto指令，以及部分云台的指令；如果要使用到DroneKit中没有的指令，则需要自定义MAVLink消息并发送。

编程示例

使用SET_POSITION_TARGET_LOCAL_NED设置速度

from pymavlink import mavutildef send_nav_velocity(self, velocity_x, velocity_y, velocity_z):    # 生成SET_POSITION_TARGET_LOCAL_NED命令    msg = self.vehicle.message_factory.set_position_target_local_ned_encode(                    0,       # time_boot_ms (not used)                    0, 0,    # target system, target component                    mavutil.mavlink.MAV_FRAME_BODY_NED,   # frame                    0b0000111111000111, # type_mask (only speeds enabled)                    0, 0, 0, # x, y, z positions (not used)                    velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s                    0, 0, 0, # x, y, z acceleration (not used)                    0, 0)    # yaw, yaw_rate (not used)     # 发送指令    self.vehicle.send_mavlink(msg)    self.vehicle.flush()

使用MAV_CMD_CONDITION_YAW设置航向（yaw轴）

from pymavlink import mavutildef condition_yaw(self, heading, relative, clock_wise):    # 使用相对角度或绝对方位    if relative:        isRelative = 1    else        isRelative = 0    # 若使用相对角度，则进行顺时针或逆时针转动    if clock_wise:        direction = 1  # "heading"所对应的角度将被加和到当前朝向角    else:        direction = -1 # "heading"所对应的角度将被从当前朝向角减去    if not relative:        direction = 0    # 生成CONDITION_YAW命令    msg = self.vehicle.message_factory.command_long_encode(                    0, 0,       # target system, target component                    mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command                    0,          # confirmation                    heading,    # param 1, yaw in degrees                    0,          # param 2, yaw speed (not used)                    direction,  # param 3, direction                    isRelative, # param 4, relative or absolute degrees                     0, 0, 0)    # param 5-7, not used    # 发送指令    self.vehicle.send_mavlink(msg)    self.vehicle.flush()

MAVLink指令

MAVLink是运行ArduPilot和PX4飞控软件的无人机与外界通信的协议。MAVLink中的指令部分称为MAVLink Command Messages(MAV_CMD)。

MAV_CMD分为三种指令类型：

• MAV_CMD_NAV导航命令（Navigation commands），用于控制无人机的运动
• MAV_CMD_DO命令（Do commands），用于辅助性的功能，不会影响无人机的位置
• MAV_CMD_CONDITION命令（Conditional commands），在满足相应条件之前，阻塞下一个DO命令的执行

对MAV_CMD，最多可以同时执行一条NAV命令，一条DO或CONDITION命令。

此外，SET_POSITION_TARGET_LOCAL_NED是无人机速度和位置控制中常用的指令。

完整的命令文档可参见ArduPilot开发者文档：Copter Commands in Guided Mode以及Mission Planner中的MAV_CMD文档。

Frame

Frame of reference，意味参考系。在使用部分MAVLink指令时，需要声明使用的参考系。常用的参考系为：

Frame名称 位置参考点 位置参考方位 速度参考方位 MAV_FRAME_LOCAL_NED home NED NED MAV_FRAME_LOCAL_OFFSET_NED vehicle NED NED MAV_FRAME_BODY_OFFSET_NED vehicle FRD FRD MAV_FRAME_BODY_NED home NED FRD

名词解释：

• home - 以home点的位置（通常是无人机初始化完成的位置）作为参考点
• vehicle - 以无人机的当前位置作为参考点
• NED - North East Down，即 x轴=正北方，y轴=正东方，z轴=正下方，绝对方位朝向
• FRD - Forward Right Down，即 x轴=正前方，y轴=正右方，z轴=正下方，相对方位朝向

生成和发送MAVLink指令

生成MAVLink指令

通过调用vehicle.message_factory，可以生成“原生”的MAVLink指令。在使用前，需要

from pymavlink import mavutil

对于多数MAVLink指令，生成相应指令的函数为“小写指令名”+“_encode”。例如，对于SET_POSITION_TARGET_LOCAL_NED，我们使用message_factory.set_position_target_local_ned_encode()：

msg = vehicle.message_factory.set_position_target_local_ned_encode(    0,       # 时间戳（未使用）    0,       # 目标系统ID（自动填充）    0,       # 目标组件ID（一般设为0，向所有组件发送）    mavutil.mavlink.MAV_FRAME_BODY_NED, # 参考系    0b0000111111000111, # 使能速度控制或位置控制的bitmask，此处使能了速度控制    0, 0, 0, # x, y, z轴位置    velocity_x, velocity_y, velocity_z, # x, y, z轴速度    0, 0, 0, # x, y, z加速度（ArduCopter固件暂不支持）    0, 0)    # 目标航向角（yaw轴角度），yaw轴加速度（ArduCopter固件暂不支持）

对于一类MAVLink指令——MAV_CMD指令，我们使用message_factory.command_long_encode()函数：

msg = vehicle.message_factory.command_long_encode(    0,          # 目标系统ID（自动填充）    0,          # 目标组件ID（一般设为0，向所有组件发送）    mavutil.mavlink.MAV_CMD_CONDITION_YAW, # 指令名    0,          # 确认    heading,    # 参数1, 目标朝向（度）    0,          # 参数2, 角速度（ArduCopter固件暂不支持）    1,          # 参数3, 方向（1为顺时针，-1为逆时针，仅当参数4=1时才有效）    1,          # 参数4, 参考系（1以当前朝向为参考，0为绝对方位）    0, 0, 0)    # 参数5 ~ 7，未使用

发送MAVLink指令

使用send_mavlink()函数发送指令，flush()函数将等待指令发送成功。

vehicle.send_mavlink(msg)vehicle.flush()

版本信息

1.0 20170915 initial commit

本作品采用知识共享署名-相同方式共享 3.0 未本地化版本许可协议进行许可。