PX4从放弃到精通（十五）：二次开发基础

PX4从放弃到精通（十五）：二次开发基础

👉👉👉**无人机硬件，提供全程指导**👈👈👈

文章目录

• ==👉👉👉**[无人机硬件，提供全程指导](.htm?spm=a1z10.1-c.w137644-23632941727.26.118775c2oUPAnL&id=688860713104)**👈👈👈==
• 前言
• 一、创建自定义进程
• 二、自定义UORB消息
• 三、UORB消息的订阅与发布
• 四、PX4 1.13.0及更新版本添加自定义模块
• 五、PX4 1.13.0及更新版本添加自定义任务
• 六、PX4 1.14.0及更新版本添加自定义工作队列

前言

分享知识，传递正能量，如有疏漏或不当之处，恳请指出．
固件版本：PX4 1.11.3 1.13.0
本文主要介绍利用PX4进行二次开发所需的基础知识，磨刀不误砍柴工，掌握这些知识是进行二次开发的前提．

一、创建自定义进程

创建代码文件夹
在src/modules文件夹下创建存放代码的文件夹（也可以在其他文件夹下创建，不同的文件夹下写文件时要区分）

我这里创建了一个01text文件夹

具体内容如下，包含一个头文件，一个cpp文件和一个文件．
我这里直接用的是PX4中自带的模板文件

复制到src/modules文件夹下

编写文件
PX4采用跨平台的编译工具CMake编译固件，CMakeList文件是CMake的配置文件，分别配置了文件名、主函数名、源文件名等信息。文件内容如下：

px4_add_module(

添加文件夹名字为01test

MODULE modules__01test

添加主函数名字为module，主函数名不能和其他主函数名重名

MAIN module

添加文件夹中的.cpp文件

SRCSmodule.cpp

添加依赖

DEPENDSmodules__uORB
)

为了让新添加的应用程序编译进固件中，还要修改编译脚本，编译脚本在boards文件夹下，以PX4 fmu-v5为例，打开ake
在MODULES下面添加需要编译的文件夹名01test．

添加完后即可将创建的应用程序软件编译到PX4固件中，在Firmware目录下打开终端，执行make px4_fmu-v5_default upload即可将固件编译并通过USB下载到飞控中。下载完成后将飞控连接到地面站，在MAVLink Console页面的终端中，输入help，可以找到module，说明代码已经编译到固件中．

二、自定义UORB消息

1.创建msg文件
PX4中所有的UORB消息都有一个msg文件，该文件存放在msg文件夹下，如果要添加自定义UORB消息，首先需要在msg文件夹下创建msg文件，这里手动创建一个a01_test.msg，如下：

msg文件中包含一个时间戳（时间戳不可省略）以及消息成员的定义，具体内容如下

uint64 timestamp	# time since system start (microseconds)uint32 a
bool b	

2.将msg文件添加到文件中
创建完msg文件后，还要将文件名添加到文件中。这样才能自动生成所需的C/C++代码。
文件位置：

添加如下：

添加完后编译固件，可以看到生成了消息头文件（执行不同的编译脚本，头文件的位置也不一样）．

头文件内容如下：

三、UORB消息的订阅与发布

在编写订阅发布程序之前，先简单介绍一下module.cpp的程序执行流程，首先在执行module_main函数，在该函数中调用了main函数．
在main函数中，如果执行start，也就是是执行module start时，会进入到start_command_base中执行．
在start_command_base中执行了task_spawn
在task_spawn中创建了一个任务，任务从run_trampoline开始执行．

在run_trampoline中调用了instantiate接口的run()函数．

而instantiate指向Module类

所以调用了如下函数

该函数中有一个While大循环，我们后面就在这个循环里面实现订阅发布就可以了．
第一步：添加头文件
主要是UORB消息头文件和订阅发布接口的头文件

第二步：在头文件中定义订阅发布接口
定义为私有类型

    a01_test_s  _sub{};a01_test_s  _pub{};// SubscriptionsuORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};uORB::Subscription	_a01_test_sub{ORB_ID(a01_test)};// PublicationsuORB::Publication<a01_test_s>		_a01_test_pub{ORB_ID(a01_test)};

第三步：订阅发布
在run函数中编写如下代码：

void Module::run()
{

while大循环

while (!should_exit()) {

发布递增数据

_pub.a++;
_a01_test_pub.publish(_pub);

打印发布数据

PX4_INFO("pub=%d",_pub.a);

订阅发布数据

_a01_py(&_sub);

打印订阅数据

PX4_INFO("sub=%d",_sub.a);

睡眠0.1秒

usleep(100000);}
}

我这里是在同一个进程中进行发布订阅的，在不同的进程中发布订阅是一样的．
测试
将程序编译下载到飞控，在地面站终端中输入

module start

可以看到能正确打印发布订阅的数据．

四、PX4 1.13.0及更新版本添加自定义模块

在PX4-Autopilot/src/modules文件夹下创建px4_simple_app文件夹

在此文件夹下创建下图三个文件

创建px4_simple_app.c，代码如下：

#include <px4_platform_common/log.h>__EXPORT int px4_simple_app_main(int argc, char *argv[]);int px4_simple_app_main(int argc, char *argv[])
{PX4_INFO("Hello Sky!");return OK;
}

再创建<，代码如下：

px4_add_module(MODULE modules__02uart_testMAIN uart_testSRCSuart_test.cpp)

再创建Kconfig，代码如下：

 menuconfig MODULES_PX4_SIMPLE_APPbool "px4_simple_app"default n---help---Enable support px4_simple_app

修改 PX4-Autopilot/boards/px4/sitl/default.px4board
添加一行

CONFIG_MODULES_PX4_SIMPLE_APP=y

验证
在PX4-Autopilot目录下执行：

make px4_sitl_default gazebo

执行

px4_simple_app start

会打印Hello Sky!

五、PX4 1.13.0及更新版本添加自定义任务

在PX4-Autopilot/src/modules文件夹下创建template_module文件夹
在文件夹中创建下图四个文件

创建template_module.cpp，代码如下：

#include "template_module.h"#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>int TemplateModule::print_status()
{PX4_INFO("Running");// TODO: print additional runtime information about the state of the modulereturn 0;
}int TemplateModule::custom_command(int argc, char *argv[])
{/*if (!is_running()) {print_usage("not running");return 1;}// additional custom commands can be handled like this:if (!strcmp(argv[0], "do-something")) {get_instance()->do_something();return 0;}*/return print_usage("unknown command");
}int TemplateModule::task_spawn(int argc, char *argv[])
{_task_id = px4_task_spawn_cmd("module",SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT,1024,(px4_main_t)&run_trampoline,(char *const *)argv);if (_task_id < 0) {_task_id = -1;return -errno;}return 0;
}TemplateModule *TemplateModule::instantiate(int argc, char *argv[])
{int example_param = 0;bool example_flag = false;bool error_flag = false;int myoptind = 1;int ch;const char *myoptarg = nullptr;// parse CLI argumentswhile ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) {switch (ch) {case 'p':example_param = (int)strtol(myoptarg, nullptr, 10);break;case 'f':example_flag = true;break;case '?':error_flag = true;break;default:PX4_WARN("unrecognized flag");error_flag = true;break;}}if (error_flag) {return nullptr;}TemplateModule *instance = new TemplateModule(example_param, example_flag);if (instance == nullptr) {PX4_ERR("alloc failed");}return instance;
}TemplateModule::TemplateModule(int example_param, bool example_flag): ModuleParams(nullptr)
{
}void TemplateModule::run()
{// Example: run the loop synchronized to the sensor_combined topic publicationint sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));px4_pollfd_struct_t fds[1];fds[0].fd = sensor_combined_sub;fds[0].events = POLLIN;// initialize parametersparameters_update(true);while (!should_exit()) {
PX4_INFO("template_module");// wait for up to 1000ms for dataint pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000);if (pret == 0) {// Timeout: let the loop run anyway, don't do `continue` here} else if (pret < 0) {// this is undesirable but not much we can doPX4_ERR("poll error %d, %d", pret, errno);px4_usleep(50000);continue;} else if (fds[0].revents & POLLIN) {struct sensor_combined_s sensor_combined;orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor_combined);// TODO: do something with }parameters_update();}orb_unsubscribe(sensor_combined_sub);
}void TemplateModule::parameters_update(bool force)
{// check for parameter updatesif (_parameter_update_sub.updated() || force) {// clear updateparameter_update_s update;_parameter_py(&update);// update parameters from storageupdateParams();}
}int TemplateModule::print_usage(const char *reason)
{if (reason) {PX4_WARN("%sn", reason);}PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
### Description
Section that describes the provided module functionality.This is a template for a module running as a task in the background with start/stop/status functionality.### Implementation
Section describing the high-level implementation of this module.### Examples
CLI usage example:
$ module start -f -p 42)DESCR_STR");PRINT_MODULE_USAGE_NAME("module", "template");PRINT_MODULE_USAGE_COMMAND("start");PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);PRINT_MODULE_USAGE_DEFAULT_COMMANDS();return 0;
}int template_module_main(int argc, char *argv[])
{return TemplateModule::main(argc, argv);
}

创建template_module.h，代码如下：

#pragma once#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;extern "C" __EXPORT int template_module_main(int argc, char *argv[]);class TemplateModule : public ModuleBase<TemplateModule>, public ModuleParams
{
public:TemplateModule(int example_param, bool example_flag);virtual ~TemplateModule() = default;/** @see ModuleBase */static int task_spawn(int argc, char *argv[]);/** @see ModuleBase */static TemplateModule *instantiate(int argc, char *argv[]);/** @see ModuleBase */static int custom_command(int argc, char *argv[]);/** @see ModuleBase */static int print_usage(const char *reason = nullptr);/** @see ModuleBase::run() */void run() override;/** @see ModuleBase::print_status() */int print_status() override;private:/*** Check for parameter changes and update them if needed.* @param parameter_update_sub uorb subscription to parameter_update* @param force for a parameter update*/void parameters_update(bool force = false);DEFINE_PARAMETERS((ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */)// SubscriptionsuORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};};

创建<，代码如下：

px4_add_module(MODULE modules__template_moduleMAIN template_moduleSRCStemplate_module.cpp)

创建Kconfig，代码如下：

menuconfig MODULES_TEMPLATE_MODULEbool "template_module"default n---help---Enable support for template_module

修改 PX4-Autopilot/boards/px4/sitl/default.px4board
添加一行

CONFIG_MODULES_TEMPLATE_MODULE=y

验证
在PX4-Autopilot目录下执行：

make px4_sitl_default gazebo

执行

 template_module start

会打印template_module

六、PX4 1.14.0及更新版本添加自定义工作队列

在modules文件夹下添加work_item文件夹

添加下面四个文件

px4_add_module(MODULE modules__work_itemMAIN work_item_testCOMPILE_FLAGS#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output#-O0             # uncomment when debuggingSRCSWorkItemTest.cppWorkItemTest.hppDEPENDSpx4_work_queue)

Kconfig

menuconfig MODULES_WORK_ITEMbool "work_item"default n---help---Enable support for work_item

WorkItemTest.cpp

#include "WorkItemTest.hpp"WorkItemTest::WorkItemTest() :ModuleParams(nullptr),ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
{
}WorkItemTest::~WorkItemTest()
{perf_free(_loop_perf);perf_free(_loop_interval_perf);
}bool WorkItemTest::init()
{
ScheduleOnInterval(100000_us);return true;
}void WorkItemTest::Run()
{if (should_exit()) {ScheduleClear();exit_and_cleanup();return;}perf_begin(_loop_perf);perf_count(_loop_interval_perf);// Check if parameters have changedif (_parameter_update_sub.updated()) {// clear updateparameter_update_s param_update;_parameter_py(&param_update);updateParams(); // update module parameters (in DEFINE_PARAMETERS)}PX4_INFO("test workitem");perf_end(_loop_perf);
}int WorkItemTest::task_spawn(int argc, char *argv[])
{WorkItemTest *instance = new WorkItemTest();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;
}int WorkItemTest::print_status()
{perf_print_counter(_loop_perf);perf_print_counter(_loop_interval_perf);return 0;
}int WorkItemTest::custom_command(int argc, char *argv[])
{return print_usage("unknown command");
}int WorkItemTest::print_usage(const char *reason)
{if (reason) {PX4_WARN("%sn", reason);}PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
### Description
Example of a simple module running out of a work queue.)DESCR_STR");PRINT_MODULE_USAGE_NAME("work_item_example", "template");PRINT_MODULE_USAGE_COMMAND("start");PRINT_MODULE_USAGE_DEFAULT_COMMANDS();return 0;
}extern "C" __EXPORT int work_item_test_main(int argc, char *argv[])
{return WorkItemTest::main(argc, argv);
}

WorkItemTest.hpp

#pragma once#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_status.h>using namespace time_literals;class WorkItemTest : public ModuleBase<WorkItemTest>, public ModuleParams, public px4::ScheduledWorkItem
{
public:WorkItemTest();~WorkItemTest() override;/** @see ModuleBase */static int task_spawn(int argc, char *argv[]);/** @see ModuleBase */static int custom_command(int argc, char *argv[]);/** @see ModuleBase */static int print_usage(const char *reason = nullptr);bool init();int print_status() override;private:void Run() override;// PublicationsuORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};// SubscriptionsuORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)};        // subscription that schedules WorkItemExample when updateduORB::SubscriptionInterval         _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updatesuORB::Subscription                 _vehicle_status_sub{ORB_ID(vehicle_status)};          // regular subscription for additional data// Performance (perf) countersperf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};// ParametersDEFINE_PARAMETERS((ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */)bool _armed{false};
};

修改仿真的编译脚本

添加如下：

CONFIG_MODULES_WORK_ITEM=y

然后执行

make px4_sitl_default gazbo

启动仿真后执行：

work_item_test start

正常的话会打印下面的信息