第3章：姿态控制核心类——MulticopterAttitudeControl 的初始化与成员变量

好，咱们直接进入正题。上一章我们把姿态控制的整体框架捋了一遍，这一章要啃的，是整个姿态控制模块的“心脏”——MulticopterAttitudeControl 类。说白了，你飞控里所有跟“转”有关的逻辑，最后都会汇聚到这个类里。

我个人习惯，看一个新模块的代码，第一件事不是看它怎么跑，而是看它“长什么样”——也就是它的成员变量和初始化过程。这就像你拿到一架新飞机，先得看看它有哪些舵面、传感器装在哪，对吧？

3.1 类的声明与核心成员

打开 PX4 源码，找到 MulticopterAttitudeControl.hpp。这个类的声明其实不长，但每个成员都很有分量。我挑几个最关键的说说。

class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
{
public:
    MulticopterAttitudeControl();
    ~MulticopterAttitudeControl() override;

    // ... 其他接口

private:
    // 核心控制参数
    mc_att_control_params _params;          // 参数结构体
    vehicle_attitude_setpoint_s _att_sp;    // 姿态设定值
    vehicle_attitude_s _att;                // 当前姿态（四元数）
    vehicle_angular_velocity_s _angular_vel; // 当前角速度

    // 控制律相关
    matrix::Quatf _q;                       // 姿态误差四元数
    matrix::Vector3f _rates_sp;             // 角速度设定值（输出给角速度环）

    // 时间与状态
    hrt_abstime _timestamp_last;            // 上一次运行时间戳
    bool _in_air;                           // 是否在空中
    bool _armed;                            // 是否解锁
};

嗯，这里要注意，_params 这个成员变量，它不是一个简单的结构体，而是把所有跟姿态控制相关的参数打包在了一起。比如你调参时改的 MC_ROLL_P、MC_PITCH_P、MC_YAW_P，全都在这里面。

3.2 初始化过程——从参数到状态

初始化函数在 MulticopterAttitudeControl.cpp 里。我截取一段核心逻辑：

MulticopterAttitudeControl::MulticopterAttitudeControl() :
    ModuleBase(nullptr, "mc_att_control"),
    _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control: loop")),
    _params(&_params_handles, &_params_defaults)
{
    // 1. 订阅必要的 uORB 主题
    _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    _vehicle_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity));

    // 2. 初始化参数句柄
    _params_handles.roll_p = param_find("MC_ROLL_P");
    _params_handles.pitch_p = param_find("MC_PITCH_P");
    _params_handles.yaw_p = param_find("MC_YAW_P");
    // ... 还有十几个参数

    // 3. 设置默认值
    _params_defaults.roll_p = 6.0f;
    _params_defaults.pitch_p = 6.0f;
    _params_defaults.yaw_p = 2.8f;

    // 4. 初始化状态变量
    _in_air = false;
    _armed = false;
    _timestamp_last = 0;
}

这段代码看起来平平无奇，但我得说，这里有个坑。你看它订阅了 vehicle_attitude 和 vehicle_angular_velocity，但并没有订阅 vehicle_attitude_setpoint。为什么？

因为姿态设定值是由上层（比如位置控制或遥控器）通过 uORB 发布的，姿态控制模块只需要在每次循环里去读最新的设定值就行。我曾经在项目中犯过这个错——把设定值也做成订阅，结果发现数据更新不同步，飞起来总是慢半拍。

3.3 参数更新机制

参数不是一成不变的。你在 QGC 上改了 MC_ROLL_P，飞控得能感知到变化。PX4 的做法是：每次主循环开始前，检查参数是否更新。

void MulticopterAttitudeControl::parameters_update()
{
    // 检查是否有参数更新
    if (_params_sub.updated()) {
        // 读取所有参数的最新值
        param_get(_params_handles.roll_p, &_params.roll_p);
        param_get(_params_handles.pitch_p, &_params.pitch_p);
        param_get(_params_handles.yaw_p, &_params.yaw_p);
        // ... 其他参数

        // 更新参数缓存
        _params.roll_p = math::constrain(_params.roll_p, 0.0f, 20.0f);
        _params.pitch_p = math::constrain(_params.pitch_p, 0.0f, 20.0f);
    }
}

这里有个细节：param_get 之后，最好做一次限幅。为什么？因为用户可能在 QGC 里输入一个离谱的值，比如 MC_ROLL_P = 999。如果不限幅，飞机会直接翻跟头。嗯，我当年调试时就被这个坑过——遥控器一打杆，飞机直接 720 度旋转，吓得我赶紧切自稳。

参数名	默认值	典型范围	说明
MC_ROLL_P	6.0	4.0 ~ 12.0	横滚角比例增益
MC_PITCH_P	6.0	4.0 ~ 12.0	俯仰角比例增益
MC_YAW_P	2.8	1.5 ~ 5.0	偏航角比例增益
MC_ROLLRATE_P	0.15	0.1 ~ 0.4	横滚角速率比例增益

3.4 核心数据结构——四元数与向量

姿态控制里，四元数是绕不开的。PX4 使用 matrix::Quatf 来表示姿态。这个类封装了四元数的基本运算，比如乘法、求逆、旋转向量等。

// 姿态误差计算
_q = _att_sp.q.inversed() * _att.q;  // 注意：这里是期望的逆乘以当前

// 将四元数误差转换为角速度设定值
_rates_sp(0) = _params.roll_p * 2.0f * _q(0) * _q(1);   // 横滚轴
_rates_sp(1) = _params.pitch_p * 2.0f * _q(0) * _q(2);  // 俯仰轴
_rates_sp(2) = _params.yaw_p * 2.0f * _q(0) * _q(3);    // 偏航轴

这段代码是姿态控制的核心。你想想看，四元数误差 _q 的实部 _q(0) 代表“旋转角度的一半的余弦”，虚部 _q(1)、_q(2)、_q(3) 代表“旋转轴乘以角度的一半的正弦”。所以 2 * q0 * q1 其实就是小角度近似下的横滚角误差。

为什么乘以 2？因为四元数误差的虚部 ≈ 角度误差的一半。乘以 2 之后，就得到了近似的角度误差（弧度）。这个技巧在很多飞控里都能看到。

3.5 时间戳与运行频率

最后提一下时间戳。飞控是实时系统，控制律的计算频率必须稳定。PX4 的姿态控制环默认运行在 250Hz 到 400Hz 之间，具体取决于 CPU 负载。

// 计算时间差
float dt = (hrt_absolute_time() - _timestamp_last) / 1e6f;
_timestamp_last = hrt_absolute_time();

// 如果时间差太大，说明发生了丢帧
if (dt > 0.01f) {  // 超过 10ms
    // 做保护处理，比如重置积分项
}

这个 dt 在角速度控制环里会用到，用于积分项的计算。如果 dt 不稳定，积分项就会出问题。我建议你在调参时，先看看 dt 的波动情况——如果波动超过 20%，说明你的 CPU 可能扛不住了，需要优化代码或者降低控制频率。

好了，这一章我们拆解了 MulticopterAttitudeControl 类的初始化流程和核心成员变量。下一章，我们会深入它的主循环，看看姿态误差到底是怎么算的，以及如何输出给角速度环。

---