论坛 › Pixhawk飞控 › 无人机 PX4 飞控 | PX4源码添加自定义模块方法

896953053 发表于 2025-2-9 19:08:13

无人机 PX4 飞控 | PX4源码添加自定义模块方法

0 前言

PX4的模块是构成PX4飞控系统的重要组成部分，负责实现特定的功能，如飞控算法、传感器数据处理、通信协议等。

每个模块负责特定的任务，例如：

navigator 模块负责航线规划与任务执行。

commander模块负责处理飞行模式切换和系统状态监控。

sensors模块负责传感器数据的采集与处理。

PX4的设计允许用户添加自定义模块，从而扩展系统的功能。模块之间通过UORB（微ORB）进行数据交换，实现高效的信息传递

本篇文章在PX4-AutoPilot 1.14.0版本中，加入了一个自定义的模块，打印了一条消息，作为PX4源码添加自定义模块方法。

1 PX4模块的代码架构介绍

在PX4的代码里 src/modules 文件夹下，有很多的不同名字的文件夹，每个文件夹就是一个模块

如下：

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSzuXq5jSvcyTsD7A3IJ5icLoUh4icickAx5mKEEcw11jqDiasOD0dxfXsSg/640?wx_fmt=png&from=appmsg

例如多旋翼位置控制任务文件夹 mc_pos_control ,点进入可以看到有一个cpp和hpp文件

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSTOdUOufvibOHPJjAshHG1twVU7aJfbhD9LkyXJVj4ibqEoATzgqGGf5A/640?wx_fmt=png&from=appmsg

打开MulticopterPositionControl.hpp文件，这个是一个头文件，头文件里面一般是一个类

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSrvwoOD2yGJEgHxHQF2ib0WyyNlDiaWd6Av2dAv6ExuicPljKHCZxZMibsA/640?wx_fmt=png&from=appmsg

里面有个重要的函数Run

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSjdYic7AsXWLMD9uSJOsa6k2cGgnvSJ7kd6SjUFCqKCVtRnXQ5ATGQlQ/640?wx_fmt=png&from=appmsg

和这个位置控制模块一样，一般的某块需要包含：

一个cpp文件

一个hpp文件

一个**_params.c文件

一个CMakeLists.txt文件

一个Kconfig 文件

上面是最小的组成部分，如果还需要其它的库，则需要像位置控制模块一样再包含其它的文件夹(PositionControl、Takeoff)，包含其它文件夹的个数不限。

2 新建分支避免弄乱源码

查看当前分支

git status

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSx2rvIPEFGtiajUhN7tSs1K7zVrtcQtiafV3L5UWqUTtZsTB6pibEemQtw/640?wx_fmt=png&from=appmsg

创建新分支

git branch feature_demo

切换到这个分支

git checkout feature_demo

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSicqqnaKBC4ibQBy2ljFAzzh5nh3VTSk0zckic5FC4yI7DtVS9891CScBg/640?wx_fmt=png&from=appmsg

以前的改动被提示

3 创建新模块

3.1 新件文件

在PX4源码里的src/modules文件夹下创建一个新文件夹命名为 jone_demo

并且在里面新建第1节提到的最小组成部分，即那几个文件分别命名为：

CMakeLists.txt

jone_demo_params.c

JoneDemo.cpp

JoneDemo.hpp

Kconfig

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSB3prJzKfaeHiaMtrZQX5qibtpicnVfeRHwGz0bLh6pj9YOVxyCHATAz0Q/640?wx_fmt=png&from=appmsg

3.2 编辑CMakeLists.txt文件

编辑CMakeLists.txt文件，该文件负责编译

还是观察mc_pos_control模块的源文件：

add_subdirectory(PositionControl)add_subdirectory(Takeoff)
px4_add_module(MODULE modules__mc_pos_controlMAIN mc_pos_controlCOMPILE_FLAGSSRCS    MulticopterPositionControl.cpp    MulticopterPositionControl.hppDEPENDS    PositionControl    Takeoff    controllib    geo    SlewRate)


自定义的模块不需要 子文件夹 PositionControl 和 Takeoff 所以前两行可以删了

MODULE 模块的名字改为 modules__jone_demo

MAIN后面的改为 jone_demo

SRCS对应的文件名称进行修改

DEPENDS中 PostionControl 和Takeoff 没有对应的子文件，所以去掉，其它的可以保留

px4_add_module(MODULE modules__jone_demoMAIN jone_demoCOMPILE_FLAGSSRCS    JoneDemo.cpp    JoneDemo.hppDEPENDS    controllib    geo    SlewRate)


3.3 编辑Kconfig文件

下面来修改Kconfig文件，该文件是生成可执行文件的

照着 mc_pos_control 的 模板改就可以了

源文件：

menuconfig MODULES_MC_POS_CONTROLbool"mc_pos_control"default n---help---    Enable support for mc_pos_control
menuconfig USER_MC_POS_CONTROLbool"mc_pos_control running as userspace module"default ydepends on BOARD_PROTECTED && MODULES_MC_POS_CONTROL---help---    Put mc_pos_control in userspace memory

修改后文件：

menuconfig MODULES_JONE_DEMObool"jone_demo"default n---help---    Enable support for jone_demo
menuconfig USER_JONE_DEMObool"jone_demo running as userspace module"default ydepends on BOARD_PROTECTED && MODULES_JONE_DEMO---help---    Put jone_demo in userspace memory

3.4 编辑**_params.c文件

下面修改参数文件 jone_demo_params.c

这个例程不需要参数，只是打印个消息，所以空的就行。

3.5 编辑**.hpp文件

下面修改头文件：JoneDemo.hpp

复制 MulticopterPositionControl.hpp 文件的内容

删除掉包含的头文件

#include"PositionControl/PositionControl.hpp"#include"Takeoff/Takeoff.hpp"

将原文件所有的 类名称 MulticopterPositionControl 替换为 JoneDemo

内容 只保留 public 里面的函数，和 private里面的Run函数和parameters_update函数，还有一个变量

classJoneDemo :public ModuleBase<JoneDemo>, public control::SuperBlock,public ModuleParams, public px4::ScheduledWorkItem{public:JoneDemo();~JoneDemo() override;
/** @see ModuleBase */staticinttask_spawn(int argc, char *argv[]);
/** @see ModuleBase */staticintcustom_command(int argc, char *argv[]);
/** @see ModuleBase */staticintprint_usage(constchar *reason = nullptr);
boolinit();
private:voidRun()override;voidparameters_update(bool force);perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};};


构造函数和析构函数是每个类都有的

task_spawn、custom_command、print_usage函数每个模块也都有

3.6 编辑**.cpp文件

下面修改cpp文件：JoneDemo.cpp

同样复制 MulticopterPositionControl.cpp 文件的内容

原包含的头文件改为

#include"JoneDemo.hpp"

删除掉包含的头文件

#include"PositionControl/ControlMath.hpp"

将原文件所有的 类名称 MulticopterPositionControl 替换为 JoneDemo

构造函数将 _vehicle_attitude_setpoint_pub 、_vel_x_deriv、_vel_y_deriv、_vel_z_deriv 删掉，内容都删除

JoneDemo::JoneDemo(bool vtol) :SuperBlock(nullptr, "MPC"),ModuleParams(nullptr),ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers){}

析构函数不用改

JoneDemo::~JoneDemo(){perf_free(_cycle_perf);}

初始化函数：

boolJoneDemo::init(){ScheduleNow();
returntrue;}

参数更新函数parameters_update内容全部删除

设置飞行状态函数 set_vehicle_states 删除

Run函数，仅保留模板内容，并加入一个打印信息

voidJoneDemo::Run(){if (should_exit()) {      exit_and_cleanup();    return;}
// reschedule backupScheduleDelayed(100_ms);
parameters_update(false);
perf_begin(_cycle_perf);    printf("hello jone\r\n");
perf_end(_cycle_perf);}

generateFailsafeSetpoint函数删除

task_spawn 函数

intJoneDemo::task_spawn(int argc, char *argv[]){JoneDemo *instance = new JoneDemo();
if (instance) {    _object.store(instance);    _task_id = task_id_is_work_queue;
    if (instance->init()) {      return PX4_OK;    }
} else {    PX4_ERR("alloc failed");}
delete instance;_object.store(nullptr);_task_id = -1;
return PX4_ERROR;}

custom_command函数

intJoneDemo::custom_command(int argc, char *argv[]){return print_usage("unknown command");}

print_usage 函数的描述修改，PRINT_MODULE_USAGE_NAME 里面的参数修改为jone_demo

intJoneDemo::print_usage(constchar *reason){if (reason) {    PX4_WARN("%s\n", reason);}
PRINT_MODULE_DESCRIPTION(    R"DESCR_STR(### Descriptionjone demo)DESCR_STR");
PRINT_MODULE_USAGE_NAME("jone_demo", "controller");PRINT_MODULE_USAGE_COMMAND("start");PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return0;}

main函数修改

extern"C"__EXPORT intjone_demo_main(int argc, char *argv[]){return JoneDemo::main(argc, argv);}

注意 jone_demo_main 这个在 jone_demo 要和 CMakeLists.txt 文件中的 MAIN jone_demo 保持一致，否则报错

4 编译

编译

make px4_sitl_default

如果报错的话，需要看具体原因进行修改

运行gazebo仿真的

make px4_sitl_default gazebo

第一个的编译要慢一些

运行起来发现：没有要打印的东西

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSABTAgc1fQrXlzzrwic97g4x2VlOWQczqzgDicicn6hm3xqnn7ycDrlB6Q/640?wx_fmt=png&from=appmsg

因为模块还没有加到PX4的运行代码里面去

下面把模块加到PX4的运行代码里面去

make px4_sitl_default boardconfig

新建了一个模块，可以通过这个指令，把模块加进去

刚进入，终端变成这样

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCS9IlPzVMDCMK0AU8kRcbb0iaf2fibrJC4iarZpmOAia1xSHv4iblwWvoGAicg/640?wx_fmt=png&from=appmsg

选择modules按回车

出来了很多的当前有的任务

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCS3kSnNQrlVIwkeupT5Ven45xhZQv4Pr8v5uUCxkJAAu4PKaibSbYUSEg/640?wx_fmt=png&from=appmsg

选到我们新建的任务 jone_demo

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCStbBGn2kO6bHMMiaFRwPzsPJGqBC0GYGsr6xlE1ydHIPvTuDp0ln0e5A/640?wx_fmt=png&from=appmsg

可以看到它前面的方框没有星号，然后按下空格建，就加上星了，也就加入了编译

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSiaUl0CZduyr5GmUlvztjKbanQONjCMEqCVJgmL0uL1O3oibQwIIwVkvA/640?wx_fmt=png&from=appmsg

按下S，弹出下面内容，再回车进行保存，

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSN0TcZxfKT1EM99auFTicN5UQfvdrJzTc4ULWusHoIGjMcI63l3PBHBA/640?wx_fmt=png&from=appmsg

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSTMsMmlicXpVWSYb7geGUTPRVyBsHIsAQXawIsF9zL7GOHVD5JShHaUA/640?wx_fmt=png&from=appmsg

最后按q，即可退出该指令

然后再编译一次，看报不报错

make px4_sitl_default

编译过了

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSMxLsAp5BTPOppjzDaH6S6po5G7qiakhnhKiarAjia4FibnKJJGwx07XyhA/640?wx_fmt=png&from=appmsg

5 测试

5.1 代码启动终端开启运行

运行

make px4_sitl_default gazebo

还是没有，需要在上面启动的终端中，先按回车，再输入指令

jone_demo start

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSfPlRNSb60g9ZjHyIMib75FN2KJuv5QMddBIIfycySriaXicianjuHbbibFQ/640?wx_fmt=png&from=appmsg

成功输出

为什么一定要在终端中 进行start才能运行呢，因为在

print_usage函数中有这样一行代码

PRINT_MODULE_USAGE_COMMAND("start");

在终端中输入了start才运行打印

5.2 代码启动直接运行

如果希望程序启动就运行，不需要外部输入指令的话，那么则需要涉及到ROMFS文件夹了，它就是负责启动的

打开该文件夹下的rc.mc.apps文件

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSUZTcdVgGle9SQpv5ysTlKb1rSSTwS0Gibe4x4Qpmxln72V6I5wGqB3A/640?wx_fmt=png&from=appmsg

在最后加一句 jone_demo start，这样程序一启动就会运行

再启动一次就会发现，直接就输出了打印信息，不用在终端输入指令了

6 提交git

最后 git status 看下状态

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSfmRNEO3vHDdNliabjiamBzVPLCiangj1NENzKiatDvI4T54ZkEfjyPkicsQ/640?wx_fmt=png&from=appmsg

上面两个箭头上修改的文件，第一个是rc.mc_apps 是为了自启动修改的

而第二个箭头default.px4board 文件则是在运行 make px4_sitl_default boardconfig指令的时候通过gui的界面自动修改的，可以打开看下

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSRx8OTz0H53lfSMYL5FHK4VkIGma7hgbODMFkicXbfWA2GsPzJ1RYw3A/640?wx_fmt=png&from=appmsg

多了第25行的内容

通过下面指令将修改的内容加上

git add .

提交上去

git commit -m "Create Task and run"

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzt7TN0qWXlGkyeQLReOFyCSDlvZePiacdDCO10egp4qzewe4Qib0osZGM9kU9HKWpwIYENJ7jURlKgQ/640?wx_fmt=png&from=appmsg

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzuic3WKiavicOiaicQAADghmUphaBia6bRveTuB78El2ib9Hn1d4tQ2Z1LUUIpcnnIqtalERiadrgd6NGhBpw/640?wx_fmt=png&from=appmsg

https://mmbiz.qpic.cn/mmbiz_png/ylvBAgCDYMaMKnUGJvU9eZJGp3DcGYFyPtGOLErGIBtFdlMJv855KQy0q8TmNuMZib4HdTDm8c9QlbjE0AXXg5Q/640?wx_fmt=other&wxfrom=5&wx_lazy=1&wx_co=1&tp=webp

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZztbjDia21cUmFRSCeP3zCOfvUaxv7Dx5878FZK0eyQqJmNSgmFWWXIX2BJhjZriarAdC5BTglNbpDug/640?wx_fmt=png&from=appmsg

https://mmbiz.qpic.cn/sz_mmbiz_png/ZUHCFZqSZzuicCsXX4tIaricWLkMjvib6JmzibeP7K5Xl2OjZ259PiabEQmsjHkHichCoiarSiaax0xNRK3icTQyU4REcpA/640?wx_fmt=png&from=appmsg

https://mmbiz.qpic.cn/sz_mmbiz_jpg/ZUHCFZqSZzsSR0RUCKGibVLNDUPWWSCbBLs2R3ovcq79dMVtF4KpjVCR3UT6ibChFEXlvksdKFQhWZiaeuBYYPMSA/640?wx_fmt=other&from=appmsg&wxfrom=5&wx_lazy=1&wx_co=1&tp=webp

https://mmbiz.qpic.cn/mmbiz_gif/US10Gcd0tQH9Pa6nvdh6CaICF2az6V5pybfOejYGmQAonWMsSLUJGhoyZcv02NIVLMSxvoUf3e6PVsSvibnruxg/640?wx_fmt=gif
点击“阅读原文”查看详情

查看完整版本: 无人机 PX4 飞控 | PX4源码添加自定义模块方法