PX4 uORB 通信机制（下）：WorkQueue 实例模板

一、dt 的意义与限幅

1.1 为什么飞控算法强依赖 dt

飞控算法是连续时间模型的离散实现，几乎所有关键计算都依赖两次执行之间的时间间隔 dt（单位秒）：

算法环节	dt 的作用
积分（I 项）	integral += error * dt，dt 大则积分快
微分（D 项）	derivative = (error - last_error) / dt，dt 小则放大噪声
低通滤波器	滤波器系数 α 由 dt 与截止频率共同决定
状态传播	v += a * dt，x += v * dt，dt 错则状态估计发散

计算方式：

const uint64_t now_us = hrt_absolute_time();
float dt = (now_us - _last_run_us) * 1e-6f;
_last_run_us = now_us;

1.2 为什么必须对 dt 限幅

实际中 dt 会因 CPU 忙、队列阻塞、线程调度抖动、任务被长时间抢占而波动，出现 0.001 s、0.004 s、0.020 s 的跳变。不限幅时：

• 积分项被偶发大 dt 破坏，产生积分爆炸
• 滤波器有效带宽被改变，响应不一致
• 状态预测产生误差积累

标准做法：

if (dt < 0.0005f) { dt = 0.0005f; }  // 最小 0.5ms，防止噪声放大
if (dt > 0.02f)   { dt = 0.02f;   }  // 最大 20ms，防止积分爆炸

---

二、HRT 的作用

HRT（High Resolution Timer）是 PX4 自建的统一微秒时基系统，提供三个接口：

hrt_absolute_time()     // 返回系统从启动以来的微秒计时（单调递增）
hrt_call_every(...)     // 注册周期回调（用于周期性任务/轮询）
hrt_call_after(...)     // 延时一次性回调

所有传感器时间戳、EKF 时间对齐、控制 dt 都基于同一个单调微秒时钟，保证系统内时间域一致。1 kHz 级别任务可靠，抖动远低于 OS 调度时钟。

相关知识：uORB 通信机制（上）

---

三、WorkQueue 实例模板

以下是 WorkQueue 模式的完整代码模板，集成了 WorkQueue + uORB 回调 + HRT dt + 参数订阅的标准范式：

#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/log.h>

#include <uORB/SubscriptionCallbackWorkItem.hpp>
#include <uORB/Publication.hpp>

#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>

using namespace time_literals;

class UorbWqExample final
    : public ModuleBase<UorbWqExample>
    , public ModuleParams
    , public px4::WorkItem
{
public:
    UorbWqExample() :
        ModuleParams(nullptr),
        WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
    {}

    ~UorbWqExample() override {
        _att_sub.unregisterCallback();
    }

    // ModuleBase 统一 CLI：start/stop/status/help
    static int task_spawn(int argc, char *argv[]) {
        UorbWqExample *inst = new UorbWqExample();
        if (!inst) { return -1; }

        _object.store(inst);
        _task_id = task_id_is_work_queue;

        if (inst->init()) { return 0; }

        delete inst;
        _object.store(nullptr);
        _task_id = -1;
        return -1;
    }

    static UorbWqExample *instantiate(int argc, char *argv[]) {
        // WorkQueue 模式不走 instantiate/run_trampoline
        return nullptr;
    }

    bool init() {
        // 1) 注册 attitude 更新回调：有新数据就 ScheduleNow() -> Run()
        if (!_att_sub.registerCallback()) {
            PX4_ERR("attitude callback registration failed");
            return false;
        }
        // 2) 定时兜底：避免完全依赖回调（可选）
        ScheduleDelayed(10_ms);
        _last_run_us = hrt_absolute_time();
        return true;
    }

    void Run() override {
        // 退出路径：WorkQueue 模式必须在 Run 内自收尾
        if (should_exit()) {
            _att_sub.unregisterCallback();
            exit_and_cleanup();
            return;
        }

        // 0) 参数刷新：parameter_update 是标准做法
        if (_param_update_sub.updated()) {
            parameter_update_s p{};
            _param_update_sub.copy(&p);
            updateParams();  // ModuleParams: 把 PARAM_* 同步到本地变量
        }

        // 1) HRT dt 计算（必须限幅）
        const uint64_t now_us = hrt_absolute_time();
        float dt = (now_us - _last_run_us) * 1e-6f;
        _last_run_us = now_us;
        if (dt < 0.0005f) { dt = 0.0005f; }
        if (dt > 0.02f)   { dt = 0.02f; }

        // 2) uORB 输入：通常有更新，但仍要用 update() 保证
        vehicle_attitude_s att{};
        if (_att_sub.update(&att)) {

            // 3) 参数控制逻辑
            if (_param_enable.get() == 0) { return; }

            // 4) 算法：此处为示意，实际需要四元数误差/角速度闭环等
            vehicle_rates_setpoint_s rates_sp{};
            rates_sp.timestamp = now_us;
            rates_sp.roll  = _param_p.get() * 0.0f;
            rates_sp.pitch = _param_p.get() * 0.0f;
            rates_sp.yaw   = _param_p.get() * 0.0f;

            // 5) uORB 输出
            _rates_sp_pub.publish(rates_sp);
        }

        // 6) 定时兜底：每 10ms 至少跑一次
        ScheduleDelayed(10_ms);
    }

    int print_status() override {
        PX4_INFO("running");
        PX4_INFO("enable=%d p=%.3f", _param_enable.get(), (double)_param_p.get());
        return 0;
    }

private:
    // 回调驱动订阅（WorkQueue 事件触发）
    uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)};
    // 参数更新订阅
    uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};
    // 发布
    uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
    // HRT 时间戳
    uint64_t _last_run_us{0};

    DEFINE_PARAMETERS(
        (ParamInt<px4::params::SYS_AUTOSTART>) _param_enable,
        (ParamFloat<px4::params::MPC_XY_P>)    _param_p
    )
};

extern "C" __EXPORT int uorb_wq_example_main(int argc, char *argv[]) {
    return UorbWqExample::main(argc, argv);
}

---

四、WorkQueue 模式关键约束

约束	原因
Run() 内禁止 px4_poll	WorkQueue 线程是共享线程，阻塞会卡住整个队列
Run() 内禁止阻塞 I/O	同上
不要打印高频日志	串口/文件 I/O 有延迟，高频调用会导致 WorkQueue 积压
回调 + ScheduleDelayed 同时存在时	频率由兜底周期决定，两者会叠加触发

---

五、三种订阅方式的选型建议

方式	类型	适用场景
uORB::Subscription::update()	轮询式	简单低频模块，独立线程循环
px4_poll() + orb_copy()	阻塞式	独立线程，等待多个 topic 中任意一个更新
SubscriptionCallbackWorkItem	回调式	WorkQueue 模式，高频控制算法（姿态、位置环）

---

上一篇：uORB 通信机制（上）
下一篇：DShot 电机转向修改