无人机PX4飞控 | PX4源码添加自定义uORB消息并保存到日志

PX4源码添加自定义uORB消息并保存到日志

0 前言

PX4的内部通信机制主要依赖于uORB（Micro Object Request Broker），这是一种跨进程的通信机制，一种轻量级的中间件，用于在PX4飞控系统的各个模块之间进行高效的数据交换，它通过发布-订阅（publish/subscribe）模式，实现了不同模块之间的异步通信。
在这里插入图片描述

uORB还允许开发者自定义新话题，首先需要创建一个新的.msg文件，将文件名添加到msg/CMakeLists.txt中，之后进行编译，来生成成对应的C++代码及.h头文件，然后在使用该话题的地方包含该头文件即可。

本篇文章介绍添加uORB消息具体方法，并将该消息记录与日志中。

1 添加uORB消息方法

第一步：添加对应的msg文件

PX4原定义好的msg文件都在PX4-Autopilot/msg 文件夹下面，已经有非常多
在这里插入图片描述
打开飞机的位置消息文件 vehicle_local_position.msg ，作为参考，可以看到里面有很多的内容，作为总结如下

定义的变量类型有：

bool
char
float32 / float64
uint8 / uint16 / uint64
int8 /int16 /int32

相关的类型定义在 px_generate_uorb_topic_helper.py 文件中

在这里插入图片描述

[!NOTE]

也可以定义对应类型的向量，例如： float32[3]

msg文件最后两行还可以定义本msg的ID

例如vehicle_local_position.msg的设置ID如下

在这里插入图片描述

[!NOTE]

一个uORB消息类型可以定义多个不同名字的msg ID ，但是不同的uORB消息类型不可以定义相同名字的msg ID。

第二步：将msg文件名添加到CMakeLists.txt 文件中

在编写好 **.msg 文件后，需要将该文件的名字添加到PX4-Autopilot/msg/CMakeLists.txt 文件中

这样系统就将**.msg文件

自动编译生成对应的**.h文件了

添加到39行那个set函数里，一般加到最下面一个
在这里插入图片描述

第三步：引用头文件

在需要引用的头文件中加入uORB相关的头文件

通常一般需要引用的有下面几个

#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>

然后就是头文件引用对应msg生成的头文件，前面的路径一般是uORB/topics/ 。例如：

#include <uORB/topics/**.h>

至此添加一个uORB消息类型即完成了！下面则是使用uORB消息的方法。

第四步：uORB消息的订阅

订阅的声明举个例子如下：

uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};

uORB::Subscription 就是声明一个uORB订阅类型
_vehicle_land_detected_sub 订阅变量的名字
ORB_ID(vehicle_land_detected) 订阅的uORB的ID，如果在msg文件中没有定义ID，那么msg文件的名字就是ID，也可以通过#TOPICS 来设置对应的ID ，一个msg可以设置多个不同的ID

获取最新的已订阅的uORB消息，伪代码如下：

vehicle_land_detected_s 变量名a
if(_vehicle_land_detected_sub.update(&变量名a))
{
    具体操作
}

第五步：uORB消息的发布

发布的声明举个例子如下：

uORB::PublicationData<takeoff_status_s>  _takeoff_status_pub{ORB_ID(takeoff_status)};

uORB::PublicationData ——声明一个uORB发布类型
<takeoff_status_s> ——发布消息的结构体，一般结构体的名字就是msg文件的名字
_takeoff_status_pub ——uORB发布类型的变量名称
ORB_ID(takeoff_status) ——发布的订阅的uORB的ID

发布指定的uORB消息，伪代码如下：

takeoff_status_s 变量名a
变量名a 各成员变量赋值
_takeoff_status_pub.publish(变量名a);

第六步：将uORB消息记录在日志中

如果希望将该消息记录在日志中

那么需要打开 PX4-Autopilot/src/modules/logger/logged_topics.cpp 文件

文件中的 void LoggedTopics::add_default_topics() 函数就是添加记录日志的topic的

可以看到原本的有这样几个功能函数：
在这里插入图片描述

它们用于控制日志记录中的主题的功能区别是：

add_topic —— 函数用于将指定的uORB主题添加到日志记录中。这意味着每当该主题的数据更新时，都会被记录到日志文件中。带间隔参数：add_topic(topic_name, interval_ms)，其中 interval_ms 指定记录间隔（毫秒），例如，add_topic("demo_uorb", 100) 表示每100毫秒记录一次该主题的数据；
add_optional_topic——函数用于添加一个可选的主题。与 add_topic() 不同，该函数允许用户选择是否启用该主题的日志记录。用户可以根据需要手动开启或关闭该主题的日志记录。
add_optional_topic_multi ——与 add_optional_topic() 类似，但允许同时添加多个主题。这使得可以一次性配置多个可选主题的日志记录。

2 实践

2.1 代码实现

PX4-Autopilot/msg 文件夹下面，新建一个文件，文件命名为：jone_demo.msg

在 jone_demo.msg 文件中定义四个参数：

uint64 timestamp			# time since system start (microseconds)


bool            enable
float32         acc_norm
float32[3]      acc

编写好jone_demo.msg文件后，

将 jone_demo.msg 文件的名字添加到PX4-Autopilot/msg/CMakeLists.txt 文件中，这样系统就将jone_demo.msg文件自动编译生成对应的jone_demo.h文件了

添加到39行那个set函数里，一般加到最下面一个

在这里插入图片描述

先编译一下

make px4_sitl_default

这样就生成了对应的jone_demo.h ，路径是：build/px4_sitil_default/uORB/topics

在这里插入图片描述

在之前添加的模块jone_demo中的JoneDemo.hpp 头文件中加入uORB相关的头文件

通常一般需要引用的有下面几个

#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>

jone_demo.msg 生成的头文件为：jone_demo.h

#include <uORB/topics/jone_demo.h>

下面订阅sensor_combined的uORB消息，获取其中的三轴加速度信息

在JoneDemo.hpp 中类的私有变量区加入

uORB::Subscription _sensor_combined_sub{ORB_ID(sensor_combined)};

去订阅传感器信息的uORB消息，所以前面要加上该消息的引用

#include <uORB/topics/sensor_combined.h>

在JoneDemo.hpp 中类的私有变量区加入

uORB::PublicationData<jone_demo_s>  _jone_demo_pub{ORB_ID(jone_demo)};

可以打开jone_demo.h文件，看下jone_demo_s 结构体的定义
在这里插入图片描述

下面在cpp文件中进行具体的uORB消息订阅与发布操作

在JoneDemo.cpp 的Run()函数中加入

	sensor_combined_s imu;
	if(_sensor_combined_sub.update(&imu))
	{
	     jone_demo_s jone;

	     jone.timestamp = hrt_absolute_time();//获得系统的绝对时间
	     jone.enable = true;
	     jone.acc[0] = accelerometer_m_s2[0];
	     jone.acc[1] = accelerometer_m_s2[1];
	     jone.acc[2] = accelerometer_m_s2[2];
	     jone.acc_norm = sqrtf(jone.acc[0]*jone.acc[0]+jone.acc[1]*jone.acc[1]+jone.acc[2]*jone.acc[2]);

	     _jone_demo_pub.publish(jone);

	}