最近做项目,需要用PX4读取角度传感器的值,并发送到QGC地面站上显示。实现思路如下:
- 自定义UORB消息,并用一个线程来发送
- 自定义Mavlink消息,并映射到UORB消息
- 修改QGC源码,在地面站界面中显示自定义Mavlink消息
开发环境为:
- 飞控:Holybro Pixhawk 6C
- 固件:PX4 1.14.0
- 地面站:QGC v4.3.0
- 程序开发软件:QT 5.15.2
主要参考资料:
以下是具体实现步骤。
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)