PX4二次开发快速入门

soloveMonica

于 2024-08-18 02:20:07 发布

阅读量61

点赞数

文章目录

前言
一、概述
二、二次开发基础（自定义工作队列，自定义uorb）

自定义工作队列
自定义uorb消息

三、自定义串口驱动（添加一个毫米波雷达并定高）
四、自定义I2C驱动（驱动一个oled显示屏）
五、自定义参数
六、自定义日志记录
七、自定义机型

一、定义机型文件
二、修改srcparser.py
三、编译并下载固件
四、修改QGC

八、自定义mavlink消息和QGC通信

一、PX4生成自定义mavlink消息文件
二、PX4创建自定义进程和uorb消息

2.1创建uorb消息
2.2创建自定义进程

三、PX4接收QGC消息

修改mavlink_receiver.h
修改mavlink_receiver.cpp

四、PX4发送消息给QGC

在`src/modules/mavlink/streams`下新建`MAVLINK_TEST.hpp`
修改mavlink_messages.cpp
修改mavlink_main.cpp

五、在QGC中添加自定义mavlink消息

安装MAVLINK生成器mavgenerate
修改XML文件并生成MAVLINK库文件

六、QGC消息处理

设计界面
消息处理

七、测试

九、自定义mavlink消息和MAVROS通信
十、OFFBAORD控制接口（状态控制 pwm输出）

一、添加自定义模块
二、测试

十一、基于模型开发（替换控制律/基于模型的编队）

前言

本教程的适用对象：
1、零基础或者有较少基础（尚未入门）
2、希望在短时间里（一到两个月）学会对PX4进行二次开发的开源爱好者或工程师

硬件：
pixhawk飞控
超维M系列无人机

软件：
PX4 1.13.3
QGC4.2.4

课程源码： https://gitee.com/Mbot_admin/px4-1.13.3-cwkj

PX4二次开发快速入门（一）：概述

PX4二次开发快速入门2.1：自定义工作队列

一、概述

二、二次开发基础（自定义工作队列，自定义uorb）

自定义工作队列

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

PX4二次开发快速入门_#include

添加下面四个文件

PX4二次开发快速入门_二次开发_02

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);
}

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.

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};
};

修改仿真的编译脚本

PX4二次开发快速入门_自定义_03

添加如下：

PX4二次开发快速入门_自定义_04

然后执行

启动仿真后执行：

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

PX4二次开发快速入门_自定义_05

自定义uorb消息

三、自定义串口驱动（添加一个毫米波雷达并定高）

PX4二次开发快速入门（三）：自定义串口驱动

四、自定义I2C驱动（驱动一个oled显示屏）

五、自定义参数

在第一节的基础上进一步修改

添加params.c

PX4二次开发快速入门_自定义_06

params.c内容如下：

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)
	}
        double param_test1=_param_test1.get();
        int param_test2=_param_test2.get();
        PX4_INFO("param_test1=%lf param_test2=%d\n",param_test1,param_test2);

	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);
}

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.

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 */
	    	(ParamFloat<px4::params::PARAM_TEST1>) _param_test1,   /**< example parameter */
    		(ParamInt<px4::params::PARAM_TEST2>) _param_test2  /**< another parameter */
	)


	bool _armed{false};
};

修改仿真的编译脚本

PX4二次开发快速入门_自定义_03

添加如下：

PX4二次开发快速入门_自定义_04

然后先执行（不执行这一步后面会报错）

再执行

最后执行

启动仿真后执行（注意要启动work_item_test ，不然地面站搜不到参数）：

PX4二次开发快速入门_自定义_09

如果在启动work_item_test 之前已经打开了地面站，还需要再刷新一下参数

PX4二次开发快速入门_二次开发_10

就可以搜到添加的参数

PX4二次开发快速入门_二次开发_11

六、自定义日志记录

首先把需要记录的数据定义成uorb消息

我这里添加了log_test.msg

PX4二次开发快速入门_PX4_12

内容如下：

然后在CMakeLists.txt中添加文件名

PX4二次开发快速入门_PX4_13

然后编译即可生成自定义uorb消息头文件

PX4二次开发快速入门_开源飞控_14

然后在之前添加的工作队列里发布这个自定义的消息

修改：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)
	}
	double param_test1=_param_test1.get();
        int param_test2=_param_test2.get();
        PX4_INFO("param_test1=%lf param_test2=%d\n",param_test1,param_test2);

	PX4_INFO("test workitem");
	_log_test.timestamp=hrt_absolute_time();
	_log_test.seq++;
	 PX4_INFO("_log_test.seq=%d\n",_log_test.seq);
	_log_test_pub.publish(_log_test);

	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);
}

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.

修改：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>
#include <uORB/topics/log_test.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)};
	uORB::Publication<log_test_s> _log_test_pub{ORB_ID(log_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 */
		(ParamFloat<px4::params::PARAM_TEST1>) _param_test1,   /**< example parameter */
    		(ParamInt<px4::params::PARAM_TEST2>) _param_test2  /**< another parameter */
	)


	bool _armed{false};
	log_test_s _log_test{};
};

然后在logged_topics.cpp中下面的位置添加：

PX4二次开发快速入门_二次开发_15

然后编译启动仿真：

启动仿真后执行（注意要启动work_item_test ，不然的uorb话题没有更新，日志不会记录）：

终端打印如下：

PX4二次开发快速入门_二次开发_16

然后下载飞控的日志，用plotjuggler打开

可以看到添加进去的日志数据，值也是随之时间递增的

PX4二次开发快速入门_#include_17

七、自定义机型

一、定义机型文件

PX4的机型文件存放在Firmware/ROMFS/px4fmu_common/init.d/airframes目录下，每个机型文件开头的数字代表是该机型的ID，即参数SYS_AUTOSTART，设置不同的SYS_AUTOSTART参数即可设置不同的机型，每个机型的SYS_AUTOSTART都不一样，本文自定义了一个机型文件80003_testframs，该机型文件对应的SYS_AUTOSTART即是80003。

PX4二次开发快速入门_自定义_18

添加完机型文件后，还需要修改该目录下的CMakeLists.txt文件，
在文件的最后添加上自定义机型的名字

PX4二次开发快速入门_PX4_19

本例中定义的机型文件如下:注意name、type、class要自己定义，其他的部分我是参考的其他的机型写的。

#!/bin/sh
#
# @name Test
#
# @url https://www.baidu.com
#
# @type Test
# @class Test
#
# @output MAIN0 Speed of left wheels
# @output MAIN1 Speed of right wheels
#
# @maintainer Xujun
#
# @board px4_fmu-v2 exclude
# @board bitcraze_crazyflie exclude
#


. ${R}etc/init.d/rc.rover_defaults

param set-default BAT1_N_CELLS 4

param set-default EKF2_GBIAS_INIT 0.01
param set-default EKF2_ANGERR_INIT 0.01
param set-default EKF2_MAG_TYPE 1

param set-default FW_AIRSPD_MIN 0
param set-default FW_AIRSPD_TRIM 1
param set-default FW_AIRSPD_MAX 3

param set-default GND_SP_CTRL_MODE 1
param set-default GND_L1_DIST 5
param set-default GND_L1_PERIOD 3
param set-default GND_THR_CRUISE 0.7
param set-default GND_THR_MAX 0.5

# Because this is differential drive, it can make a turn with radius 0.
# This corresponds to a turn angle of pi radians.
# If a special case is made for differential-drive, this will need to change.
param set-default GND_MAX_ANG 3.142
param set-default GND_WHEEL_BASE 0.3

# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
# to support negative throttle.
param set-default GND_THR_MIN 0
param set-default GND_SPEED_P 0.25
param set-default GND_SPEED_I 3
param set-default GND_SPEED_D 0.001
param set-default GND_SPEED_IMAX 0.125
param set-default GND_SPEED_THR_SC 1

param set-default MIS_LTRMIN_ALT 0.01
param set-default MIS_TAKEOFF_ALT 0.01

param set-default NAV_ACC_RAD 0.5

# Provide ESC a constant 1500 us pulse, which corresponds to
# idle on the Roboclaw motor controller on the Aion R1
param set-default PWM_MAIN_DISARM 1500
param set-default PWM_MAIN_DIS0 1500
param set-default PWM_MAIN_DIS1 1500
param set-default PWM_MAIN_MAX 2000
param set-default PWM_MAIN_MIN 1000

# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
param set-default CBRK_AIRSPD_CHK 162128

# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
param set-default GND_MAX_ANG 3.1415

param set-default RBCLW_BAUD 8
param set-default RBCLW_COUNTS_REV 1200
param set-default RBCLW_ADDRESS 128
# 104 corresponds to Telem 4
param set-default RBCLW_SER_CFG 104
# Start this driver after setting parameters, because the driver uses some of those parameters.
# roboclaw start /dev/ttyS3

# Set mixer
set MIXER generic_diff_rover

set PWM_MAIN_REV2 1

type对应地面站上的机架类型，name对应地面站上的载具。

PX4二次开发快速入门_PX4_20

二、修改srcparser.py

修改下图的srcparser.py文件

PX4二次开发快速入门_自定义_21

在下图位置加入一行：

PX4二次开发快速入门_#include_22

注意这里的elif和return的缩进，elif不能超过上面的elif，return不能超过最下面的那个return，例如：

PX4二次开发快速入门_开源飞控_23

否则后面make airframe_metadata编译会报下面的错：

File “/home/amov/6/PX4-Autopilot/Tools/px4airframes/srcparser.py”,
line 103
elif (self.name == “Test”):
^ IndentationError: unindent does not match any outer indentation level FAILED:
CMakeFiles/metadata_airframes

修改后的文件如下：

import sys
import re
import os

class ParameterGroup(object):
    """
    Single parameter group
    """
    def __init__(self, name, af_class):
        self.name = name
        self.af_class = af_class
        self.params = []


    def AddParameter(self, param):
        """
        Add parameter to the group
        """
        self.params.append(param)

    def GetName(self):
        """
        Get parameter group name
        """
        return self.name

    def GetClass(self):
        """
        Get parameter group vehicle type.
        """
        return self.af_class

    def GetImageName(self):
        """
        Get parameter group image base name (w/o extension)
        """
        if (self.name == "Standard Plane"):
            return "Plane"
        elif (self.name == "Flying Wing"):
            return "FlyingWing"
        elif (self.name == "Quadrotor x"):
            return "QuadRotorX"
        elif (self.name == "Quadrotor +"):
            return "QuadRotorPlus"
        elif (self.name == "Hexarotor x"):
            return "HexaRotorX"
        elif (self.name == "Hexarotor +"):
            return "HexaRotorPlus"
        elif (self.name == "Octorotor +"):
            return "OctoRotorPlus"
        elif (self.name == "Octorotor x"):
            return "OctoRotorX"
        elif (self.name == "Octorotor Coaxial"):
            return "OctoRotorXCoaxial"
        elif (self.name == "Octo Coax Wide"):
            return "OctoRotorXCoaxial"
        elif (self.name == "Quadrotor Wide"):
            return "QuadRotorWide"
        elif (self.name == "Quadrotor H"):
            return "QuadRotorH"
        elif (self.name == "Dodecarotor cox"):
            return "DodecaRotorXCoaxial"
        elif (self.name == "Simulation"):
            return "AirframeSimulation"
        elif (self.name == "Plane A-Tail"):
            return "PlaneATail"
        elif (self.name == "Plane V-Tail"):
            return "PlaneVTail"
        elif (self.name == "VTOL Duo Tailsitter"):
            return "VTOLDuoRotorTailSitter"
        elif (self.name == "Standard VTOL"):
            return "VTOLPlane"
        elif (self.name == "VTOL Quad Tailsitter"):
            return "VTOLQuadRotorTailSitter"
        elif (self.name == "VTOL Tiltrotor"):
            return "VTOLTiltRotor"
        elif (self.name == "VTOL Octoplane"):
            return "VTOLPlaneOcto"
        elif (self.name == "Coaxial Helicopter"):
            return "HelicopterCoaxial"
        elif (self.name == "Helicopter"):
            return "Helicopter"
        elif (self.name == "Hexarotor Coaxial"):
            return "Y6B"
        elif (self.name == "Y6A"):
            return "Y6A"
        elif (self.name == "Tricopter Y-"):
            return "YMinus"
        elif (self.name == "Tricopter Y+"):
            return "YPlus"
        elif (self.name == "Autogyro"):
            return "Autogyro"
        elif (self.name == "Airship"):
            return "Airship"
        elif (self.name == "Rover"):
            return "Rover"
        elif (self.name == "Boat"):
            return "Boat"
        elif (self.name == "Balloon"):
            return "Balloon"
        elif (self.name == "Vectored 6 DOF UUV"):
            return "Vectored6DofUUV"
        elif (self.name == "Test"):
    	     return "Test"
        return "AirframeUnknown"

    def GetParams(self):
        """
        Returns the parsed list of parameters. Every parameter is a Parameter
        object. Note that returned object is not a copy. Modifications affect
        state of the parser.
        """

        return sorted(self.params, key=lambda x: x.GetId())

class Parameter(object):
    """
    Single parameter
    """

    # Define sorting order of the fields
    priority = {
		"board": 9,
        "short_desc": 8,
        "long_desc": 7,
        "min": 5,
        "max": 4,
        "unit": 3,
        "AUX1": -10,
        "AUX2": -10,
        "AUX3": -10,
        "AUX4": -10,
        "AUX5": -10,
        "AUX6": -10,
        "AUX7": -10,
        "AUX8": -10,
        # all others == 0 (sorted alphabetically)
    }

    def __init__(self, path, post_path, name, airframe_type, airframe_class, airframe_id, maintainer):
        self.fields = {}
        self.outputs = {}
        self.archs = {}
        self.path = path
        self.post_path = post_path
        self.name = name
        self.type = airframe_type
        self.af_class = airframe_class
        self.id = airframe_id
        self.maintainer = maintainer

    def GetPath(self):
        """
        Get path to airframe startup script
        """
        return self.path

    def GetPostPath(self):
        """
        Get path to airframe post startup script
        """
        return self.post_path

    def GetName(self):
        """
        Get airframe name
        """
        return self.name

    def GetType(self):
        """
        Get airframe type
        """
        return self.type

    def GetClass(self):
        """
        Get airframe class
        """
        return self.af_class

    def GetId(self):
        """
        Get airframe id
        """
        return self.id

    def GetMaintainer(self):
        """
        Get airframe maintainer
        """
        return self.maintainer

    def SetField(self, code, value):
        """
        Set named field value
        """
        self.fields[code] = value

    def SetOutput(self, code, value):
        """
        Set named output value
        """
        self.outputs[code] = value

    def SetArch(self, code, value):
        """
        Set named arch value
        """
        self.archs[code] = value

    def GetFieldCodes(self):
        """
        Return list of existing field codes in convenient order
        """
        keys = self.fields.keys()
        keys = sorted(keys)
        keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
        return keys

    def GetFieldValue(self, code):
        """
        Return value of the given field code or None if not found.
        """
        fv =  self.fields.get(code)
        if not fv:
                # required because python 3 sorted does not accept None
                return ""
        return self.fields.get(code)

    def GetOutputCodes(self):
        """
        Return list of existing output codes in convenient order
        """
        keys = self.outputs.keys()
        keys = sorted(keys)
        keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
        return keys

    def GetOutputValue(self, code):
        """
        Return value of the given output code or None if not found.
        """
        fv =  self.outputs.get(code)
        if not fv:
                # required because python 3 sorted does not accept None
                return ""
        return self.outputs.get(code)

    def GetArchCodes(self):
        """
        Return list of existing arch codes in convenient order
        """
        keys = self.archs.keys()
        keys = sorted(keys)
        keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
        return keys

    def GetArchValue(self, code):
        """
        Return value of the given arch code or None if not found.
        """
        fv =  self.archs.get(code)
        if not fv:
                # required because python 3 sorted does not accept None
                return ""
        return self.archs.get(code)

class SourceParser(object):
    """
    Parses provided data and stores all found parameters internally.
    """

    re_split_lines = re.compile(r'[\r\n]+')
    re_comment_start = re.compile(r'^\#\s')
    re_comment_content = re.compile(r'^\#\s*(.*)')
    re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
    re_comment_end = re.compile(r'(.*?)\s*\#\n/')
    re_cut_type_specifier = re.compile(r'[a-z]+$')
    re_is_a_number = re.compile(r'^-?[0-9\.]')
    re_remove_dots = re.compile(r'\.+$')
    re_remove_carriage_return = re.compile('\n+')

    valid_tags = set(["url", "maintainer", "output", "arch", "name", "type", "desc"])

    # Order of parameter groups
    priority = {
        # All other groups = 0 (sort alphabetically)
        "Miscellaneous": -10
    }

    def __init__(self):
        self.param_groups = {}

    def GetSupportedExtensions(self):
        """
        Returns list of supported file extensions that can be parsed by this
        parser. The parser uses any extension.
        """
        return ["", ".hil"]

    def Parse(self, path, contents):
        """
        Incrementally parse program contents and append all found airframes
        to the list.
        """

        airframe_id = None
        airframe_id = os.path.split(path)[1].split('_',1)[0]

        # Skip if not numeric
        if (not self.IsNumber(airframe_id)):
            return True

        # This code is essentially a comment-parsing grammar. "state"
        # represents parser state. It contains human-readable state
        # names.
        state = None
        tags = {}
        outputs = {}
        archs = {}
        for line in self.re_split_lines.split(contents):
            line = line.strip()
            # Ignore empty lines
            if line == "":
                continue
            if state is None and self.re_comment_start.match(line):
                state = "wait-short"
                short_desc = None
                long_desc = None
            if state is not None and state != "comment-processed":
                m = self.re_comment_end.search(line)
                if m:
                    line = m.group(1)
                    last_comment_line = True
                else:
                    last_comment_line = False
                m = self.re_comment_content.match(line)
                if m:
                    comment_content = m.group(1)
                    if comment_content == "":
                        # When short comment ends with empty comment line,
                        # start waiting for the next part - long comment.
                        if state == "wait-short-end":
                            state = "wait-long"
                    else:
                        m = self.re_comment_tag.match(comment_content)
                        if m:
                            tag, desc = m.group(1, 2)
                            if (tag == "output"):
                                key, text = desc.split(' ', 1)
                                outputs[key] = text;
                            elif (tag == "board"):
                                key, text = desc.split(' ', 1)
                                archs[key] = text;
                            else:
                                tags[tag] = desc
                            current_tag = tag
                            state = "wait-tag-end"
                        elif state == "wait-short":
                            # Store first line of the short description
                            short_desc = comment_content
                            state = "wait-short-end"
                        elif state == "wait-short-end":
                            # Append comment line to the short description
                            short_desc += "\n" + comment_content
                        elif state == "wait-long":
                            # Store first line of the long description
                            long_desc = comment_content
                            state = "wait-long-end"
                        elif state == "wait-long-end":
                            # Append comment line to the long description
                            long_desc += "\n" + comment_content
                        elif state == "wait-tag-end":
                            # Append comment line to the tag text
                            tags[current_tag] += "\n" + comment_content
                        else:
                            raise AssertionError(
                                    "Invalid parser state: %s" % state)
                elif not last_comment_line:
                    # Invalid comment line (inside comment, but not starting with
                    # "*" or "*/". Reset parsed content.
                    state = None
                if last_comment_line:
                    state = "comment-processed"
            else:
                state = None

        # Process parsed content
        airframe_type = None
        maintainer = "John Doe <john@example.com>"
        airframe_name = None
        airframe_class = None

        # Done with file, store
        for tag in tags:
            if tag == "maintainer":
                maintainer = tags[tag]
            elif tag == "type":
                airframe_type = tags[tag]
            elif tag == "class":
                airframe_class = tags[tag]
            elif tag == "name":
                airframe_name = tags[tag]
            elif tag == "desc":
                # General documentation - not a parameter to be saved.
                pass
            elif tag not in self.valid_tags:
                sys.stderr.write("Aborting due to invalid documentation tag: '%s'\n" % tag)
                return False

        # Sanity check
        if airframe_type == None:
            sys.stderr.write("Aborting due to missing @type tag in file: '%s'\n" % path)
            return False

        if airframe_class == None:
            sys.stderr.write("Aborting due to missing @class tag in file: '%s'\n" % path)
            return False

        if airframe_name == None:
            sys.stderr.write("Aborting due to missing @name tag in file: '%s'\n" % path)
            return False

        # Check if a .post script exists
        if os.path.isfile(path + '.post'):
            post_path = path + '.post'
        else:
            post_path = None

        # We already know this is an airframe config, so add it
        param = Parameter(path, post_path, airframe_name, airframe_type, airframe_class, airframe_id, maintainer)

        # Done with file, store
        for tag in tags:
            if tag == "maintainer":
                maintainer = tags[tag]
            if tag == "type":
                airframe_type = tags[tag]
            if tag == "class":
                airframe_class = tags[tag]
            if tag == "name":
                airframe_name = tags[tag]
            else:
                param.SetField(tag, tags[tag])

        # Store outputs
        for output in outputs:
            param.SetOutput(output, outputs[output])

        # Store outputs
        for arch in archs:
            param.SetArch(arch, archs[arch])

        # Store the parameter

        # Create a class-specific airframe group. This is needed to catch cases where an airframe type might cross classes (e.g. simulation)
        class_group_identifier=airframe_type+airframe_class
        if class_group_identifier not in self.param_groups:
            #self.param_groups[airframe_type] = ParameterGroup(airframe_type)  #HW TEST REMOVE
            self.param_groups[class_group_identifier] = ParameterGroup(airframe_type, airframe_class)
        self.param_groups[class_group_identifier].AddParameter(param)

        return True

    def IsNumber(self, numberString):
        try:
            float(numberString)
            return True
        except ValueError:
            return False

    def Validate(self):
        """
        Validates the airframe meta data.
        """
        seenParamNames = []
        for group in self.GetParamGroups():
            for param in group.GetParams():
                name  = param.GetName()
                board = param.GetFieldValue("board")
                # Check for duplicates
                name_plus_board = name + "+" + board
                for seenParamName in seenParamNames:
                    if seenParamName == name_plus_board:
                        sys.stderr.write("Duplicate parameter definition: {0}\n".format(name_plus_board))
                        return False
                seenParamNames.append(name_plus_board)

        return True

    def GetParamGroups(self):
        """
        Returns the parsed list of parameters. Every parameter is a Parameter
        object. Note that returned object is not a copy. Modifications affect
        state of the parser.
        """
        groups = self.param_groups.values()
        groups = sorted(groups, key=lambda x: x.GetName())
        groups = sorted(groups, key=lambda x: x.GetClass())
        groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True)

        #Rename duplicate groups to include the class (creating unique headings in page TOC)
        duplicate_test=set()
        duplicate_set=set()
        for group in groups:
            if group.GetName() in duplicate_test:
                duplicate_set.add(group.GetName())
            else:
                duplicate_test.add(group.GetName() )
        for group in groups:
            if group.GetName() in duplicate_set:
                group.name=group.GetName()+' (%s)' % group.GetClass()

        return groups

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.
191.
192.
193.
194.
195.
196.
197.
198.
199.
200.
201.
202.
203.
204.
205.
206.
207.
208.
209.
210.
211.
212.
213.
214.
215.
216.
217.
218.
219.
220.
221.
222.
223.
224.
225.
226.
227.
228.
229.
230.
231.
232.
233.
234.
235.
236.
237.
238.
239.
240.
241.
242.
243.
244.
245.
246.
247.
248.
249.
250.
251.
252.
253.
254.
255.
256.
257.
258.
259.
260.
261.
262.
263.
264.
265.
266.
267.
268.
269.
270.
271.
272.
273.
274.
275.
276.
277.
278.
279.
280.
281.
282.
283.
284.
285.
286.
287.
288.
289.
290.
291.
292.
293.
294.
295.
296.
297.
298.
299.
300.
301.
302.
303.
304.
305.
306.
307.
308.
309.
310.
311.
312.
313.
314.
315.
316.
317.
318.
319.
320.
321.
322.
323.
324.
325.
326.
327.
328.
329.
330.
331.
332.
333.
334.
335.
336.
337.
338.
339.
340.
341.
342.
343.
344.
345.
346.
347.
348.
349.
350.
351.
352.
353.
354.
355.
356.
357.
358.
359.
360.
361.
362.
363.
364.
365.
366.
367.
368.
369.
370.
371.
372.
373.
374.
375.
376.
377.
378.
379.
380.
381.
382.
383.
384.
385.
386.
387.
388.
389.
390.
391.
392.
393.
394.
395.
396.
397.
398.
399.
400.
401.
402.
403.
404.
405.
406.
407.
408.
409.
410.
411.
412.
413.
414.
415.
416.
417.
418.
419.
420.
421.
422.
423.
424.
425.
426.
427.
428.
429.
430.
431.
432.
433.
434.
435.
436.
437.
438.
439.
440.
441.
442.
443.
444.
445.
446.
447.
448.
449.
450.
451.
452.
453.
454.
455.
456.
457.
458.
459.
460.
461.
462.
463.
464.
465.
466.
467.
468.
469.
470.
471.
472.
473.
474.
475.
476.
477.
478.
479.
480.
481.
482.
483.
484.
485.
486.
487.
488.
489.
490.
491.
492.
493.
494.
495.
496.
497.
498.
499.
500.
501.
502.
503.
504.
505.
506.
507.
508.
509.
510.
511.
512.
513.
514.
515.

三、编译并下载固件

先在Firmware路径下执行

然后执行

然后会在下面的目录生成airframes.xml文件

PX4二次开发快速入门_自定义_24

可以在里面找到生成了之前定义的80003这个机型

PX4二次开发快速入门_#include_25

然后执行下面命令编译固件

注意在编译固件时不要使用upload上传代码，必须用地面站进行上传
打开下图这个页面，然后插上飞控

PX4二次开发快速入门_#include_26

选择自定义固件

PX4二次开发快速入门_二次开发_27

选择刚才编译生成的.px4固件，双击上传即可

PX4二次开发快速入门_自定义_28

四、修改QGC

将第三步生成的airframes.xml中所有内容复制到QGC的AirframeFactMetaData.xml文件中，替换掉原有的内容

PX4二次开发快速入门_自定义_29

将自定义机型的svg图标放在qgroundcontrol/src/AutoPilotPlugins/PX4/Images目录下，命名为Test.svg：

PX4二次开发快速入门_PX4_30

在 qgcimages.qrc中点击添加-》添加文件，然后选择上一步的Test.svg，并将别名命名为Airframe/+下图image后面的名字，我这里是Test，所以命名为Airframe/Test

PX4二次开发快速入门_自定义_31

如下：

PX4二次开发快速入门_PX4_32

然后重新编译地面站然后将飞控连到新编译的地面站上，就可以在机架那一栏设置新添加的机型

PX4二次开发快速入门_PX4_33

设置之后如下：

PX4二次开发快速入门_PX4_34

机型对应的SYS_AUTOSTART参数为80003

PX4二次开发快速入门_开源飞控_35

八、自定义mavlink消息和QGC通信

一、PX4生成自定义mavlink消息文件

PX4 1.13.0以及更新版本的固件生成自定义mavlink消息文件的方法老版本固件不同，老版本固件生成自定义MAVLINk消息可以参考：

PX4 1.13.0以前版本的固件生成自定义mavlink消息文件的方法可以参考本文。

修改下图路径中的common.xml文件

PX4二次开发快速入门_自定义_36

在文件的下图位置加上

<message id="12921" name="TEST_MAVLINK">
  <description>Test Mavlink.</description>
  <field type="uint8_t" name="test1"> </field>
  <field type="int16_t" name="test2"> </field>
  <field type="float" name="test3"> </field>
</message>

PX4二次开发快速入门_自定义_37

注意id不能与其他的消息相同，我这里取的是12921。

然后编译

先

再

编译完成后会在下图的路径中生成mavlink_msg_test_mavlink.h

PX4二次开发快速入门_PX4_38

二、PX4创建自定义进程和uorb消息

假设PX4源码的路径为~/PX4-Autopilot

2.1创建uorb消息

定义两个消息

都填入下面的内容

uint64 timestamp # time since system start (microseconds)

uint8 test1
int16 test2
float32 test3

修改CMakeLists.txt

加入两行

如下图

PX4二次开发快速入门_自定义_39

然后编译固件生成消息头文件

2.2创建自定义进程

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

PX4二次开发快速入门_自定义_40

在该文件夹中创建CMakeLists.txt，Kconfig，test_mavlink.cpp，test_mavlink.h四个文件

编辑CMakeLists，填入以下内容后保存退出

px4_add_module(
	MODULE modules__test_mavlink
	MAIN test_mavlink
	COMPILE_FLAGS
		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
		#-O0             # uncomment when debugging
	SRCS
		test_mavlink.cpp
	DEPENDS
		px4_work_queue
	)

编辑Kconfig文件

menuconfig MODULES_TEST_MAVLINK
	bool "test_mavlink"
	default n
	---help---
		Enable support for test_mavlink

编辑test_mavlink.cpp文件
填写内容如下：

#include "test_mavlink.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 test_mavlink::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int test_mavlink::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 test_mavlink::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;
}

test_mavlink *test_mavlink::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;
    }

    test_mavlink *instance = new test_mavlink(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

test_mavlink::test_mavlink(int example_param, bool example_flag)
    : ModuleParams(nullptr)
{
}

void test_mavlink::run()
{

    // initialize parameters
    parameters_update(true);

    while (!should_exit())
    {
//        PX4_INFO("run\n");
        if(test_mavlink_rx_sub.updated())
        {
            PX4_INFO("updated\n");
            test_mavlink_rx_sub.copy(&_test_mavlink_rx);
//            PX4_INFO("_test_mavlink_rx.test1=%c\n",_test_mavlink_rx.test1);
//            PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
//            PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);
            _test_mavlink_tx.test1=_test_mavlink_rx.test1+1;
            _test_mavlink_tx.test2=_test_mavlink_rx.test2+1;
            _test_mavlink_tx.test3=_test_mavlink_rx.test3+1;
            test_mavlink_tx_pub.publish(_test_mavlink_tx);
        }
        PX4_INFO("_test_mavlink_rx.test1=%d\n",_test_mavlink_rx.test1);
        PX4_INFO("_test_mavlink_rx.test2=%d\n",_test_mavlink_rx.test2);
        PX4_INFO("_test_mavlink_rx.test3=%f\n",(double)_test_mavlink_rx.test3);
        px4_usleep(50000);


        parameters_update();
    }
}

void test_mavlink::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 test_mavlink::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 test_mavlink_main(int argc, char *argv[])
{
	return test_mavlink::main(argc, argv);
}

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.

编辑test_mavlink.h文件
填写内容如下：

#pragma once

#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_mavlink_tx.h>
#include <uORB/topics/test_mavlink_rx.h>
using namespace time_literals;

extern "C" __EXPORT int test_mavlink_main(int argc, char *argv[]);


class test_mavlink : public ModuleBase<test_mavlink>, public ModuleParams
{
public:
    test_mavlink(int example_param, bool example_flag);

    virtual ~test_mavlink() = default;

    /** @see ModuleBase */
    static int task_spawn(int argc, char *argv[]);

    /** @see ModuleBase */
    static test_mavlink *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 */
    )
    test_mavlink_tx_s _test_mavlink_tx;
    test_mavlink_rx_s _test_mavlink_rx;
    // Subscriptions
    uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
    uORB::Subscription test_mavlink_rx_sub{ORB_ID(test_mavlink_rx)};
    uORB::Publication<test_mavlink_tx_s>	test_mavlink_tx_pub{ORB_ID(test_mavlink_tx)};
};

编辑下图编译脚本

PX4二次开发快速入门_自定义_41

加入一行

如下：

PX4二次开发快速入门_自定义_42

三、PX4接收QGC消息

修改mavlink_receiver.h

PX4二次开发快速入门_开源飞控_43

添加自定义消息头文件：

PX4二次开发快速入门_自定义_44

添加自定义消息处理函数

PX4二次开发快速入门_PX4_45

添加话题发布

PX4二次开发快速入门_自定义_46

修改mavlink_receiver.cpp

PX4二次开发快速入门_开源飞控_47

调用消息处理函数：

PX4二次开发快速入门_#include_48

实现消息处理函数，主要是解析mavlink消息并通过uorb发布：

void
MavlinkReceiver::handle_message_test_mavlink_rx(mavlink_message_t *msg)
{
    mavlink_test_mavlink_t mavlink_test_msg;
    mavlink_msg_test_mavlink_decode(msg, &mavlink_test_msg);
    test_mavlink_rx_s __test_mavlink_rx;
    __test_mavlink_rx.test1=mavlink_test_msg.test1;
    __test_mavlink_rx.test2=mavlink_test_msg.test2;
    __test_mavlink_rx.test3=mavlink_test_msg.test3;
    _test_mavlink_rx_pub.publish(__test_mavlink_rx);
}

PX4二次开发快速入门_自定义_49

四、PX4发送消息给QGC

在`src/modules/mavlink/streams`下新建`MAVLINK_TEST.hpp`

PX4二次开发快速入门_开源飞控_50

在MAVLINK_TEST.hpp中填入下面内容，主要是订阅test_mavlink_tx消息然后将消息内容打包为MAVLINK数据帧并发送：

#ifndef MAVLINK_TEST_HPP
#define MAVLINK_TEST_HPP

#include <uORB/topics/test_mavlink_tx.h>
class MavlinkStreamMavlinktest : public MavlinkStream
{
public:
	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMavlinktest(mavlink); }

	static constexpr const char *get_name_static() { return "TEST_MAVLINK"; }
	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TEST_MAVLINK; }

	const char *get_name() const override { return get_name_static(); }
	uint16_t get_id() override { return get_id_static(); }

	unsigned get_size() override
	{
		return test_mavlink_tx_sub.advertised() ? MAVLINK_MSG_ID_TEST_MAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
	}

private:
	explicit MavlinkStreamMavlinktest(Mavlink *mavlink) : MavlinkStream(mavlink) {}

	uORB::Subscription test_mavlink_tx_sub{ORB_ID(test_mavlink_tx)};

	bool send() override
	{
		test_mavlink_tx_s test_mavlink_tx;

		if (test_mavlink_tx_sub.update(&test_mavlink_tx)) {
			mavlink_test_mavlink_t msg{};
msg.test1=test_mavlink_tx.test1;
msg.test2=test_mavlink_tx.test2;
msg.test3=test_mavlink_tx.test3;			
			mavlink_msg_test_mavlink_send_struct(_mavlink->get_channel(), &msg);
			return true;
		}

		return false;
	}
};

#endif // MOUNT_ORIENTATION

修改mavlink_messages.cpp

PX4二次开发快速入门_二次开发_51

添加hpp头文件

PX4二次开发快速入门_自定义_52

添加接口

#if defined(MAVLINK_TEST_HPP)
    create_stream_list_item<MavlinkStreamMavlinktest>(),
#endif // MAVLINK_TEST_HPP

PX4二次开发快速入门_自定义_53

修改mavlink_main.cpp

PX4二次开发快速入门_PX4_54

配置MAVLINK发送速率，注意这里要加在switch (_mode)上面

PX4二次开发快速入门_开源飞控_55

注意双引号里面的是消息的名称，在前面的MAVLINK_TEST.hpp中定义：

PX4二次开发快速入门_开源飞控_56

五、在QGC中添加自定义mavlink消息

QGC中的MAVLINK库和PX4中的不一样，使用的方言也不一样，因此两者的库不能直接复制粘贴用，在QGC中也需要进行定义

安装MAVLINK生成器mavgenerate

下载后进入到工程文件夹

PX4二次开发快速入门_PX4_57

进入这个文件夹路径下执行

会出现下面这个窗口

PX4二次开发快速入门_开源飞控_58

修改XML文件并生成MAVLINK库文件

QGC采用的是ardupilotmega方言

修改QGC下图目录的ardupilotmega.xml

PX4二次开发快速入门_自定义_59

在最后加上

<message id="12921" name="TEST_MAVLINK">
  <description>Test Mavlink.</description>
  <field type="uint8_t" name="test1"> </field>
  <field type="int16_t" name="test2"> </field>
  <field type="float" name="test3"> </field>
</message>

PX4二次开发快速入门_开源飞控_60

打开上一节安装的mavgenerate

XML选择修改后的ardupilotmega.xml，Out选择库文件的输出目录，Language选择C，Protocol选择2.0。最后点Generate

PX4二次开发快速入门_PX4_61

然后会在指定的输出目录下生成消息库文件

PX4二次开发快速入门_二次开发_62

将生成的这些文件全部复制并覆盖到qgroundcontrol/libs/mavlink/include/mavlink/v2.0目录下

六、QGC消息处理

设计界面

参考src/AnalyzeView/MavlinkConsolePage.qml 创建src/AnalyzeView/MavlinkTestPage.qml 。

PX4二次开发快速入门_二次开发_63

MavlinkTestPage.qml内容如下：

import QtQuick          2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs  1.2
import QtQuick.Layouts      1.2

import QGroundControl               1.0
import QGroundControl.Palette       1.0
import QGroundControl.FactSystem    1.0
import QGroundControl.FactControls  1.0
import QGroundControl.Controls      1.0
import QGroundControl.ScreenTools   1.0
import QGroundControl.Controllers   1.0

AnalyzePage {
    id:              mavlinkConsolePage
    pageComponent:   pageComponent
    pageName:        qsTr("Mavlink Test")

    Mavlinktest {
       id: test_mavlink
    }

    TextEdit {
        id: textEdit
        x: 285
        y: 189
        width: 81
        height: 25
        color: "#f5f3f3"
        text: qsTr("TEST1")
        font.family: "Times New Roman"
        font.pixelSize: 12
    }

    Button {
        id: button
        x: 372
        y: 189
        text: qsTr("发送")
        onClicked: {
       test_mavlink._sendcom(textEdit.text,textEdit1.text,textEdit2.text)
        }
    }

    TextEdit {
        id: textEdit1
        x: 285
        y: 220
        width: 81
        height: 25
        color: "#f5f3f3"
        text: qsTr("TEST2")
        font.pixelSize: 12
    }

    TextEdit {
        id: textEdit2
        x: 285
        y: 251
        width: 81
        height: 25
        color: "#f5f3f3"
        text: qsTr("TEST3")
        font.pixelSize: 12
    }

    Label {
        id: label
        x: 285
        y: 317
        width: 30
        height: 19
        text: qsTr("接收")
    }

    TextEdit {
        id: textEdit3
        x: 286
        y: 354
        width: 80
        height: 19
        color: "#f5f3f3"
        text: test_mavlink.test1
        font.pixelSize: 12
    }

    TextEdit {
        id: textEdit4
        x: 286
        y: 391
        width: 80
        height: 19
        color: "#f5f3f3"
        text: test_mavlink.test2
        font.pixelSize: 12
    }

    TextEdit {
        id: textEdit5
        x: 286
        y: 424
        width: 80
        height: 19
        color: "#f5f3f3"
        text:test_mavlink.test3
        font.pixelSize: 12
    }



    // Component
} // AnalyzePage

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.

在qgroundcontrol.qrc中将上面创建的QML资源添加进去，取别名叫MavlinkTestPage.qml。注意添加完后一定不要忘了把下面别名那个输入框填上别名，注意别名不要写错，不要忘了加上后缀.qml。否则如果后面QGCCorePlugin.cc引用的时候和别名不一样，会导致MavlinkTestPage页面里是空的，不是MavlinkTestPage.qml设计的页面

PX4二次开发快速入门_#include_64

PX4二次开发快速入门_#include_65

PX4二次开发快速入门_PX4_66

然后修改QGCCorePlugin.cc文件

添加一行

_p->analyzeList.append(QVariant::fromValue(new QmlComponentInfo(tr("Mavlinktest"),     QUrl::fromUserInput("qrc:/qml/MavlinkTestPage.qml"),      QUrl::fromUserInput("qrc:/qmlimages/MavlinkConsoleIcon"))));

PX4二次开发快速入门_二次开发_67

消息处理

参考src/AnalyzeView/MavlinkConsoleController.h 创建src/AnalyzeView/Mavlinktest.h

参考src/AnalyzeView/MavlinkConsoleController.cc

创建src/AnalyzeView/Mavlinktest.cpp

PX4二次开发快速入门_PX4_68

Mavlinktest.cpp内容如下：

#include "Mavlinktest.h"
#include "QGCApplication.h"
#include "UAS.h"
#include "MAVLinkInspectorController.h"
#include "MultiVehicleManager.h"
#include <QtCharts/QLineSeries>
Mavlinktest::Mavlinktest()
    : QStringListModel(),
      _cursor_home_pos{-1},
      _cursor{0},
      _vehicle{nullptr}
{
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &Mavlinktest::_setActiveVehicle);
    _setActiveVehicle(manager->activeVehicle());
    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
    connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Mavlinktest::_receiveMessage);
}

Mavlinktest::~Mavlinktest()
{
    if (_vehicle)
    {
        QByteArray msg;
        _sendSerialData(msg, true);
    }
}

void
Mavlinktest::sendCommand(QString command)
{
    _history.append(command);
    command.append("\n");
    _sendSerialData(qPrintable(command));
    _cursor_home_pos = -1;
    _cursor = rowCount();
}

QString
Mavlinktest::historyUp(const QString& current)
{
    return _history.up(current);
}

QString
Mavlinktest::historyDown(const QString& current)
{
    return _history.down(current);
}

void
Mavlinktest::_setActiveVehicle(Vehicle* vehicle)
{
    for (auto &con : _uas_connections)
    {
        disconnect(con);
    }
    _uas_connections.clear();

    _vehicle = vehicle;

    if (_vehicle)
    {
        _incoming_buffer.clear();
        // Reset the model
        setStringList(QStringList());
        _cursor = 0;
        _cursor_home_pos = -1;
        _uas_connections << connect(_vehicle, &Vehicle::mavlinkSerialControl, this, &Mavlinktest::_receiveData);
    }
}

void
Mavlinktest::_receiveData(uint8_t device, uint8_t, uint16_t, uint32_t, QByteArray data)
{
    if (device != SERIAL_CONTROL_DEV_SHELL)
    {
        return;
    }
   // auto idx = index(_cursor);
//setData(idx,  QString("%1 ttyS6 -> * [%2]").arg(QTime::currentTime().toString("HH:mm:ss.zzz")).arg(12));


    // Append incoming data and parse for ANSI codes
    _incoming_buffer.append(data);
    while(!_incoming_buffer.isEmpty())
    {
        bool newline = false;
        int idx = _incoming_buffer.indexOf('\n');
        if (idx == -1)
        {
            // Read the whole incoming buffer
            idx = _incoming_buffer.size();
        }
        else
        {
            newline = true;
        }

        QByteArray fragment = _incoming_buffer.mid(0, idx);
        if (_processANSItext(fragment))
        {
            writeLine(_cursor, fragment);
            if (newline)
            {
                _cursor++;
            }
            _incoming_buffer.remove(0, idx + (newline ? 1 : 0));
        }
        else
        {
            // ANSI processing failed, need more data
            return;
        }
    }
}

void
Mavlinktest::_receiveMessage(LinkInterface*, mavlink_message_t message)
{

    if(message.msgid==MAVLINK_MSG_ID_TEST_MAVLINK)
    {
        qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";
        mavlink_test_mavlink_t mavlink_test;

        mavlink_msg_test_mavlink_decode(&message, &mavlink_test);

//id=pwm.base_mode;
//_name=QString::number(heartbeat.base_mode);
        _test1=QString::number(mavlink_test.test1);
        _test2=QString::number(mavlink_test.test2);
        _test3=QString::number(mavlink_test.test3);
        emit valueChanged();
        qDebug()<<_test1<<_test2<<_test3;
//        qDebug()<<"从模式"<<heartbeat.custom_mode;
    }
}


void Mavlinktest::_sendcom(QString test1,QString test2,QString test3)
{

    if (!_vehicle)
    {
        qWarning() << "Internal error";
        return;
    }

    WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
    if (!weakLink.expired()) {
        SharedLinkInterfacePtr sharedLink = weakLink.lock();

        if (!sharedLink) {
            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
            return;
        }
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
            auto priority_link =sharedLink;

            uint8_t send_test1=test1.toUInt();
            int16_t send_test2=test2.toShort();
            float send_test3=test3.toFloat();
            mavlink_message_t msg;
            mavlink_msg_test_mavlink_pack_chan(_vehicle->id(),
                                               1,
                                               priority_link->mavlinkChannel(),
                                               &msg,
                                               send_test1,
                                               send_test2,
                                               send_test3);
           _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
            qDebug()<<"send"<<send_test1<<send_test2<<send_test3;
        }
}
void
Mavlinktest::_sendSerialData(QByteArray data, bool close)
{
    if (!_vehicle)
    {
        qWarning() << "Internal error";
        return;
    }

    WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink();
    if (!weakLink.expired()) {
        SharedLinkInterfacePtr sharedLink = weakLink.lock();

        if (!sharedLink) {
            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
            return;
        }


        // Send maximum sized chunks until the complete buffer is transmitted
//        while(data.size())
//        {
//            QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
//            uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE |  SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
//            if (close)
//            {
//                flags = 0;
//            }
//            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
//            auto priority_link =sharedLink;
//            mavlink_message_t msg;



//            mavlink_msg_serial_control_pack_chan(
//                protocol->getSystemId(),
//                protocol->getComponentId(),
//                priority_link->mavlinkChannel(),
//                &msg,
//                SERIAL_CONTROL_DEV_SHELL,
//                flags,
//                0,
//                0,
//                chunk.size(),
//                reinterpret_cast<uint8_t*>(chunk.data()));
//            _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
//            data.remove(0, chunk.size());
//        }
    }




}

bool
Mavlinktest::_processANSItext(QByteArray &line)
{
    // Iterate over the incoming buffer to parse off known ANSI control codes
    for (int i = 0; i < line.size(); i++)
    {
        if (line.at(i) == '\x1B')
        {
            // For ANSI codes we expect at least 3 incoming chars
            if (i < line.size() - 2 && line.at(i+1) == '[')
            {
                // Parse ANSI code
                switch(line.at(i+2))
                {
                    default:
                        continue;
                    case 'H':
                        if (_cursor_home_pos == -1)
                        {
                            // Assign new home position if home is unset
                            _cursor_home_pos = _cursor;
                        }
                        else
                        {
                            // Rewind write cursor position to home
                            _cursor = _cursor_home_pos;
                        }
                        break;
                    case 'K':
                        // Erase the current line to the end
                        if (_cursor < rowCount())
                        {
                            setData(index(_cursor), "");
                        }
                        break;
                    case '2':
                        // Check for sufficient buffer size
                        if ( i >= line.size() - 3)
                        {
                            return false;
                        }

                        if (line.at(i+3) == 'J' && _cursor_home_pos != -1)
                        {
                            // Erase everything and rewind to home
                            bool blocked = blockSignals(true);
                            for (int j = _cursor_home_pos; j < rowCount(); j++)
                            {
                                setData(index(j), "");
                            }
                            blockSignals(blocked);
                            QVector<int> roles;
                            roles.reserve(2);
                            roles.append(Qt::DisplayRole);
                            roles.append(Qt::EditRole);
                            emit dataChanged(index(_cursor), index(rowCount()), roles);
                        }
                        // Even if we didn't understand this ANSI code, remove the 4th char
                        line.remove(i+3,1);
                        break;
                }
                // Remove the parsed ANSI code and decrement the bufferpos
                line.remove(i, 3);
                i--;
            }
            else
            {
                // We can reasonably expect a control code was fragemented
                // Stop parsing here and wait for it to come in
                return false;
            }
        }
    }
    return true;
}

void
Mavlinktest::writeLine(int line, const QByteArray &text)
{
    auto rc = rowCount();
    if (line >= rc)
    {
        insertRows(rc, 1 + line - rc);
    }
    auto idx = index(line);
    setData(idx, data(idx, Qt::DisplayRole).toString() + text);
}

void Mavlinktest::CommandHistory::append(const QString& command)
{
    if (command.length() > 0)
    {

        // do not append duplicates
        if (_history.length() == 0 || _history.last() != command)
        {

            if (_history.length() >= maxHistoryLength)
            {
                _history.removeFirst();
            }
            _history.append(command);
        }
    }
    _index = _history.length();
}

QString Mavlinktest::CommandHistory::up(const QString& current)
{
    if (_index <= 0)
    {
        return current;
    }

    --_index;
    if (_index < _history.length())
    {
        return _history[_index];
    }
    return "";
}

QString Mavlinktest::CommandHistory::down(const QString& current)
{
    if (_index >= _history.length())
    {
        return current;
    }

    ++_index;
    if (_index < _history.length())
    {
        return _history[_index];
    }
    return "";
}

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.
191.
192.
193.
194.
195.
196.
197.
198.
199.
200.
201.
202.
203.
204.
205.
206.
207.
208.
209.
210.
211.
212.
213.
214.
215.
216.
217.
218.
219.
220.
221.
222.
223.
224.
225.
226.
227.
228.
229.
230.
231.
232.
233.
234.
235.
236.
237.
238.
239.
240.
241.
242.
243.
244.
245.
246.
247.
248.
249.
250.
251.
252.
253.
254.
255.
256.
257.
258.
259.
260.
261.
262.
263.
264.
265.
266.
267.
268.
269.
270.
271.
272.
273.
274.
275.
276.
277.
278.
279.
280.
281.
282.
283.
284.
285.
286.
287.
288.
289.
290.
291.
292.
293.
294.
295.
296.
297.
298.
299.
300.
301.
302.
303.
304.
305.
306.
307.
308.
309.
310.
311.
312.
313.
314.
315.
316.
317.
318.
319.
320.
321.
322.
323.
324.
325.
326.
327.
328.
329.
330.
331.
332.
333.
334.
335.
336.
337.
338.
339.
340.
341.
342.
343.
344.
345.
346.
347.
348.
349.
350.
351.
352.
353.
354.
355.
356.
357.
358.
359.
360.
361.
362.
363.
364.
365.
366.

Mavlinktest.h内容如下：

#pragma once

#include "MAVLinkProtocol.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "FactMetaData.h"
#include <QObject>
#include <QString>
#include <QMetaObject>
#include <QStringListModel>

// Fordward decls
class Vehicle;

/// Controller for MavlinkConsole.qml.
class Mavlinktest : public QStringListModel
{
    Q_OBJECT

public:
    Mavlinktest();
    virtual ~Mavlinktest();

    Q_INVOKABLE void sendCommand(QString command);
    Q_INVOKABLE void _sendcom(QString test1,QString test2,QString test3);
    Q_INVOKABLE QString historyUp(const QString& current);
    Q_INVOKABLE QString historyDown(const QString& current);

    Q_PROPERTY(QString          test1       READ test1      NOTIFY valueChanged)
    QString         test1           ()
    {
        return _test1;
    }

    Q_PROPERTY(QString          test2       READ test2      NOTIFY valueChanged)
    QString         test2           ()
    {
        return _test2;
    }

    Q_PROPERTY(QString          test3       READ test3      NOTIFY valueChanged)
    QString         test3           ()
    {
        return _test3;
    }


private slots:
    void _setActiveVehicle  (Vehicle* vehicle);
    void _receiveData(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
    void _receiveMessage            (LinkInterface* link, mavlink_message_t message);
signals:
    void            valueChanged        ();
    void selectedChanged                ();

private:
    bool _processANSItext(QByteArray &line);
    void _sendSerialData(QByteArray, bool close = false);
    void writeLine(int line, const QByteArray &text);

    class CommandHistory
    {
    public:
        void append(const QString& command);
        QString up(const QString& current);
        QString down(const QString& current);
    private:
        static constexpr int maxHistoryLength = 100;
        QList<QString> _history;
        int _index = 0;
    };
    QString     _test1,_test2,_test3;
    int           _cursor_home_pos;
    int           _cursor;
    QByteArray    _incoming_buffer;
    Vehicle*      _vehicle;
    QList<QMetaObject::Connection> _uas_connections;
    CommandHistory _history;

};

将.h和.cpp源文件加入工程（如果QT Creator 没有自动添加的话），修改qgroundcontrol.pro

分别添加到下图的位置：

PX4二次开发快速入门_二次开发_69

在src/QGCApplication.cc注册QML

先添加头文件

PX4二次开发快速入门_二次开发_70

然后添加

PX4二次开发快速入门_#include_71

经过上面注册后就可以在ＱＭＬ中用双引号里的类名进行直接构造，通过id来访问属性和方法，例如：

Mavlinktest { id: testsa}

七、测试

将编辑好的PX4固件编译烧入飞控，编译生成QGC地面站

编译QGC后，界面如下，上面有三个输入框，输入需要发送的数据，点击发送即可通过自定义MAVLINK消息发送给飞控，下面有显示接收数据的地方，会将飞控飞控QGC的自定义MAVLIK消息解析后在下面显示。

PX4二次开发快速入门_开源飞控_72

将飞控通过USB连接地面站。先在MAVLINK Console里面输入

PX4二次开发快速入门_开源飞控_73

然后打开Mavlinktest那一栏，在“发送”按钮左边的三个输入框输入发送的数据，正常的话飞控会通过自定义MAVLINK消息接收到这些数据然后加1发送给地面站，并在地面站上显示。

PX4二次开发快速入门_自定义_74

九、自定义mavlink消息和MAVROS通信

十、OFFBAORD控制接口（状态控制 pwm输出）

一、添加自定义模块

在PX4-Autopilot/src/modules目录下新建control_node文件夹

PX4二次开发快速入门_二次开发_75

在control_node文件夹下新建下面四个文件

PX4二次开发快速入门_PX4_76

新建CMakeLists.txt，复制下面内容进去：

px4_add_module(
	MODULE modules__control_node
	MAIN control_node
	SRCS
		control_node.cpp
	)

新建Kconfig，复制下面内容进去：

menuconfig MODULES_CONTROL_NODE
	bool "control_node"
	default n
	---help---
		Enable support for control_node

新建control_node.cpp，复制下面内容进去：

#include "control_node.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>


int ControlNode::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int ControlNode::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 ControlNode::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;
}

ControlNode *ControlNode::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;
    }

    ControlNode *instance = new ControlNode(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

ControlNode::ControlNode(int example_param, bool example_flag)
    : ModuleParams(nullptr)
{
}

void ControlNode::run()
{
    //记录起始点位置
    float begin_x;
    float begin_y;
    float begin_z;

    float_t xy_rad=1;
    float_t z_rad=1;
    float_t yaw_rad=0.1;
//进入offboard模式并解锁
    while(!should_exit())
    {
        _vehicle_status_sub.copy(&_status);
        if((_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)&&(_status.arming_state==vehicle_status_s::ARMING_STATE_ARMED))
        {
            PX4_INFO("in offboard and armed");
            break;
        }
        _command.target_system = _status.system_id;
        _command.target_component = _status.component_id;
        ocm.timestamp = hrt_absolute_time();
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        sp_local.x=_vehicle_local_position.x;
        sp_local.y=_vehicle_local_position.y;
        sp_local.z=_vehicle_local_position.z-5;
        sp_local.timestamp = hrt_absolute_time();
        _trajectory_setpoint_pub.publish(sp_local);
        ocm.position=true;
        ocm.timestamp = hrt_absolute_time();
        _offboard_control_mode_pub.publish(ocm);
        _command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
        _command.param1=1.0f;
        _command.param2=PX4_CUSTOM_MAIN_MODE_OFFBOARD;
        _command.timestamp = hrt_absolute_time();
        _vehicle_command_pub.publish(_command);
        usleep(10000);
        _command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
        _command.param1 = 1.0f;
        _command.timestamp = hrt_absolute_time();
        _vehicle_command_pub.publish(_command);
    }
//记录当前点的位置
    _vehicle_local_position_sub.copy(&_vehicle_local_position);
    begin_x=_vehicle_local_position.x;
    begin_y=_vehicle_local_position.y;
    begin_z=_vehicle_local_position.z;
    time_tick=hrt_absolute_time();
    //业务逻辑
    while (!should_exit())
    {
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        if(flag==0)//升至5米
        {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=begin_x;
            sp_local.y=begin_y;
            sp_local.z=begin_z-5;
                    ocm.position=true;
                    ocm.velocity=false;
                    ocm.acceleration=false;
            PX4_INFO("pos 005");
        }
        if((flag==1)&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))//升至5米
        {
            if(flag2==0)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==1)&&(hrt_absolute_time()-time_tick)>1000000)
            {
                flag++;
                memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
                sp_local.x=begin_x+5;
                sp_local.y=begin_y;
                sp_local.z=begin_z-5;
                sp_local.vx=(float)NAN;
                sp_local.vy=(float)NAN;
                sp_local.vz=(float)NAN;
                sp_local.acceleration[0]=(float)NAN;
                sp_local.acceleration[1]=(float)NAN;
                sp_local.acceleration[2]=(float)NAN;
                ocm.position=true;
                ocm.velocity=false;
                ocm.acceleration=false;
                time_tick=hrt_absolute_time();
                PX4_INFO("pos 505");
            }

        }
        if((flag==2)&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))//升至5米
        {
            if(flag2==1)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==2)&&(hrt_absolute_time()-time_tick)>1000000)
            {
                flag++;
                memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
                sp_local.x=begin_x+5;
                sp_local.y=begin_y+5;
                sp_local.z=begin_z-5;
                sp_local.vx=(float)NAN;
                sp_local.vy=(float)NAN;
                sp_local.vz=(float)NAN;
                sp_local.acceleration[0]=(float)NAN;
                sp_local.acceleration[1]=(float)NAN;
                sp_local.acceleration[2]=(float)NAN;
                        ocm.position=true;
                        ocm.velocity=false;
                        ocm.acceleration=false;
                time_tick=hrt_absolute_time();
                PX4_INFO("pos 555");
            }

        }
        if(flag==3&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))
        {
            if(flag2==2)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==3)&&(hrt_absolute_time()-time_tick)>1000000)
            {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=begin_x+5;
            sp_local.y=begin_y+5;
            sp_local.z=begin_z-5;
            sp_local.vx=(float)NAN;
            sp_local.vy=(float)NAN;
            sp_local.vz=(float)NAN;
            sp_local.acceleration[0]=(float)NAN;
            sp_local.acceleration[1]=(float)NAN;
            sp_local.acceleration[2]=(float)NAN;
            sp_local.yaw=1;
            sp_local.yawspeed=(float)NAN;
            ocm.position=true;
            ocm.velocity=false;
            ocm.acceleration=false;
            PX4_INFO("yaw");
            }
        }
        if(flag==4&&(abs(_vehicle_local_position.heading-sp_local.yaw)<yaw_rad))
        {
            if(flag2==3)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==4)&&(hrt_absolute_time()-time_tick)>1000000)
            {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=begin_x+5;
            sp_local.y=begin_y+5;
            sp_local.z=begin_z-5;
            sp_local.vx=(float)NAN;
            sp_local.vy=(float)NAN;
            sp_local.vz=(float)NAN;
            sp_local.acceleration[0]=(float)NAN;
            sp_local.acceleration[1]=(float)NAN;
            sp_local.acceleration[2]=(float)NAN;
            sp_local.yaw=(float)NAN;
            sp_local.yawspeed=1;
            ocm.position=true;
            ocm.velocity=false;
            ocm.acceleration=false;
            PX4_INFO("yawspeed");
            }
        }
        if(flag==5)
        {
            if(flag2==4)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==5)&&(hrt_absolute_time()-time_tick)>5000000)
            {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=(float)NAN;
            sp_local.y=(float)NAN;
            sp_local.z=begin_z-5;
            sp_local.vx=1;
            sp_local.vy=0;
            sp_local.vz=(float)NAN;
            sp_local.acceleration[0]=(float)NAN;
            sp_local.acceleration[1]=(float)NAN;
            sp_local.acceleration[2]=(float)NAN;
            sp_local.yaw=(float)NAN;
            sp_local.yawspeed=(float)NAN;
            ocm.position=true;
            ocm.velocity=true;
            ocm.acceleration=false;
            PX4_INFO("alt and velocity");
            }
        }
        if(flag==6)
        {
            if(flag2==5)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==6)&&(hrt_absolute_time()-time_tick)>5000000)
            {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=(float)NAN;
            sp_local.y=(float)NAN;
            sp_local.z=begin_z-5;
            sp_local.vx=(float)NAN;
            sp_local.vy=(float)NAN;
            sp_local.vz=(float)NAN;
            sp_local.acceleration[0]=0;
            sp_local.acceleration[1]=1;
            sp_local.acceleration[2]=(float)NAN;
            sp_local.yaw=(float)NAN;
            sp_local.yawspeed=(float)NAN;
            ocm.position=true;
            ocm.velocity=false;
            ocm.acceleration=true;
            PX4_INFO("alt and acceleration");
            }
        }

        if(flag==7)
        {
            if(flag2==6)
            {
                flag2++;
                time_tick=hrt_absolute_time();
            }
            if((flag2==7)&&(hrt_absolute_time()-time_tick)>5000000)
            {
            flag++;
            _command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
            _command.param1=1.0f;
            _command.param2=PX4_CUSTOM_MAIN_MODE_AUTO;
            _command.param3=PX4_CUSTOM_SUB_MODE_AUTO_RTL;
            _command.timestamp = hrt_absolute_time();
            _vehicle_command_pub.publish(_command);
            PX4_INFO("return");
            }
        }


if(ocm.position||ocm.velocity||ocm.acceleration)
{
        ocm.timestamp = hrt_absolute_time();
        _offboard_control_mode_pub.publish(ocm);
        _vehicle_status_sub.copy(&_status);
if(_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)
{
        sp_local.timestamp = hrt_absolute_time();
        _trajectory_setpoint_pub.publish(sp_local);
//        PX4_INFO("sp_local.x=%lf\n",(double)sp_local.x);
//        PX4_INFO("sp_local.y=%lf\n",(double)sp_local.y);
//        PX4_INFO("sp_local.z=%lf\n",(double)sp_local.z);
//        PX4_INFO("sp_local.vx=%lf\n",(double)sp_local.vx);
//        PX4_INFO("sp_local.vy=%lf\n",(double)sp_local.vy);
//        PX4_INFO("sp_local.vz=%lf\n",(double)sp_local.vz);
}
}
        usleep(100000);

        parameters_update();
    }
}

void ControlNode::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 ControlNode::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 control_node_main(int argc, char *argv[])
{
	return ControlNode::main(argc, argv);
}

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.
191.
192.
193.
194.
195.
196.
197.
198.
199.
200.
201.
202.
203.
204.
205.
206.
207.
208.
209.
210.
211.
212.
213.
214.
215.
216.
217.
218.
219.
220.
221.
222.
223.
224.
225.
226.
227.
228.
229.
230.
231.
232.
233.
234.
235.
236.
237.
238.
239.
240.
241.
242.
243.
244.
245.
246.
247.
248.
249.
250.
251.
252.
253.
254.
255.
256.
257.
258.
259.
260.
261.
262.
263.
264.
265.
266.
267.
268.
269.
270.
271.
272.
273.
274.
275.
276.
277.
278.
279.
280.
281.
282.
283.
284.
285.
286.
287.
288.
289.
290.
291.
292.
293.
294.
295.
296.
297.
298.
299.
300.
301.
302.
303.
304.
305.
306.
307.
308.
309.
310.
311.
312.
313.
314.
315.
316.
317.
318.
319.
320.
321.
322.
323.
324.
325.
326.
327.
328.
329.
330.
331.
332.
333.
334.
335.
336.
337.
338.
339.
340.
341.
342.
343.
344.
345.
346.
347.
348.
349.
350.
351.
352.
353.
354.
355.
356.
357.
358.
359.
360.
361.
362.
363.
364.
365.
366.
367.
368.
369.
370.
371.
372.
373.
374.
375.
376.
377.
378.
379.
380.
381.
382.
383.
384.
385.
386.
387.
388.
389.
390.
391.
392.
393.
394.
395.
396.
397.
398.
399.
400.
401.
402.
403.
404.
405.
406.
407.
408.
409.
410.
411.
412.
413.
414.
415.
416.
417.
418.
419.
420.
421.
422.
423.
424.
425.
426.
427.
428.
429.
430.
431.
432.
433.
434.
435.

新建control_node.h，复制下面内容进去：

#pragma once
#include "commander/px4_custom_mode.h"
#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>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>

using namespace time_literals;

extern "C" __EXPORT int control_node_main(int argc, char *argv[]);


class ControlNode : public ModuleBase<ControlNode>, public ModuleParams
{
public:
	ControlNode(int example_param, bool example_flag);

	virtual ~ControlNode() = default;

	/** @see ModuleBase */
	static int task_spawn(int argc, char *argv[]);

	/** @see ModuleBase */
	static ControlNode *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 */
	)
	
    uint64_t time_tick=hrt_absolute_time();
    int flag=0,flag2=0;
    vehicle_local_position_s _vehicle_local_position;
    vehicle_status_s _status;
    vehicle_command_s _command = {};
    offboard_control_mode_s ocm{};
    position_setpoint_triplet_s _pos_sp_triplet{};
    vehicle_command_ack_s _ack{};
    vehicle_local_position_setpoint_s sp_local{};
    vehicle_attitude_setpoint_s attitude_setpoint{};
	// Subscriptions
	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
        uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
        uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
       
       // Publications
       uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
       uORB::Publication<vehicle_command_s>		_vehicle_command_pub{ORB_ID(vehicle_command)};
       uORB::Publication<position_setpoint_triplet_s>		_position_setpoint_triplet_pub{ORB_ID(position_setpoint_triplet)};
       uORB::Publication<vehicle_local_position_setpoint_s>	_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
};

添加到编译脚本

打开下图文件

PX4二次开发快速入门_PX4_77

添加一行

PX4二次开发快速入门_#include_78

二、测试

执行：

打开仿真后执行

正常的话无人机会进入Offboard，并起飞到5米高然后按程序设定自主运动

十一、基于模型开发（替换控制律/基于模型的编队）

原创作者: u_15352549 转载于: https://blog.51cto.com/u_15352549/11773216

soloveMonica

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
PX4二次开发快速入门

文章目录前言一、概述二、二次开发基础（自定义工作队列，自定义uorb）自定义工作队列自定义uorb消息三、自定义串口驱动（添加一个毫米波雷达并定高）四、自定义I2C驱动（驱动一个oled显示屏）五、自定义参数六、自定义日志记录七、自定义机型一、定义机型文件二、修改srcparser.py三、编译并下载固件四、修改QGC八...
复制链接

扫一扫