当前位置：首页 > news >正文

基于PX4和Ardupilot固件下自定义MAVLink消息测试（QGroundControl和Mission Planner）

news 2025/4/2 16:48:23

在无人机行业，MAVLink（Micro Air Vehicle Link）协议已经成为了通信的标准协议。MAVLink协议定义了一种轻量级的消息传输格式，广泛应用于PX4和ArduPilot等开源无人机飞控固件中。通过自定义MAVLink消息，我们可以为无人机添加特定功能或扩展现有功能。本文将详细介绍如何在PX4和ArduPilot固件下自定义MAVLink消息，并使用QGroundControl和Mission Planner进行测试。

一、创建自定义MAVLink消息文件

MAVLink的消息格式定义是通过XML文件来描述的。在这个XML文件中，我们可以定义自定义的消息类型、字段及其数据类型。

以下是一个简单的自定义消息示例，它用于请求云台设备消息：

<?xml version="1.0"?>
<mavlink><message id="154" name="GIMBAL_DEVICE_INFORMATION"><description>Gimbal device information message, sent in response to MAV_CMD_REQUEST_MESSAGE (512).</description><!-- Message fields --><field type="uint32_t" name="time_boot_ms">Time since boot in milliseconds</field><field type="uint64_t" name="uid">Unique identifier for the gimbal</field><field type="uint32_t" name="firmware_version">Firmware version of the gimbal</field><field type="uint32_t" name="hardware_version">Hardware version of the gimbal</field><field type="float" name="pitch_max">Maximum pitch angle in radians</field><field type="float" name="pitch_min">Minimum pitch angle in radians</field><field type="float" name="roll_max">Maximum roll angle in radians</field><field type="float" name="roll_min">Minimum roll angle in radians</field><field type="float" name="yaw_max">Maximum yaw angle in radians</field><field type="float" name="yaw_min">Minimum yaw angle in radians</field><field type="uint32_t" name="cap_flags">Capabilities flags</field><field type="uint32_t" name="custom_cap_flags">Custom capability flags</field><field type="string" name="vendor_name">Vendor name of the gimbal</field><field type="string" name="model_name">Model name of the gimbal</field><field type="string" name="custom_name">Custom name of the gimbal</field></message>
</mavlink>

XML文件解析：
<message id="154" name="GIMBAL_DEVICE_INFORMATION">：这是定义消息的标识符和名称。这里的ID为154，这是您在代码中看到的ack_id。名称为GIMBAL_DEVICE_INFORMATION，描述了消息内容是云台设备信息。

<field type="uint32_t" name="time_boot_ms">：每个字段都定义了数据类型和名称。time_boot_ms字段类型为uint32_t，表示系统启动以来的时间（以毫秒为单位）。

<field type="float" name="pitch_max">、<field type="float" name="yaw_min"> 等：这些字段描述了云台设备的最大和最小角度（俯仰角、滚转角、偏航角），它们的类型是float，表示浮动精度的小数。

<field type="uint32_t" name="cap_flags">：这个字段定义了云台的能力标志，它的类型是uint32_t，表示一个32位的无符号整数。

<field type="string" name="vendor_name">、<field type="string" name="model_name"> 等：这些字段描述了云台设备的厂商名称、型号名称和自定义名称，字段类型是string，表示字符串。

二、编写测试脚本

我这里选择通过python导入mavlink包去测试，简单方便

1. 导入库和设置连接

from pymavlink import mavutil
import time

pymavlink 是一个 Python 库，用于通过 MAVLink 协议与飞行控制器或其他设备进行通信。
time 库用于设置延时（如：等待1秒再发送下一次请求）。

2. 设置串口连接参数

SERIAL_PORT = 'COM35'   # Windows 示例，Linux 用 '/dev/ttyUSB0'
BAUD_RATE = 115200      # 修改为你的设备波特率master = mavutil.mavlink_connection(SERIAL_PORT, baud=BAUD_RATE)

SERIAL_PORT：设置了与MAVLink设备（如飞控）连接的串口端口。对于Windows系统通常是COMx，Linux系统通常是/dev/ttyUSB0等。
BAUD_RATE：设置串口波特率，通常为115200，这个值需要根据设备配置进行调整。
mavutil.mavlink_connection：创建一个MAVLink连接实例，用于和设备通信。

3. 发送心跳包和请求消息

message_id = 512
target_system = 1
target_component = 154
ack_id = 283while True:# 发送心跳包master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,mavutil.mavlink.MAV_AUTOPILOT_INVALID,0, 0, 0)

心跳包：心跳包是MAVLink通信中定期发送的数据包，用来告知其他设备自己在线。这里，MAV_TYPE_GCS 表示地面站类型的心跳包，MAV_AUTOPILOT_INVALID 表示未指定自动驾驶仪类型。
master.mav.heartbeat_send()：发送心跳包，用于维护设备之间的连接。

    # 发送 MAV_CMD_REQUEST_MESSAGE (512)master.mav.command_long_send(target_system, target_component, 512, 0, 283, 0, 0, 0, 0, 0, 0)

请求云台设备信息：通过发送MAV_CMD_REQUEST_MESSAGE命令（命令ID为512）请求设备信息。

target_system 和 target_component：目标系统和目标组件的标识符，这里目标系统为1，目标组件为154（表示云台设备）。
283：这是MAV_CMD_REQUEST_MESSAGE命令的“ack_id”，用来标识这条命令的特定请求。

4. 接收和解析返回的MAVLink消息

    msg = master.recv_match(blocking=True)if msg:if msg.get_msgId() == ack_id:# 打印消息的字段内容，检查实际的字段名msg_dict = msg.to_dict()print(f"收到消息, 消息内容: {msg_dict}")

master.recv_match(blocking=True)：接收一个MAVLink消息，如果没有收到消息，程序会阻塞直到接收到消息。
msg.get_msgId()：获取返回消息的ID，如果它与预期的ack_id（即283）匹配，表示这是我们请求的云台设备信息的响应。
msg.to_dict()：将接收到的消息转为字典格式，方便后续操作。

5. 解析云台设备信息消息

time_boot_ms = msg_dict.get('time_boot_ms', '未知')
uid = msg_dict.get('uid', '未知')
firmware_version = msg_dict.get('firmware_version', '未知')
hardware_version = msg_dict.get('hardware_version', '未知')
pitch_max = msg_dict.get('pitch_max', '未知')
pitch_min = msg_dict.get('pitch_min', '未知')
roll_max = msg_dict.get('roll_max', '未知')
roll_min = msg_dict.get('roll_min', '未知')
yaw_max = msg_dict.get('yaw_max', '未知')
yaw_min = msg_dict.get('yaw_min', '未知')
cap_flags = msg_dict.get('cap_flags', '未知')
custom_cap_flags = msg_dict.get('custom_cap_flags', '未知')
vendor_name = msg_dict.get('vendor_name', '未知')
model_name = msg_dict.get('model_name', '未知')
custom_name = msg_dict.get('custom_name', '未知')

通过msg_dict.get() 方法从返回的字典中提取各个字段的值，如果某个字段没有值，则返回默认值’未知’。

time_boot_ms：系统启动以来的时间（毫秒）。
uid：云台的唯一标识符。
firmware_version：固件版本。
hardware_version：硬件版本。
pitch_max, pitch_min：云台的最大和最小俯仰角。
roll_max, roll_min：云台的最大和最小滚转角。
yaw_max, yaw_min：云台的最大和最小偏航角。
cap_flags：云台的能力标志。
custom_cap_flags：云台的自定义能力标志。
vendor_name：厂商名称。
model_name：型号名称。
custom_name：自定义名称。

6. 输出解析结果

print(f"Time Boot MS: {time_boot_ms} ms")
print(f"UID: {uid}")
print(f"Firmware Version: {firmware_version}")
print(f"Hardware Version: {hardware_version}")
print(f"Pitch Max: {pitch_max} rad")
print(f"Pitch Min: {pitch_min} rad")
print(f"Roll Max: {roll_max} rad")
print(f"Roll Min: {roll_min} rad")
print(f"Yaw Max: {yaw_max} rad")
print(f"Yaw Min: {yaw_min} rad")
print(f"Capability Flags: {cap_flags}")
print(f"Custom Capability Flags: {custom_cap_flags}")
print(f"Vendor Name: {vendor_name}")
print(f"Model Name: {model_name}")
print(f"Custom Name: {custom_name}")

通过打印输出的方式将解析出来的云台设备信息展示在控制台上，方便查看。

7. 延时等待下一次请求

    # 等待 1 秒后再发送下一个请求# time.sleep(1)

time.sleep(1)：等待1秒后再发送下一次请求（这部分被注释掉了，表示发送请求后不会立即休眠）。

代码示例：

#请求云台设备消息 MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATIONfrom pymavlink import mavutil
import time# 连接到 MAVLink 设备
SERIAL_PORT = 'COM35'   # Windows 示例，Linux 用 '/dev/ttyUSB0'
BAUD_RATE = 115200      # 修改为你的设备波特率master = mavutil.mavlink_connection(SERIAL_PORT, baud=BAUD_RATE)# 发送心跳包和 MAV_CMD_REQUEST_MESSAGE 每秒一次
message_id = 512
target_system = 1
target_component = 154
ack_id = 283while True:# 发送心跳包master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,mavutil.mavlink.MAV_AUTOPILOT_INVALID,0, 0, 0)# print("已发送心跳包")# 发送 MAV_CMD_REQUEST_MESSAGE (512)master.mav.command_long_send(target_system, target_component, 512, 0, 283, 0, 0, 0, 0, 0, 0)# print(f"已发送 MAV_CMD_REQUEST_MESSAGE ({message_id}) 请求")# 监听返回的 MAVLink 消息并解析特定消息 IDmsg = master.recv_match(blocking=True)# print(f"msg: {msg} ")if msg:# 通过 msg.id 过滤需要的消息if msg.get_msgId() == ack_id:# 打印消息的字段内容，检查实际的字段名msg_dict = msg.to_dict()print(f"收到消息, 消息内容: {msg_dict}")# 解析字段time_boot_ms = msg_dict.get('time_boot_ms', '未知')uid = msg_dict.get('uid', '未知')firmware_version = msg_dict.get('firmware_version', '未知')hardware_version = msg_dict.get('hardware_version', '未知')pitch_max = msg_dict.get('pitch_max', '未知')pitch_min = msg_dict.get('pitch_min', '未知')roll_max = msg_dict.get('roll_max', '未知')roll_min = msg_dict.get('roll_min', '未知')yaw_max = msg_dict.get('yaw_max', '未知')yaw_min = msg_dict.get('yaw_min', '未知')cap_flags = msg_dict.get('cap_flags', '未知')custom_cap_flags = msg_dict.get('custom_cap_flags', '未知')vendor_name = msg_dict.get('vendor_name', '未知')model_name = msg_dict.get('model_name', '未知')custom_name = msg_dict.get('custom_name', '未知')# 输出解析后的数据print(f"Time Boot MS: {time_boot_ms} ms")print(f"UID: {uid}")print(f"Firmware Version: {firmware_version}")print(f"Hardware Version: {hardware_version}")print(f"Pitch Max: {pitch_max} rad")print(f"Pitch Min: {pitch_min} rad")print(f"Roll Max: {roll_max} rad")print(f"Roll Min: {roll_min} rad")print(f"Yaw Max: {yaw_max} rad")print(f"Yaw Min: {yaw_min} rad")print(f"Capability Flags: {cap_flags}")print(f"Custom Capability Flags: {custom_cap_flags}")print(f"Vendor Name: {vendor_name}")print(f"Model Name: {model_name}")print(f"Custom Name: {custom_name}")# 等待 1 秒后再发送下一个请求# time.sleep(1)