PX4自定义UORB和Mavlink消息，并发送到QGC

最近做项目，需要用PX4读取角度传感器的值，并发送到QGC地面站上显示。实现思路如下：

1. 自定义UORB消息，并用一个线程来发送
2. 自定义Mavlink消息，并映射到UORB消息
3. 修改QGC源码，在地面站界面中显示自定义Mavlink消息

开发环境为：

• 飞控：Holybro Pixhawk 6C
• 固件：PX4 1.14.0
• 地面站：QGC v4.3.0
• 程序开发软件：QT 5.15.2

主要参考资料：

PX4模块设计之五：自定义MAVLink消息_px4如何更改mavlink协议-优快云博客

PX4实战之旅（二）：通过自定义mavlink消息和QGC通信_qgc中mavlink控制台的使用方法-优快云博客

以下是具体实现步骤。

Step1：自定义UORB消息，并用一个线程来发送

创建./msg/anglesensor.msg文件，自定义一个名为“anglesensor”的UORB消息来存储角度传感器的值：

uint64 timestamp	# time since system start (microseconds)

float32[4] angle
float32[4] anglespeed
uint64 c        #uart count
uint64 v        #valid data count
uint64 u        #uorb count
uint64 mytime

make一下，即可在build文件夹中找到编译后的anglesensor.h文件。我的飞控是Pixhawk 6C，因此该文件的路径为./build/px4_fmu-v6c_default/uORB/topics/anglesensor.h，具体内容如下：

#pragma once


#include <uORB/uORB.h>


#ifndef __cplusplus

#endif


#ifdef __cplusplus
struct __EXPORT anglesensor_s {
#else
struct anglesensor_s {
#endif
	uint64_t timestamp;
	uint64_t c;
	uint64_t v;
	uint64_t u;
	uint64_t mytime;
	float angle[4];
	float anglespeed[4];


#ifdef __cplusplus

#endif
};

#ifdef __cplusplus
namespace px4 {
	namespace msg {
		using anglesensor = anglesensor_s;
	} // namespace msg
} // namespace px4
#endif

/* register this as object request broker structure */
ORB_DECLARE(anglesensor);


#ifdef __cplusplus
void print_message(const orb_metadata *meta, const anglesensor_s& message);
#endif

然后自定义一个线程，先从串口DMA缓存中读取传感器的值，然后通过anglesensor消息发送出去：

//定义UORB消息结构体
struct anglesensor_s anglesensor_ad;

//以下内容截取自DMA接收中断程序

//从DMA缓存中读取角度传感器数据
memcpy(&anglesensor_ad.angle[0], &H7IORXBuffer[1], 16);
anglesensor_ad.timestamp = timesample; //打上时间戳
anglesensor_ad.c = interrupt_count; //记录中断次数

//发布UORB消息
orb_publish(ORB_ID(anglesensor), anglesensor_ad_pub, &anglesensor_ad);

PX4自定义线程的操作参考PX4官方文档即可，这里不再赘述。

Step2：自定义Mavlink消息，并映射到UORB消息

找到PX4源码中定义Mavlink消息的文件：./src/modules/mavlink/mavlink/message_definitions/v1.0/common.xml。在其中自定义一条“ANGLE”消息：

 <message id="3" name="ANGLE">
        <description>mavlink angle sensor message</description>
        <field type="uint64_t" name="time_usec">uint64_t</field>
        <field type="float" name="angle0">float</field>
        <field type="float" name="angle1">float</field>
        <field type="float" name="angle2">float</field>
        <field type="float" name="angle3">float</field>
 </message>

注意消息id不要与已有的id重复，并在合适的地方插入。然后还是先make一下，就可以得到编译后的mavlink消息文件，我的是./build/px4_fmu-v6c_default/mavlink/common/mavlink_msg_angle.h，文件具体内容就不展示了。

接下来需要将上面自定义的Mavlink消息与UORB消息关联起来。在./src/modules/mavlink/streams/中新建ANGLE.hpp文件，内容可以照抄./src/modules/mavlink/streams/DEBUG_VECT.hpp文件（改几个名称就行），主要作用就是将UORB消息中的内容复制到Mavlink结构体中：

#ifndef ANGLE_HPP
#define ANGLE_HPP

#include <uORB/topics/anglesensor.h>

class MavlinkStreamAngle : public MavlinkStream
{
public:
	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAngle(mavlink); }

	static constexpr const char *get_name_static() { return "ANGLE"; }
	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ANGLE; }

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

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

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

	uORB::Subscription anglesensor_sub{ORB_ID(anglesensor)};

	bool send() override
	{
		anglesensor_s angle;

        //将UORB消息中的内容复制到Mavlink结构体中
		if (anglesensor_sub.update(&angle)) {
			mavlink_angle_t msg{};
			msg.time_usec = angle.timestamp;
			msg.angle0 = angle.angle[0];
			msg.angle1 = angle.angle[1];
			msg.angle2 = angle.angle[2];
			msg.angle3 = angle.angle[3];

			mavlink_msg_angle_send_struct(_mavlink->get_channel(), &msg);
			//PX4_INFO("Sending anglesensor data");

			return true;
		}

		return false;
	}
};

#endif // ANGLE_HPP

接着找到./src/modules/mavlink/mavlink_messages.cpp文件，对其进行两处修改：

一是添加我们刚写好的ANGLE.hpp头文件：

#include "streams/ANGLE.hpp"

二是在streams_list[]中添加我们自定义消息流：

#if defined(ANGLE_HPP)
	create_stream_list_item<MavlinkStreamAngle>()
#endif // ANGLE_HPP

最后找到./src/modules/mavlink/mavlink_main.cpp文件，在“switch (_mode)”语句的上方增加一行，配置mavlink消息的发送速率：（我这里是200Hz）

configure_stream_local("ANGLE", 200.0f);

以上便完成了对PX4源码的消息，烧写固件。

Step3：修改QGC源码，在地面站界面中显示自定义Mavlink消息

在QGC源码中找到定义Mavlink消息的文件：\libs\mavlink\include\mavlink\v2.0\message_definitions\common.xml。该文件的结构与PX4源码中的common.xml基本一致，使用与Step1中同样的方法在其中插入自定义消息，此处不再重复。

然后下载Mavlink源码，其中有一个mavgenerate.py文件，用于生成Mavlink消息的.h头文件：

运行该文件：

python mavgenerate.py

此时将弹出一个GUI界面，其中“XML”栏选择QGC源码中的\libs\mavlink\include\mavlink\v2.0\message_definitions\all.xml（注意不是common.xml），“Out”栏选择一个输出文件夹，语言选择“C”，协议选择“2.0”：

点击“Generate”，即可在“Out”中指定的文件夹找到生成的文件：

将以上生成的全部文件拷回QGC源码的\libs\mavlink\include\mavlink\v2.0\文件夹，覆盖原有文件。

 提示1：下载的Mavlink源码中也有一个message_definations文件夹，内有common.xml文件。但是，若使用Mavlink源码中的common.xml生成头文件，下一步编译QGC可能报错！（下载的Mavlink源码与QGC源码的版本不一定匹配）

提示2：很多教程提到，QGC默认使用的是arduipilotmega.xml的dialect，但是查阅QGC官网，发现v4.2.8以后的版本已经改为默认使用all.xml（我使用的是v4.3.0）：

最后，用QT打开QGC源码中的qgroundcontrol.pro项目，编译QGC软件，具体设置可以参考这篇文章。点击下图“播放”按钮开始编译，完成后会自动打开QGC地面站：

提示3：注意QGC的版本要与QT的版本相匹配。我的QGC版本为v4.3.0，对应的QT版本为5.15.2，使用其他版本的QT编译会报错。（现在的最新版本似乎指定QT6.6.1，详见QGC官网）

打开编译后的QGC地面站，连接飞控，可以在“Analyze Tools - Mavlink Inspector”中查看Mavlink列表：

由于此时还未启动Step1中定义的UORB发送线程，因此Mavlink Inspector中看不到ANGLE消息。通过Mavlink Console启动线程，就能看到ANGLE消息了：（角度传感器没插，所以收到的都是默认值0）