DroneKit教程（五）：使用自定義MAVLink指令
DroneKit的實質是通過發送和接受MAVLink消息，向飛控發送控制指令、從飛控獲取各種狀態信息。DroneKit的所有內置功能都是基於MAVLink實現的。DroneKit內置了simple_takeoff和simple_goto指令，以及部分雲臺的指令；如果要使用到DroneKit中沒有的指令，則需要自定義MAVLink消息併發送。
編程示例
使用SET_POSITION_TARGET_LOCAL_NED設置速度
from pymavlink import mavutil
def 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 mavutil
def 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 未本地化版本許可協議進行許可。