PX4 uORB 通信机制(上):概念与用法

一、uORB 的定位

uORB(Micro Object Request Broker)是 PX4 的进程内/系统内 IPC 机制:将 drivers、估计器、导航、控制等模块解耦为统一的发布/订阅模型。

数据流路径:传感器驱动采样 → 解析 → 发布 uORB → 上层模块订阅使用。

相关知识:模块入口与运行模式


二、三个核心对象与两个数字

2.1 Topic(主题)

Topic 是”一类消息的定义”:

  • 消息结构体(由 msg/*.msg 自动生成):如 vehicle_attitude_ssensor_accel_s
  • 元数据:topic 名称、结构体大小、队列长度、唯一 ID
  • Publisher:将最新数据写入 Topic
  • Subscriber:读取 Topic 最新数据,并感知是否有更新

2.2 DeviceNode(节点)

某个 Topic 的实例化数据容器:

  • 一个 topic 可以有多个实例(multi-instance),对应多 IMU、多气压计等
  • 每个实例对应一个 DeviceNode,内部保存最新数据(或环形队列)+ 订阅者状态 + 唤醒机制

2.3 Manager(管理器)

Topic → DeviceNode 的总表,负责:

  • 查找/创建 DeviceNode(advertise/subscribe 时)
  • 发布时把数据写入正确的 DeviceNode
  • 订阅时把订阅者挂到正确的 DeviceNode

2.4 两个关键数字

字段 含义
generation DeviceNode 内部计数器,每发布一次自增
last_generation 订阅者上次读到的 generation
判定有更新 node.generation != subscriber.last_generation

三、发布链路

1
2
3
4
5
6
7
8
9
10
11
12
// 最小发布者示例
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude.h>

uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};

void publish_attitude() {
vehicle_attitude_s msg{};
msg.timestamp = hrt_absolute_time(); // 必须填写 timestamp
// 填写其他字段
_att_pub.publish(msg); // 若未 advertise,内部自动 advertise
}

发布不是将指针传给订阅者,而是把数据拷贝到 broker(DeviceNode)的存储里,再由 broker 负责通知订阅者。


四、订阅链路

PX4 支持三种订阅方式:

4.1 轮询式(非阻塞)

适合低频任务、简单模块、线程循环:

1
2
3
4
5
6
7
8
9
10
11
12
13
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/sensor_accel.h>

// 5ms 最小间隔
uORB::SubscriptionInterval _accel_sub{ORB_ID(sensor_accel), 5000};

void handle_accel() {
sensor_accel_s a{};
if (_accel_sub.update(&a)) {
// 至少间隔 5ms 才会 copy 一次
// 保证:hrt_elapsed_time >= interval 才检查 updated() 并 copy
}
}

底层语义:orb_check 比较 generationlast_generationorb_copy 把 DeviceNode 最新数据拷贝到栈变量,并更新 last_generation

4.2 阻塞式

通过 px4_poll() 等待数据,适合独立线程模式:

1
2
3
4
5
6
7
8
9
uORB::Subscription _sub{ORB_ID(vehicle_attitude)};
px4_pollfd_struct_t fds[1];
fds[0].fd = _sub.getHandle();
fds[0].events = POLLIN;
px4_poll(fds, 1, 100); // 阻塞最多 100ms
if (fds[0].revents & POLLIN) {
vehicle_attitude_s att{};
_sub.copy(&att);
}

DeviceNode 内部通过 poll_notify(POLLIN) 通知订阅者。

4.3 回调式(WorkQueue 事件驱动)

适合高频控制算法(见 uORB 通信机制(下)):

1
2
3
4
// init() 中注册回调
_att_sub.registerCallback();
// uORB 发布时 -> DeviceNode 遍历 _callbacks 调用 call()
// -> 触发 ScheduleNow() -> WorkQueue 调用 Run()

五、新增一条消息的完整步骤

Step 1:创建 .msg 文件

msg/ 目录下新建 MyTopic.msg,必须包含 timestamp

1
2
3
uint64 timestamp      # 必须有,PX4 生态依赖此字段做日志同步
float32 value
int32 id

Step 2:加入构建系统

编辑 msg/CMakeLists.txt,将 MyTopic.msg 加入列表,重新编译后自动生成:

  • uORB/topics/my_topic.h(结构体 my_topic_s
  • topic 元数据注册(ORB_ID(my_topic) 可用)

Step 3:发布与订阅

1
2
3
4
5
6
7
8
9
10
11
12
13
// 发布
#include <uORB/Publication.hpp>
#include <uORB/topics/my_topic.h>

uORB::Publication<my_topic_s> _pub{ORB_ID(my_topic)};

void send() {
my_topic_s m{};
m.timestamp = hrt_absolute_time();
m.value = 1.0f;
m.id = 42;
_pub.publish(m);
}
1
2
3
4
5
6
7
8
9
10
11
12
// 订阅
#include <uORB/Subscription.hpp>
#include <uORB/topics/my_topic.h>

uORB::Subscription _sub{ORB_ID(my_topic)};

void recv() {
my_topic_s m{};
if (_sub.updated()) {
_sub.copy(&m);
}
}

六、消息文件名到结构体名的转换

PX4 的 uORB 消息生成器(Tools/msg/px_generate_uorb_topic_files.py)将 PascalCase 文件名转为 snake_case 结构体名:

1
2
3
4
5
6
# 核心正则
re.sub(r'(?<!^)(?=[A-Z])', '_', file_base_name).lower()
# 在不是开头的位置且后面是大写字母处插入下划线

# VehicleStatus.msg -> vehicle_status
# vehicle_status_s(结构体名加 _s 后缀)

生成内容:

  • vehicle_status.h:结构体定义
  • vehicle_status.cpp:uORB topic 元数据
  • ORB_DECLARE(vehicle_status)ORB_DEFINE(vehicle_status, ...)

七、多实例与队列深度

多实例

同一种 topic 可能有多路来源(多 IMU、多 GPS),uORB 允许同一 topic 有多个 instance:

1
2
// 发布多实例(advertise_multi)
// 订阅多实例(subscribe_multi,需要指定 instance 编号)

注意:

  • 不要混用 orb_advertise_multiorb_advertise
  • 遇到多实例时,发布端只发布 instance 0 但订阅端订阅了 instance 1,会导致永远收不到数据

队列深度

  • 大多数 topic 只要最新值,queue=1 足够
  • 少数 topic 需要保留短历史,如 vehicle_command,队列 > 1

上一篇:模块入口与运行模式
下一篇:uORB 通信机制(下):WorkQueue 实例模板