👉👉👉**无人机硬件，提供全程指导**👈👈👈
文章目录
前言
分享知识，传递正能量，如有疏漏或不当之处，恳请指出．
固件版本：PX4 1.11.3 1.13.0
本文主要介绍利用PX4进行二次开发所需的基础知识，磨刀不误砍柴工，掌握这些知识是进行二次开发的前提．
PX4二次开发快速入门2.1：自定义工作队列
一、创建自定义进程
创建代码文件夹
在src/modules文件夹下创建存放代码的文件夹（也可以在其他文件夹下创建，不同的文件夹下写CMakeLists.txt文件时要区分）
我这里创建了一个01text文件夹
具体内容如下，包含一个头文件，一个cpp文件和一个CMakeLists.txt文件．
我这里直接用的是PX4中自带的模板文件
复制到src/modules文件夹下
编写CMakeLists.txt文件
PX4采用跨平台的编译工具CMake编译固件，CMakeList文件是CMake的配置文件，分别配置了文件名、主函数名、源文件名等信息。文件内容如下：
px4_add_module(
添加文件夹名字为01test
MODULE modules__01test
添加主函数名字为module，主函数名不能和其他主函数名重名
MAIN module
添加文件夹中的.cpp文件
SRCS
module.cpp
添加依赖
DEPENDS
modules__uORB
)
为了让新添加的应用程序编译进固件中，还要修改编译脚本，编译脚本在boards文件夹下，以PX4 fmu-v5为例，打开default.cmake
在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文件添加到CMakeLists.txt文件中
创建完msg文件后，还要将文件名添加到msg/CMakeLists.txt文件中。这样才能自动生成所需的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{};
// Subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _a01_test_sub{ORB_ID(a01_test)};
// Publications
uORB::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_test_sub.copy(&_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;
}
再创建CMakeLists.txt，代码如下：
px4_add_module(
MODULE modules__02uart_test
MAIN uart_test
SRCS
uart_test.cpp
)
再创建Kconfig，代码如下：
menuconfig MODULES_PX4_SIMPLE_APP
bool "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 module
return 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 arguments
while ((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 publication
int 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 parameters
parameters_update(true);
while (!should_exit()) {
PX4_INFO("template_module");
// wait for up to 1000ms for data
int 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 do
PX4_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 the data...
}
parameters_update();
}
orb_unsubscribe(sensor_combined_sub);
}
void TemplateModule::parameters_update(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
}
}
int TemplateModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", 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 */
)
// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
};
创建CMakeLists.txt，代码如下：
px4_add_module(
MODULE modules__template_module
MAIN template_module
SRCS
template_module.cpp
)
创建Kconfig，代码如下：
menuconfig MODULES_TEMPLATE_MODULE
bool "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文件夹
添加下面四个文件
CMakeLists.txt
px4_add_module(
MODULE modules__work_item
MAIN work_item_test
COMPILE_FLAGS
#-DDEBUG_BUILD # uncomment for PX4_DEBUG output
#-O0 # uncomment when debugging
SRCS
WorkItemTest.cpp
WorkItemTest.hpp
DEPENDS
px4_work_queue
)
Kconfig
menuconfig MODULES_WORK_ITEM
bool "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 changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(¶m_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("%s\n", 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;
// Publications
uORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};
// Subscriptions
uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)}; // subscription that schedules WorkItemExample when updated
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data
// Performance (perf) counters
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
// Parameters
DEFINE_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
正常的话会打印下面的信息
扫一扫
507
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
