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

前言

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

一、创建自定义进程

创建代码文件夹
在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(&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("%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

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