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

896953053

0 前言

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

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

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

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

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

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

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

1 PX4模块的代码架构介绍

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

如下：



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



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



里面有个重要的函数Run



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

一个cpp文件

一个hpp文件

一个**_params.c文件

一个CMakeLists.txt文件

一个Kconfig 文件

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

2 新建分支避免弄乱源码

查看当前分支

git status



创建新分支

git branch feature_demo

切换到这个分支

git checkout feature_demo



以前的改动被提示

3 创建新模块

3.1 新件文件

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

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

CMakeLists.txt

jone_demo_params.c

JoneDemo.cpp

JoneDemo.hpp

Kconfig



3.2 编辑CMakeLists.txt文件

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

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

add_subdirectory(PositionControl)add_subdirectory(Takeoff)
px4_add_module(  MODULE modules__mc_pos_control  MAIN mc_pos_control  COMPILE_FLAGS  SRCS    MulticopterPositionControl.cpp    MulticopterPositionControl.hpp  DEPENDS    PositionControl    Takeoff    controllib    geo    SlewRate  )


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

MODULE 模块的名字改为 modules__jone_demo

MAIN后面的改为 jone_demo

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

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

px4_add_module(  MODULE modules__jone_demo  MAIN jone_demo  COMPILE_FLAGS  SRCS    JoneDemo.cpp    JoneDemo.hpp  DEPENDS    controllib    geo    SlewRate  )


3.3 编辑Kconfig文件

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

照着 mc_pos_control 的 模板改就可以了

源文件：

menuconfig MODULES_MC_POS_CONTROL  bool"mc_pos_control"  default n  ---help---    Enable support for mc_pos_control
menuconfig USER_MC_POS_CONTROL  bool"mc_pos_control running as userspace module"  default y  depends on BOARD_PROTECTED && MODULES_MC_POS_CONTROL  ---help---    Put mc_pos_control in userspace memory

修改后文件：

menuconfig MODULES_JONE_DEMO  bool"jone_demo"  default n  ---help---    Enable support for jone_demo
menuconfig USER_JONE_DEMO  bool"jone_demo running as userspace module"  default y  depends 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 backup  ScheduleDelayed(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(### Description  jone 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

第一个的编译要慢一些

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



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

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

make px4_sitl_default boardconfig

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

刚进入，终端变成这样



选择modules按回车

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



选到我们新建的任务 jone_demo



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



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





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

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

make px4_sitl_default

编译过了



5 测试

5.1 代码启动终端开启运行

运行

make px4_sitl_default gazebo

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

jone_demo start



成功输出

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

print_usage函数中有这样一行代码

PRINT_MODULE_USAGE_COMMAND("start");

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

5.2 代码启动直接运行

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

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



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

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

6 提交git

最后 git status 看下状态



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

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



多了第25行的内容

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

git add .

提交上去

git commit -m "Create Task and run"














点击“阅读原文”查看详情