DroneKit-python入门教程教程-SITL仿真-使用自定义MAVLink指令

DroneKit-python入门教程教程-SITL仿真-使用自定义MAVLink指令

说明：

• 介绍如何使用自定义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是无人机速度和位置控制中常用的指令
• 完整的命令文档可参见Copter Commands in Guided Mode 和 Mission Commands

Frame

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

• 名词解释：

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，我们使用

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()