PX4开始之旅(二)通过自定义 MAVLink 消息与 QGroundControl (QGC) 通信

核心知识点：通过自定义 MAVLink 消息与 QGroundControl (QGC) 通信

1. 通俗易懂的解释

想象一下，MAVLink 就像是无人机（飞控）和地面站（QGroundControl）之间约定好的一种“语言”。这种语言有很多标准的“词汇”和“句子”，比如用来报告无人机位置、速度、电池电量，或者发送起飞、降落指令等等。

但是，有时候你可能想让无人机做一些特别的事情，或者发送一些标准 MAVLink 语言里没有的信息。比如，你的无人机上装了一个很特别的传感器，你想把这个传感器的数据显示在 QGC 上；或者你想通过 QGC 控制一个自定义的机械臂。这时候，标准的 MAVLink 语言就不够用了。

“自定义 MAVLink 消息”就像是在这种标准“语言”里创造一些新的“词汇”和“句子”。你和 QGC 需要提前约定好这些新词汇的意思和用法。这样，当无人机发送这些新的“句子”时，QGC 就能“听懂”并做出相应的处理（比如显示数据、控制设备）。反过来，QGC 也可以使用这些新“词汇”发送指令给无人机。

现实例子：

你和你的朋友约定了一种暗号。标准的交流方式是普通话，但你们为了某个特定目的，约定用“香蕉”代表“一切正常”，“苹果”代表“需要帮助”。这样，当你说“香蕉”时，你的朋友就知道你一切安好，即使在嘈杂的环境中也能快速沟通。自定义 MAVLink 消息就是无人机和地面站之间的这种“暗号”或“扩展词汇”。

2. 抽象理解

共性 (Abstract Understanding):

自定义 MAVLink 消息是通信协议扩展的一种具体应用。在许多协议设计中，为了满足特定或未来的需求，都会预留扩展机制。MAVLink 协议作为一个开放标准的无人机通信协议，提供了定义自定义消息的能力，允许用户在不修改核心协议的情况下，增加新的数据类型和命令。

其抽象本质是在现有协议框架内，利用预留的或可扩展的字段、消息 ID 范围等，定义具有特定含义的数据结构和消息类型。这使得通信双方能够在理解标准协议的基础上，额外交换定制化的信息。

对于自定义 MAVLink 消息，核心是在飞控端和地面站端同步定义和实现新的消息结构及其处理逻辑。

潜在问题 (Potential Issues):

1. 兼容性问题： 如果地面站或飞控没有更新支持你定义的自定义消息，它们将无法识别或正确解析这些消息，可能导致通信失败或数据丢失。

2. 版本管理： 随着自定义消息的迭代和修改，需要严格的版本控制，确保飞控和地面站使用相同版本的消息定义，避免因版本不匹配导致的通信错误。

3. 消息 ID 冲突： 虽然 MAVLink 协议为自定义消息预留了 ID 范围，但在多个不同的自定义扩展之间，仍有可能出现消息 ID 冲突，需要统一管理或使用独立的方言（Dialect）文件。

4. 地面站支持程度： QGC 需要修改其代码才能完全支持自定义消息的发送、接收、显示和控制界面。这通常需要修改 QGC 的源代码并重新编译，不像标准消息那样开箱即用。

5. 性能影响： 发送大量或频率过高的自定义消息可能会占用通信带宽和处理资源，影响飞控和地面站的其他功能。

解决方案 (Solutions):

1. 兼容性问题：

   • 解决方案： 确保所有涉及自定义消息的设备（飞控、地面站、伴随计算机等）都使用相同的自定义 MAVLink 方言（XML 定义文件）生成代码，并在部署时同步更新。

2. 版本管理：

   • 解决方案： 在自定义 MAVLink 方言文件中明确版本信息。在飞控和地面站代码中加入版本检查机制，如果检测到版本不匹配，则给出警告或拒绝使用自定义消息。

   • 解决方案： 使用版本控制系统（如 Git）管理自定义方言文件，并为每个版本打标签。

3. 消息 ID 冲突：

   • 解决方案： 仔细规划自定义消息的 ID 范围，尽量使用 MAVLink 协议推荐的自定义范围。如果与第三方自定义消息集成，尝试合并方言文件或协调 ID 分配。

   • 解决方案： 为你的自定义消息创建一个独立的 MAVLink 方言文件（XML），并在其中定义你的消息和枚举类型。

4. 地面站支持程度：

   • 解决方案： 修改 QGC 源代码。这是最彻底的解决方案，需要根据自定义消息的内容和功能，在 QGC 中添加相应的解析逻辑、UI 元素（如新的仪表盘、控制面板）和发送逻辑。

   • 解决方案： 使用 QGC 的插件机制（如果可用）或外部工具/脚本。对于一些简单的数据显示，可以考虑开发 QGC 插件或使用伴随计算机上的脚本来处理自定义消息，并通过 MAVLink 或其他方式与 QGC 集成。

5. 性能影响：

   • 解决方案： 优化消息发送频率。只在必要时发送自定义消息，并根据通信带宽和处理能力限制发送频率。

   • 解决方案： 优化消息内容。只包含必要的数据，避免发送冗余信息。

   • 解决方案： 考虑使用其他通信方式。对于大量、高频的数据传输，如果条件允许，可以考虑使用其他更适合的通信方式（如 TCP/IP）通过伴随计算机进行。

3. 实现的原理

实现自定义 MAVLink 消息与 QGC 通信的主要原理包括：

1. 定义消息结构： 使用 MAVLink 官方提供的 XML 格式定义文件（称为方言文件，Dialect File），在其中描述自定义消息的名称、ID、包含的数据字段（类型、名称、单位等）以及相关的枚举类型。自定义消息的 ID 通常需要在 MAVLink 协议规定的自定义范围内选择，以避免与标准消息冲突。

2. 生成代码： 使用 MAVLink 官方提供的代码生成工具（mavgenerate.py）读取自定义方言 XML 文件，自动生成支持该自定义消息的各种编程语言（如 C++, Python）的 MAVLink 库代码。

3. 飞控端实现：

   • 将生成的 MAVLink 库代码集成到飞控固件项目中。

   • 在飞控代码中实现发送自定义消息的逻辑。这包括填充消息结构体的数据，然后调用 MAVLink 库提供的函数将消息序列化并发送出去（通过串口、UDP 等）。

   • 实现接收和处理 QGC 发送的自定义消息的逻辑（如果需要）。

4. 地面站端实现 (QGC):

   • 将包含自定义消息定义的方言 XML 文件添加到 QGC 的 MAVLink 方言目录中。

   • （通常需要）修改 QGC 的源代码：

     • 在 MAVLink 消息解析部分添加对自定义消息 ID 的识别和解析逻辑。

     • 根据自定义消息的内容，在 QGC 的用户界面中添加相应的显示元素（如新的仪表、图表）或控制元素（如按钮、滑块）。

     • 实现 QGC 发送自定义消息给飞控的逻辑（如果需要）。

   • 重新编译 QGC 源代码。

5. 通信： 飞控和 QGC 通过物理连接（如数传电台、USB）或网络连接（如 UDP）进行通信，交换标准和自定义 MAVLink 消息。

整个过程的关键在于飞控和 QGC 都使用基于同一个自定义方言 XML 文件生成的 MAVLink 库代码，从而确保双方对自定义消息的格式和含义有相同的理解。

4. 实现代码 (示例)

由于在 QGC 中实现自定义消息需要修改和编译 QGC 源代码，这里提供一个简化的 Python 示例，演示如何定义一个简单的自定义 MAVLink 消息，并使用 pymavlink 库进行发送和接收。这个示例不涉及 QGC 的具体修改，但展示了自定义消息定义和基本通信的流程。

步骤 1: 定义自定义 MAVLink 消息 (custom_messages.xml)

创建一个 XML 文件，例如 custom_messages.xml：

<?xml version="1.0"?>
<mavlink><messages><message id="15000" name="MY_CUSTOM_DATA"><description>自定义传感器数据示例</description><field type="uint64_t" name="timestamp_us">时间戳 (微秒)</field><field type="float" name="temperature">温度 (摄氏度)</field><field type="uint8_t" name="status">状态码</field><field type="char[10]" name="name">传感器名称</field></message></messages>
</mavlink>

• message id="15000": 自定义消息 ID，需要在 MAVLink 规定的自定义范围内（通常是 15000-19999）。

• name="MY_CUSTOM_DATA": 消息名称。

• field: 定义消息包含的数据字段，包括类型、名称、描述等。

步骤 2: 使用 mavgenerate.py 生成 Python 代码

下载 MAVLink 库（如果还没有）：git clone https://github.com/mavlink/mavlink.git。

进入 MAVLink 库的 pymavlink 目录，运行代码生成工具。假设 custom_messages.xml 文件也在当前目录：

python mavgenerate.py --lang=Python --wire-protocol=2.0 custom_messages.xml

这会在当前目录或指定目录生成一个名为 mavutil.py 的文件以及一个包含生成代码的子目录（例如 mavlink_dialect_name）。

步骤 3: Python 发送和接收示例

创建一个 Python 脚本，使用生成的库代码发送和接收自定义消息。

import time
import sys
import os# 添加生成的 mavlink 库路径
# 假设生成的库在当前脚本的子目录 'mavlink_custom' 中
# 你需要根据实际生成的路径进行修改
mavlink_dir = os.path.join(os.path.dirname(__file__), 'mavlink_custom')
if mavlink_dir not in sys.path:sys.path.append(mavlink_dir)# 导入生成的 mavlink 方言模块
# 模块名称取决于你的 XML 文件名和生成时的设置
# 假设生成了一个名为 'custom_messages' 的方言
from mavlink_custom import custom_messages as mavutil# --- 发送端示例 ---
def send_custom_message(mav):"""发送自定义 MY_CUSTOM_DATA 消息"""timestamp = int(time.time() * 1e6) # 微秒时间戳temperature = 25.5status = 1name = "SensorA"# 创建自定义消息# 注意：字段顺序和类型必须与 XML 定义一致msg = mavutil.MAVLink_my_custom_data_message(timestamp,temperature,status,name.encode('ascii') # 字符串需要编码为字节)# 发送消息mav.send(msg)print(f"发送自定义消息: {msg}")# --- 接收端示例 ---
def receive_messages(mav):"""接收并处理 MAVLink 消息"""print("开始接收消息...")while True:# 接收下一条消息，超时时间 1 秒msg = mav.recv_match(blocking=True, timeout=1.0)if msg is not None:print(f"收到消息: {msg.get_type()}")# 检查是否是我们的自定义消息if msg.get_type() == 'MY_CUSTOM_DATA':print("  收到自定义 MY_CUSTOM_DATA 消息:")print(f"    时间戳: {msg.timestamp_us} us")print(f"    温度: {msg.temperature} °C")print(f"    状态: {msg.status}")print(f"    名称: {msg.name.decode('ascii').strip()}") # 解码字节为字符串并去除空白# --- 主程序 ---
if __name__ == "__main__":# 模拟一个简单的 MAVLink 连接 (例如 UDP)# 在实际应用中，这里会连接到飞控或 QGC 的 MAVLink 端口# 例如: mavutil.mavlink_connection('udpin:0.0.0.0:14550')# 或者连接到串口: mavutil.mavlink_connection('/dev/ttyACM0', baud=57600)# 为了演示，我们创建一个内存中的模拟连接# 在实际应用中，你需要根据你的连接方式修改这里from pymavlink import mavutil as standard_mavutil # 导入标准的 mavutilmaster = standard_mavutil.mavlink_connection('tcp:127.0.0.1:5760', baud=57600) # 模拟一个 TCP 连接# 等待第一个心跳包，确认连接建立# master.wait_heartbeat()# print("心跳包收到，连接建立")# 简单的发送和接收演示# 在实际应用中，发送和接收通常在不同的线程或事件循环中进行# 发送示例print("\n--- 发送自定义消息 ---")# 在实际应用中，你需要使用上面生成的 mavutil 模块来创建和发送消息# 这里的 master 对象是标准的 pymavlink 连接，需要确保它能处理自定义消息# 如果使用上面生成的 custom_messages 模块，创建消息的方式如下：# msg_to_send = mavutil.MAVLink_my_custom_data_message(...)# master.mav.send(msg_to_send)# 注意：直接使用 standard_mavutil 发送自定义消息需要确保标准库也包含了该定义# 更标准的做法是使用生成的 mavutil 模块来创建消息对象，然后通过连接对象发送# 这里为了简化示例，假设 master 对象可以发送任何 MAVLink 消息对象# 在真实的自定义消息开发中，你会使用生成的 mavutil 对象来创建消息# 例如:# from mavlink_custom import custom_messages as my_mavutil# msg_to_send = my_mavutil.MAVLink_my_custom_data_message(...)# master.mav.send(msg_to_send)# 模拟发送一条自定义消息 (使用标准库模拟，实际应使用生成的库)# 需要手动构造消息字节流，或者确保标准库已包含自定义消息定义# 这是一个简化的概念演示，实际发送需要使用生成的库函数print("请注意：这里的发送部分是概念演示，实际应使用生成的 MAVLink 库创建消息对象并发送。")# send_custom_message(master.mav) # 假设 master.mav 可以处理自定义消息对象# 接收示例print("\n--- 接收消息 ---")# 在实际应用中，你需要使用上面生成的 mavutil 模块来解析消息# 这里的 master 对象是标准的 pymavlink 连接，它会尝试解析所有收到的消息# 如果收到了自定义消息，并且标准库或加载的方言包含了其定义，就可以通过 get_type() 识别receive_messages(master) # 开始接收消息 (需要有消息发送过来)

重要说明：

• 上面的 Python 代码示例是一个概念演示。在实际应用中，你需要确保 mavgenerate.py 生成的代码路径正确添加到 sys.path 中，并且导入的是你生成的自定义方言模块。

• 将自定义消息集成到 QGC 需要修改 QGC 的 C++ 源代码，这比修改 Python 脚本复杂得多，涉及到 Qt 框架、QGC 的 MAVLink 处理架构以及 UI 开发。你需要找到 QGC 中处理 MAVLink 消息的部分，添加对自定义消息的解析和处理逻辑，并在界面上添加相应的控件来显示或控制数据。

• 为了让 QGC 识别你的自定义消息，你需要将 custom_messages.xml 文件放置在 QGC 源代码的 MAVLink 方言目录中，并在编译 QGC 时确保它被包含进去。

5. 实际应用和场景

自定义 MAVLink 消息在许多特定的无人机应用中非常有用：

1. 集成自定义传感器： 当无人机搭载了非标准的传感器（如气体传感器、辐射传感器、高精度相机参数等）时，可以通过自定义消息将这些传感器的原始数据或处理后的信息发送到地面站进行显示、记录或分析。

2. 控制自定义负载： 如果无人机携带了需要地面站控制的定制设备（如投放装置、机械臂、探照灯等），可以使用自定义消息发送控制指令。

3. 传输特定状态信息： 除了标准的飞行状态，你可能想传输一些特定的系统状态信息，例如自定义设备的故障代码、工作模式、进度等。

4. 实现专有功能： 对于商业或研究用途的无人机系统，自定义消息可以用于实现一些专有的、不适合公开的功能或数据传输。

5. 地面站与伴随计算机通信： 在带有伴随计算机的无人机系统中，自定义消息可以用于伴随计算机与地面站之间的数据交换，例如伴随计算机处理的图像分析结果、路径规划信息等。

6. 调试和诊断： 在开发和调试阶段，可以使用自定义消息发送详细的内部状态、变量值等信息到地面站，方便开发者监控和诊断问题。

通过自定义 MAVLink 消息，可以极大地扩展无人机系统的功能和应用范围，实现更灵活、更专业的无人机解决方案。但这需要对 MAVLink 协议有深入理解，并具备相应的软件开发能力（包括飞控端和地面站端）。