第4章：uORB消息发布与订阅——打通PX4模块的“神经网络”

各位同学，欢迎来到第四章。这一章我们聊点核心的东西——uORB。

说实话，uORB是PX4里最容易被忽视但又最重要的机制。它就像无人机的“神经网络”，把各个模块串联起来。传感器数据怎么从驱动跑到EKF？位置估计怎么传给导航？全靠它。

我刚开始接触PX4时，觉得uORB不就是个消息总线嘛，有啥好学的？直到有一次调试IMU数据，发现姿态估计死活不对，查了两天才发现是消息发布频率和订阅者的预期不匹配。嗯，从那以后我再也不敢小看uORB了。

4.1 uORB到底是什么？

uORB全称是micro Object Request Broker，说白了就是一个轻量级的发布-订阅通信框架。它不依赖操作系统，专门为嵌入式实时系统设计。

它的核心思想很简单：

  • 发布者：谁有数据，谁就发布消息
  • 订阅者：谁需要数据，谁就订阅消息
  • 消息主题：数据通过主题名来区分，比如sensor_accel、vehicle_attitude

你想想看，如果没有uORB，每个模块之间直接调用函数，那代码耦合度得多高？IMU驱动要直接调用EKF的接口，GPS驱动又要调用导航的接口……改一个模块，全得跟着改。uORB把这一切解耦了，每个模块只关心自己发布和订阅的消息，互不干扰。

核心特点：

  • 支持多对多通信：一个发布者，多个订阅者；多个发布者，一个订阅者
  • 实时性高：基于共享内存，零拷贝（Zero-copy）设计
  • 线程安全：内部使用互斥锁保护数据
  • 支持阻塞和非阻塞订阅

4.2 uORB的工作机制——一张图看懂

先别急着看代码，咱们先理清uORB的整个数据流。我画了一张图，你看完就明白了。

uORB消息发布与订阅工作机制 硬件传感器 IMU / 磁力计 / GPS 传感器驱动 读取原始数据 uORB 核心 消息主题注册 发布/订阅管理 数据缓存 通知机制 共享内存区 EKF 估计器 订阅 sensor_accel 姿态控制 订阅 vehicle_attitude 位置估计 订阅 sensor_gps 发布 订阅 订阅 订阅 数据流向：硬件 → 驱动（发布） → uORB总线 → 各模块（订阅） 关键点： 驱动只负责发布，不关心谁订阅了 订阅者只负责接收，不关心数据从哪来

你看，整个流程非常清晰。驱动把传感器数据发布到uORB，EKF、姿态控制、位置估计各自订阅自己需要的消息。各干各的，互不依赖。

4.3 如何在驱动中发布传感器数据

好，理论讲完了，咱们直接上代码。我以加速度计为例，演示如何在驱动中发布数据。

4.3.1 定义消息结构

首先，你得知道要发布什么数据。PX4里每个消息都有对应的结构体定义，放在msg/目录下。比如加速度计的消息定义在sensor_accel.msg中：

# sensor_accel.msg
uint64 timestamp          # 时间戳（微秒）
uint64 timestamp_sample   # 采样时间
uint32 device_id          # 设备ID
float32[3] x              # X轴加速度（m/s^2）
float32[3] y              # Y轴加速度
float32[3] z              # Z轴加速度
float32 temperature       # 温度（摄氏度）
uint32 error_count        # 错误计数
uint8[4] _padding0        # 对齐填充

我个人习惯在写驱动前，先看一眼对应的.msg文件。这样能确保你发布的数据字段和订阅者期望的一致。我曾经见过有人把加速度单位搞成g而不是m/s²，结果EKF算出来的姿态全是歪的。

4.3.2 在驱动中发布消息

假设你正在写一个加速度计驱动，核心代码大概长这样：

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

class AccelDriver {
private:
    int _orb_accel_fd;          // 发布句柄
    sensor_accel_s _accel_data; // 消息数据

public:
    AccelDriver() : _orb_accel_fd(-1) {
        // 初始化消息数据
        memset(&_accel_data, 0, sizeof(_accel_data));
    }

    int init() {
        // 第一步：广告（advertise）一个主题
        // 这相当于告诉uORB：“我要发布sensor_accel这个消息了”
        _orb_accel_fd = orb_advertise(ORB_ID(sensor_accel), &_accel_data);

        if (_orb_accel_fd == nullptr) {
            PX4_ERR("Failed to advertise sensor_accel");
            return -1;
        }

        return 0;
    }

    void publish_data(float accel_x, float accel_y, float accel_z) {
        // 第二步：填充数据
        _accel_data.timestamp = hrt_absolute_time();
        _accel_data.timestamp_sample = _accel_data.timestamp;
        _accel_data.device_id = _device_id;
        _accel_data.x[0] = accel_x;
        _accel_data.y[0] = accel_y;
        _accel_data.z[0] = accel_z;
        _accel_data.temperature = 25.0f;

        // 第三步：发布消息
        // orb_publish会把数据拷贝到共享内存区
        int ret = orb_publish(ORB_ID(sensor_accel), _orb_accel_fd, &_accel_data);

        if (ret != PX4_OK) {
            PX4_ERR("Failed to publish sensor_accel");
        }
    }
};

小技巧：

调用orb_advertise时，uORB会分配一块共享内存。这块内存的大小就是消息结构体的大小。所以消息结构体不要定义得太臃肿，否则会浪费内存。我一般控制在64字节以内。

4.3.3 发布频率的控制

发布频率是个坑，我踩过。加速度计通常以1000Hz（1ms）发布，但如果你CPU负载高，发布间隔可能不稳定。

我的做法是：在发布前检查时间戳，确保两次发布间隔不小于预期值。

void publish_with_rate_control() {
    static uint64_t last_pub = 0;
    uint64_t now = hrt_absolute_time();

    // 确保发布间隔不小于1ms
    if (now - last_pub < 1000) {  // 1000微秒 = 1ms
        return;
    }

    last_pub = now;
    // 然后调用publish_data()
}

注意：

不要在主循环里无脑调用orb_publish。如果发布频率过高，订阅者可能来不及处理，导致数据积压。EKF对数据的时间戳非常敏感，时间戳乱跳会直接导致估计发散。

4.4 如何订阅消息——以EKF为例

发布搞定了，咱们看看订阅端怎么玩。EKF是uORB的大户，它订阅的消息少说也有十几个。

4.4.1 订阅的基本流程

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

class EkfEstimator {
private:
    int _sensor_accel_sub;      // 订阅句柄
    sensor_accel_s _accel_data; // 接收数据的缓冲区

public:
    EkfEstimator() : _sensor_accel_sub(-1) {}

    void init() {
        // 第一步：订阅主题
        // orb_subscribe返回一个文件描述符
        _sensor_accel_sub = orb_subscribe(ORB_ID(sensor_accel));

        if (_sensor_accel_sub < 0) {
            PX4_ERR("Failed to subscribe sensor_accel");
            return;
        }

        // 可选：设置订阅队列深度
        // 默认是1，如果发布频率高，可以设大一点
        orb_set_interval(_sensor_accel_sub, 0);  // 0表示不限制
    }

    void update() {
        bool updated = false;

        // 第二步：检查是否有新数据
        // orb_check返回true表示有新消息
        orb_check(_sensor_accel_sub, &updated);

        if (updated) {
            // 第三步：拷贝数据到本地缓冲区
            orb_copy(ORB_ID(sensor_accel), _sensor_accel_sub, &_accel_data);

            // 现在可以使用_accel_data了
            process_accel_data(&_accel_data);
        }
    }

    void process_accel_data(const sensor_accel_s *data) {
        // 在这里做EKF的预测更新
        // 使用data->x[0], data->y[0], data->z[0]
    }
};

4.4.2 阻塞式订阅 vs 非阻塞式订阅

uORB支持两种订阅模式：

模式 函数 特点 适用场景
非阻塞 orb_check() + orb_copy() 立即返回，没有新数据就跳过 主循环中定期检查
阻塞 orb_poll() 等待直到有新数据 专用线程，等待数据到来

我个人推荐在主循环里用非阻塞模式。为什么呢？因为EKF除了处理加速度计数据，还要处理陀螺仪、磁力计、GPS等一堆数据。如果用阻塞模式等加速度计，其他数据就处理不了了。

4.4.3 多消息订阅的轮询技巧

EKF要同时订阅多个消息，怎么高效地轮询？我一般用px4_poll()：

void ekf_poll_all() {
    // 把所有订阅句柄放到一个数组里
    px4_pollfd_struct_t fds[] = {
        { .fd = _sensor_accel_sub,   .events = POLLIN },
        { .fd = _sensor_gyro_sub,    .events = POLLIN },
        { .fd = _sensor_mag_sub,     .events = POLLIN },
        { .fd = _sensor_gps_sub,     .events = POLLIN },
    };

    int ret = px4_poll(fds, sizeof(fds)/sizeof(fds[0]), 100); // 超时100ms

    if (ret > 0) {
        // 检查哪个句柄有数据
        for (int i = 0; i < 4; i++) {
            if (fds[i].revents & POLLIN) {
                // 处理对应的消息
                handle_subscription(fds[i].fd);
            }
        }
    }
}

避坑指南：

我曾经在写EKF时，把订阅句柄的数组顺序搞错了，导致加速度计数据被当成陀螺仪处理。结果姿态估计直接炸了，飞机在地上疯狂抖动。后来我加了个日志，打印每个句柄对应的主题名，才找到问题。

所以，建议你在初始化时打印一下订阅句柄和主题的对应关系：

PX4_INFO("Subscribed to sensor_accel, fd=%d", _sensor_accel_sub);

4.5 uORB的进阶用法

4.5.1 多实例发布

有时候一个系统里有多个同类型传感器，比如两个IMU。这时候就需要多实例发布。每个实例用不同的实例ID区分。

// 发布第一个IMU
orb_advertise_multi(ORB_ID(sensor_accel), &data1, &instance1, 1);

// 发布第二个IMU
orb_advertise_multi(ORB_ID(sensor_accel), &data2, &instance2, 1);

订阅者可以通过orb_subscribe_multi()订阅特定实例，或者订阅所有实例然后自己筛选。

4.5.2 消息优先级

uORB本身不提供优先级机制。但你可以通过发布频率来间接控制。比如，加速度计1000Hz，GPS 10Hz，那加速度计的数据自然更“优先”。

如果你需要真正的优先级，我建议在消息结构体里加一个priority字段，让订阅者自己判断。

4.6 调试uORB的实用技巧

调试uORB问题，我常用的几个命令：

  • uorb top：查看所有主题的发布频率和订阅者数量
  • listener sensor_accel：实时打印某个主题的最新数据
  • uorb info：列出所有已注册的主题

在QGroundControl的MAVLink控制台里就能运行这些命令。我调试时基本离不开它们。

我的习惯：

每写一个新的驱动，我都会先用listener命令确认数据是否正确发布。然后再去订阅端验证接收。这样能快速定位问题是出在发布端还是订阅端。

4.7 本章小结

uORB是PX4的通信基石。说白了，它就是一套“你发布，我订阅”的机制。驱动只管把数据扔到总线上，谁需要谁去拿。

核心要点就三个：

  • 发布：orb_advertise + orb_publish
  • 订阅：orb_subscribe + orb_check + orb_copy
  • 注意：时间戳要准确，发布频率要稳定，订阅要非阻塞

下一章我们会把这些知识串起来，写一个完整的传感器驱动。到时候你会看到uORB在整个系统中的实际运作。

公众号:蓝海资料掘金营，微信deep3321

← 上一章 📖 返回目录 下一章 →