PX4二次开发实战：从零构建自定义飞控模块

1. PX4二次开发环境搭建

要开始PX4的二次开发，首先需要搭建完整的开发环境。这里我推荐使用Ubuntu 20.04 LTS作为开发系统，因为PX4官方对Ubuntu的支持最为完善。

开发环境主要包含以下几个部分：

1. PX4固件源码：直接从GitHub克隆最新版本
2. 工具链：包括编译器、调试工具等
3. 仿真环境：Gazebo或JMAVSim
4. 地面站软件：QGroundControl

具体安装步骤如下：

# 1. 安装依赖 sudo apt-get update sudo apt-get install git zip qtcreator cmake build-essential genromfs ninja-build -y # 2. 安装Python3和相关工具 sudo apt-get install python3 python3-pip python3-dev python3-wheel -y pip3 install --user numpy pandas jinja2 pyserial pyyaml # 3. 克隆PX4源码 git clone https://github.com/PX4/PX4-Autopilot.git --recursive cd PX4-Autopilot # 4. 安装工具链 bash ./Tools/setup/ubuntu.sh

安装完成后，可以通过以下命令验证环境是否配置成功：

make px4_sitl_default gazebo

如果看到Gazebo仿真界面弹出，说明环境配置成功。在实际开发中，我建议使用VSCode作为代码编辑器，配合PX4插件可以获得更好的开发体验。

2. 创建自定义飞控模块

PX4采用模块化架构，我们可以很方便地添加自定义模块。下面我将演示如何创建一个完整的工作队列模块。

2.1 模块文件结构

一个标准的PX4模块通常包含以下文件：

• CMakeLists.txt：模块构建配置
• Kconfig：模块菜单配置
• <模块名>.cpp：主实现文件
• <模块名>.h：头文件

以创建一个名为"custom_module"的模块为例：

cd PX4-Autopilot/src/modules mkdir custom_module cd custom_module touch CMakeLists.txt Kconfig custom_module.cpp custom_module.h

2.2 编写CMake构建文件

CMakeLists.txt内容如下：

px4_add_module( MODULE modules__custom_module MAIN custom_module COMPILE_FLAGS #-O0 # 调试时取消注释 SRCS custom_module.cpp DEPENDS px4_work_queue )

这个配置文件告诉构建系统如何编译我们的模块。DEPENDS指定了模块依赖的工作队列。

2.3 配置模块选项

Kconfig文件用于配置模块在menuconfig中的选项：

menuconfig MODULES_CUSTOM_MODULE bool "Custom Module" default n ---help--- Enable custom module for PX4.

2.4 实现模块功能

custom_module.h头文件内容：

#pragma once #include <px4_platform_common/module.h> #include <px4_platform_common/module_params.h> #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> class CustomModule : public ModuleBase<CustomModule>, public ModuleParams, public px4::ScheduledWorkItem { public: CustomModule(); ~CustomModule() override; static int task_spawn(int argc, char *argv[]); static int custom_command(int argc, char *argv[]); static int print_usage(const char *reason = nullptr); int print_status() override; private: void Run() override; // 添加你的私有成员变量和方法 };

custom_module.cpp实现文件内容：

#include "custom_module.h" CustomModule::CustomModule() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1) { // 初始化代码 } CustomModule::~CustomModule() { // 清理代码 } void CustomModule::Run() { if (should_exit()) { ScheduleClear(); exit_and_cleanup(); return; } // 主循环逻辑 PX4_INFO("Custom module running..."); } // 其他必要的方法实现...

3. 定义自定义uORB消息

uORB是PX4中用于模块间通信的轻量级发布-订阅系统。要添加自定义消息，需要创建.msg文件。

3.1 创建消息定义文件

在PX4-Autopilot/msg目录下创建custom_message.msg：

uint64 timestamp # 时间戳(微秒) float32[3] position # 三维位置 float32[3] velocity # 三维速度 uint8 status # 状态标志

3.2 注册消息

编辑msg/CMakeLists.txt，添加新消息：

set(msg_files custom_message.msg # 其他已有消息... )

3.3 生成消息头文件

执行编译命令后，系统会自动生成uORB消息头文件：

make px4_sitl_default

生成的头文件位于build/px4_sitl_default/uORB/topics/custom_message.h，可以在代码中直接包含使用。

4. 集成外部传感器驱动

假设我们要集成一个激光雷达传感器，以下是具体步骤：

4.1 创建驱动框架

在src/drivers目录下创建lidar_driver文件夹，包含以下文件：

lidar_driver/ ├── CMakeLists.txt ├── Kconfig ├── lidar_driver.cpp └── lidar_driver.h

4.2 实现驱动接口

lidar_driver.h关键部分：

#include <px4_platform_common/module.h> #include <uORB/topics/distance_sensor.h> class LidarDriver : public ModuleBase<LidarDriver> { public: LidarDriver(); ~LidarDriver(); static int task_spawn(int argc, char *argv[]); private: int init(); void publish_distance(float distance); uORB::Publication<distance_sensor_s> _distance_pub{ORB_ID(distance_sensor)}; };

4.3 添加编译配置

CMakeLists.txt内容：

px4_add_module( MODULE drivers__lidar_driver MAIN lidar_driver SRCS lidar_driver.cpp DEPENDS drivers__device )

4.4 测试驱动

编译并启动仿真：

make px4_sitl_default gazebo

在NuttShell中启动驱动：

lidar_driver start

5. 实现自定义控制算法

在PX4中实现自定义控制算法通常需要修改控制器模块或创建新的控制器。

5.1 创建控制器模块

在src/modules/controller目录下创建custom_controller：

// custom_controller.h关键部分 class CustomController : public ModuleBase<CustomController> { public: void update(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &attitude, vehicle_rates_setpoint_s &rates_sp); private: // 控制器参数和状态变量 float _kp, _ki, _kd; };

5.2 集成到PX4主循环

需要在mc_pos_control或mc_att_control模块中调用自定义控制器：

// 在适当的位置调用自定义控制器 _custom_controller.update(local_pos, attitude, _rates_sp);

5.3 参数配置

在ROMFS/px4fmu_common/init.d-posix/rcS中添加参数：

param set-default MC_CUSTOM_KP 0.5 param set-default MC_CUSTOM_KI 0.1 param set-default MC_CUSTOM_KD 0.2

6. 模块测试与调试

完善的测试是确保飞控稳定性的关键。

6.1 单元测试

为自定义模块添加测试用例：

#include <gtest/gtest.h> #include "custom_module.h" class CustomModuleTest : public ::testing::Test { protected: void SetUp() override { _module = new CustomModule(); } void TearDown() override { delete _module; } CustomModule *_module; }; TEST_F(CustomModuleTest, InitializationTest) { EXPECT_TRUE(_module->init()); }

6.2 仿真测试

使用Gazebo进行硬件在环测试：

make px4_sitl_default gazebo commander takeoff

6.3 日志分析

PX4提供了强大的日志系统，可以通过以下命令查看模块输出：

ulog2csv logfile.ulg

或者使用Flight Review在线工具分析日志。

7. 与地面站通信

实现自定义MAVLink消息与QGroundControl通信：

7.1 定义MAVLink消息

在mavlink/include/mavlink/v2.0/common/common.xml中添加：

<message id="12921" name="CUSTOM_DATA"> <field type="uint32_t" name="time_boot_ms">Timestamp</field> <field type="float" name="value1">Data field 1</field> <field type="float" name="value2">Data field 2</field> </message>

7.2 在PX4中实现消息处理

// 发送自定义消息 mavlink_custom_data_t msg; msg.time_boot_ms = hrt_absolute_time() / 1000; msg.value1 = 123.45f; msg.value2 = 678.90f; mavlink_msg_custom_data_send_struct(_mavlink->get_channel(), &msg);

7.3 QGC界面扩展

在QGroundControl中添加自定义界面显示数据：

// 在QML文件中添加自定义UI元素 Text { text: "Custom Data: " + customData.value1.toFixed(2) }

8. 性能优化技巧

在实际项目中，我总结了以下优化经验：

1. 工作队列选择：根据任务实时性要求选择适当的工作队列
2. 内存管理：避免动态内存分配，使用预分配缓冲区
3. 发布频率：合理设置uORB消息发布频率
4. 优先级设置：为关键任务分配更高优先级

关键性能指标监控代码示例：

perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; void CustomModule::Run() { perf_begin(_loop_perf); // 模块逻辑... perf_end(_loop_perf); if (_loop_count % 100 == 0) { perf_print_counter(_loop_perf); } }

9. 常见问题解决

在开发过程中可能会遇到以下问题：

1. 模块无法启动：

   • 检查Kconfig配置是否启用
   • 确认CMakeLists.txt配置正确
   • 查看启动日志排查依赖问题

2. uORB消息无法接收：

   • 确认消息定义正确
   • 检查发布者和订阅者的消息名称是否一致
   • 使用uorb top命令查看消息发布状态

3. 实时性不达标：

   • 优化算法复杂度
   • 调整任务优先级
   • 考虑使用DSP加速计算

10. 实际项目经验分享

在最近的一个农业无人机项目中，我们需要实现精准喷洒控制。通过PX4二次开发，我们：

1. 添加了流量传感器驱动
2. 实现了基于位置的速度控制算法
3. 开发了与喷洒系统的专用通信协议
4. 优化了任务执行效率，使控制周期从50ms降低到20ms

关键的技术决策点包括：

• 选择uORB而非MAVLink进行实时数据传输
• 使用工作队列而非独立线程
• 采用固定内存池管理动态数据

这些经验表明，PX4的模块化架构既保持了核心稳定性，又为特定应用需求提供了充分的灵活性。