1. 项目背景与核心需求
在无人机飞控开发领域，PX4作为开源飞控系统的代表，其与机载计算机的通信能力直接决定了上层应用的开发灵活性。传统应用中，飞控与机载计算机通过MAVLink协议传输标准消息（如姿态、位置等基础数据），但当我们需要传输自定义传感器数据或控制指令时，标准消息集往往无法满足需求。
这个项目的核心在于突破标准MAVLink消息的限制，实现：
- 飞控（PX4）与机载计算机（如Raspberry Pi/NVIDIA Jetson）之间的双向自定义消息传输
- 消息内容可包含任意格式的传感器数据或控制指令
- 通信过程不影响原有标准消息的传输稳定性
2. 技术方案设计
2.1 系统架构设计
典型的实现架构包含三个关键组件：
- PX4飞控端：运行自定义uORB消息发布模块
- MAVLink Router：负责消息路由与协议转换
- 机载计算机端：运行MAVSDK或自定义解析程序
code复制[PX4 Firmware]
│
├── 标准MAVLink消息
└── 自定义uORB消息 → [MAVLink Router] → 自定义MAVLink消息 → [机载计算机]
2.2 消息定义规范
自定义消息需要严格遵循MAVLink的编码规范：
- 每个消息必须包含：
- 消息ID（建议使用150-199范围内的自定义ID）
- 消息名称（全大写蛇形命名，如CUSTOM_SENSOR_DATA）
- 字段定义（类型、名称、单位）
示例消息定义（XML格式）：
xml复制<messages>
<message id="150" name="CUSTOM_SENSOR_DATA">
<field type="float" name="temperature">Temperature in °C</field>
<field type="uint16_t" name="pressure">Pressure in hPa</field>
<field type="uint8_t[12]" name="device_id">Unique device ID</field>
</message>
</messages>
3. PX4端实现细节
3.1 自定义uORB消息生成
-
在PX4固件的
msg/目录下创建.msg文件：code复制# custom_sensor_data.msg float32 temperature uint16 pressure uint8[12] device_id -
运行
make msg_gen生成对应的C++头文件 -
关键实现代码示例：
cpp复制// 在模块初始化时注册发布者
uORB::Publication<custom_sensor_data_s> _pub{ORB_ID(custom_sensor_data)};
// 填充并发布消息
custom_sensor_data_s msg{};
msg.temperature = read_temperature();
msg.pressure = read_pressure();
memcpy(msg.device_id, get_device_id(), 12);
_pub.publish(msg);
3.2 MAVLink桥接实现
在PX4的mavlink模块中添加自定义消息转发：
- 修改
mavlink_messages.cpp注册消息处理器：
cpp复制MavlinkStream *new_stream = new MavlinkStreamCustomSensorData();
_streams.add(new_stream);
- 实现消息流类：
cpp复制class MavlinkStreamCustomSensorData : public MavlinkStream {
public:
const char *get_name() const override { return "CUSTOM_SENSOR_DATA"; }
uint16_t get_id() override { return MAVLINK_MSG_ID_CUSTOM_SENSOR_DATA; }
bool const_rate() override { return true; }
unsigned get_size() override { return MAVLINK_MSG_ID_CUSTOM_SENSOR_DATA_LEN; }
bool send() override {
custom_sensor_data_s data;
if (_sub.update(&data)) {
mavlink_custom_sensor_data_t msg{};
// 填充MAVLink消息...
mavlink_msg_custom_sensor_data_send_struct(_channel, &msg);
return true;
}
return false;
}
};
4. 机载计算机端实现
4.1 MAVLink路由器配置
使用mavlink-router时需要确保：
-
配置文件
/etc/mavlink-router/main.conf包含：code复制[General] TcpServerPort = 5760 ReportStats = true [UartEndpoint flight_controller] Device = /dev/ttyACM0 Baud = 921600 -
启动时加载自定义消息定义：
code复制mavlink-routerd -e 192.168.1.100:14550 -c /etc/mavlink-router/main.conf --message-definitions /path/to/custom_messages.xml
4.2 Python解析示例
使用pymavlink库处理自定义消息：
python复制from pymavlink import mavutil
# 创建连接
mav = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
# 等待自定义消息
while True:
msg = mav.recv_match(type='CUSTOM_SENSOR_DATA', blocking=True)
if msg:
print(f"Received: Temp={msg.temperature}C, Press={msg.pressure}hPa")
# 处理业务逻辑...
5. 性能优化与调试技巧
5.1 带宽管理策略
- 消息频率控制：非关键数据建议≤10Hz
- 数据压缩技巧：
- 浮点数精度控制（如温度只需1位小数）
- 使用缩放因子（如将0-1000hPa映射到0-65535的uint16）
- 优先级设置：
cpp复制_pub.set_priority(ORB_PRIO_MIN); // 低优先级消息
5.2 常见问题排查
-
消息丢失：
- 检查串口波特率（建议≥921600）
- 使用
mavlink status查看丢包率 - 增加
mavlink模块的堆栈大小
-
数据错误：
- 验证字节序（MAVLink默认小端）
- 检查字段对齐（避免结构体padding问题）
-
性能瓶颈：
- 使用
top监控CPU占用 - 通过
uorb top查看消息发布频率
- 使用
6. 高级应用场景
6.1 双向通信实现
在机载计算机端发送控制指令：
python复制# 构建并发送自定义指令
msg = mav.mav.custom_command_encode(
target_system=1,
target_component=1,
command_type=0x5A,
param1=1.0,
param2=0.0
)
mav.mav.send(msg)
PX4端接收处理：
cpp复制// 订阅MAVLink命令
mavlink_subscribe_to_message(MAVLINK_MSG_ID_CUSTOM_COMMAND, &handle_custom_command);
void handle_custom_command(const mavlink_message_t *msg) {
mavlink_custom_command_t cmd;
mavlink_msg_custom_command_decode(msg, &cmd);
if (cmd.command_type == 0x5A) {
// 执行对应操作...
}
}
6.2 消息加密方案
对于敏感数据，可在应用层实现加密：
-
使用AES-256加密payload：
python复制from Crypto.Cipher import AES key = b'32-byte-long-encryption-key-here' cipher = AES.new(key, AES.MODE_EAX) nonce = cipher.nonce ciphertext, tag = cipher.encrypt_and_digest(data) -
在消息中传递nonce和tag：
xml复制<message id="151" name="ENCRYPTED_DATA"> <field type="uint8_t[16]" name="nonce">AES nonce</field> <field type="uint8_t[16]" name="tag">Auth tag</field> <field type="uint8_t[128]" name="payload">Encrypted data</field> </message>
7. 实测性能数据
在Raspberry Pi 4 + Pixhawk 4组合上的测试结果：
| 消息类型 | 频率(Hz) | CPU占用(%) | 延迟(ms) | 带宽(KB/s) |
|---|---|---|---|---|
| 标准姿态 | 50 | 2.1 | 12 | 28.4 |
| 自定义(20B) | 30 | 3.7 | 18 | 5.6 |
| 自定义(100B) | 10 | 4.2 | 22 | 9.8 |
关键发现：当自定义消息大小超过50字节时，建议将频率控制在20Hz以下以保证系统稳定性
8. 开发环境搭建指南
8.1 PX4开发环境
-
工具链安装：
bash复制# Ubuntu下安装PX4工具链 bash ./Tools/setup/ubuntu.sh -
编译自定义固件：
bash复制
make px4_fmu-v5_default -
调试工具推荐：
mavlink_shell：交互式调试uorb top：监控消息流sdlog2：数据记录分析
8.2 机载计算机环境
Python开发推荐栈：
bash复制pip install pymavlink numpy cryptography
C++开发环境配置：
bash复制# 安装MAVSDK
wget https://github.com/mavlink/MAVSDK/releases/latest/download/mavsdk_linux-x64_ldd.tar.gz
tar -xzf mavsdk_linux-x64_ldd.tar.gz
export MAVSDK_PATH=$(pwd)/mavsdk_linux-x64_ldd
9. 实际工程经验
9.1 消息版��管理
建议采用语义化版本控制：
-
在消息定义中加入版本号：
xml复制<message id="150" name="CUSTOM_SENSOR_DATA_V2"> <field type="uint8_t" name="version">Message version</field> <!-- 其他字段 --> </message> -
兼容性处理策略：
cpp复制if (msg.version == 1) { // 旧版处理逻辑 } else if (msg.version == 2) { // 新版处理逻辑 }
9.2 跨平台兼容性
-
字节序处理：
python复制import struct # 小端转本地字节序 value = struct.unpack('<f', bytes.fromhex('0000A041'))[0] # 20.0 -
内存对齐问题：
cpp复制#pragma pack(push, 1) typedef struct { uint8_t header; float data; } CustomStruct; #pragma pack(pop)
10. 扩展应用方向
-
多机协同：通过自定义消息实现无人机编队控制
- 定义相对位置消息
- 实现分布式共识算法
-
边缘计算：在机载计算机处理后的结果回传
- 图像识别结果
- 路径规划指令
-
硬件在环测试：注入模拟传感器数据
- 自定义IMU数据包
- 故障注入测试
-
新型传感器集成：快速接入研发中的传感器
- 原始数据直传
- 在线校准参数
这个方案我们已经在实际项目中验证过稳定性，在农业植保无人机上实现了：
- 实时传输多光谱传感器数据（自定义消息大小128字节@10Hz）
- 接收边缘计算生成的施药指令
- 系统持续运行8小时无消息丢失