4. PX4姿态控制代码结构：mc_att_control模块解析

好，咱们进入正题。这一章我打算带你拆开 mc_att_control 模块，看看PX4的姿态控制到底是怎么跑起来的。说实话，我刚接触这个模块时也被它的调用链绕晕过——明明就是个姿态控制，怎么还牵扯到混控器、传感器、参数更新？后来跟了几遍代码才摸清门道。

说白了，姿态控制就是回答三个问题：我现在姿态是多少？我想去哪个姿态？怎么过去？ 代码里对应的就是状态估计、期望生成、控制律计算。咱们今天重点看第三个。

4.1 模块入口与主循环

模块入口在 src/modules/mc_att_control/MulticopterAttitudeControl.cpp。启动后，主循环长这样：

// 伪代码示意，实际更复杂
while (!should_exit()) {
    // 1. 等待新传感器数据
    vehicle_attitude_sub.update();
    vehicle_angular_velocity_sub.update();
    
    // 2. 读取参数（如果更新了）
    parameter_update_sub.update();
    
    // 3. 核心控制计算
    generate_attitude_setpoint();
    control_attitude();
    control_rate();
    
    // 4. 输出到混控器
    actuator_controls_pub.publish(control_output);
    
    // 5. 调度频率控制
    schedule_at_frequency(250); // 250Hz
}

嗯，这里要注意：主循环频率是250Hz，但内环速率控制跑得更快——实际是400Hz。为什么差这么多？我当年也纳闷，后来发现姿态环（外环）对延迟不敏感，但速率环（内环）必须快，否则飞起来会抖。

核心要点： 外环（姿态）250Hz，内环（角速率）400Hz。这个比例是经验值，别乱改。

4.2 姿态控制主循环：三步走

控制主循环分三步，我习惯叫它「期望→姿态→速率」链路。你想想看，这其实是个串级PID：外环算期望力矩，内环算期望油门。

4.2.1 生成姿态期望

这一步在 generate_attitude_setpoint() 里。输入是遥控器或任务规划器给的期望，输出是四元数形式的期望姿态。代码里有个关键函数：

// 从遥控杆或航点生成期望姿态
void generate_attitude_setpoint() {
    // 如果是遥控模式，直接映射杆量
    if (_control_mode.flag_control_manual_enabled) {
        _att_sp = manual_to_attitude_setpoint(_manual);
    } else {
        // 自动模式：从位置环拿期望
        _att_sp = _pos_sp_triplet_sub.get();
    }
    // 限幅：最大倾斜角
    constrain_tilt(_att_sp, _param_mpc_tiltmax_l.get());
}

我个人习惯在这里加个日志，打印期望四元数——调试时能快速判断是期望给错了还是控制算错了。

4.2.2 姿态控制律（外环）

核心函数 control_attitude()。它用四元数误差算期望角速率：

// 简化版姿态控制律
void control_attitude() {
    // 1. 计算四元数误差
    Quatf q_error = q_desired * q_current.inversed();
    
    // 2. 提取旋转向量（轴角）
    Vector3f rot_vec = q_error.to_axis_angle();
    
    // 3. P控制 + 前馈
    Vector3f rate_sp = rot_vec * _param_mc_att_p.get();
    rate_sp += _rate_feedforward; // 前馈项
    
    // 4. 限幅
    constrain_rate(rate_sp, _param_mc_rate_max.get());
}

避坑指南： 我曾经把四元数误差符号搞反过——飞控一解锁就翻跟头。检查方法很简单：悬停时给个小角度期望，看机体是不是朝正确方向转。

4.2.3 速率控制律（内环）

内环在 control_rate() 里，用的是标准PID：

void control_rate() {
    // 三轴独立PID
    for (int i = 0; i < 3; i++) {
        float error = rate_sp(i) - rate_measured(i);
        float p_term = error * _param_mc_rate_p.get(i);
        float i_term = _integral(i) + error * dt * _param_mc_rate_i.get(i);
        float d_term = (error - _prev_error(i)) / dt * _param_mc_rate_d.get(i);
        
        // 积分限幅
        i_term = constrain(i_term, -_param_mc_rate_imax.get(i), _param_mc_rate_imax.get(i));
        
        _output(i) = p_term + i_term + d_term;
        _prev_error(i) = error;
    }
}

这里有个细节：D项用的是误差微分，不是测量值微分。为什么？因为测量值微分会放大噪声，而误差微分能避免「微分爆炸」。我踩过这个坑——直接用陀螺仪微分做D项，结果飞控高频抖动，差点炸机。

4.3 参数调优接口

调参接口集中在 mc_att_control_params.c 里。我把常用参数整理成了一张表：

参数名	默认值	作用	调优建议
`MC_ATT_P`	6.5	姿态外环P增益	越大响应越快，但超过10容易超调
`MC_RATE_P`	[0.15, 0.15, 0.20]	速率内环P增益（滚转/俯仰/偏航）	从0.1开始，每次加0.02直到轻微震荡
`MC_RATE_I`	[0.05, 0.05, 0.10]	速率内环I增益	消除稳态误差，太大导致积分饱和
`MC_RATE_D`	[0.003, 0.003, 0.000]	速率内环D增益	抑制震荡，偏航通常不加D
`MC_RATE_MAX`	[360, 360, 180]	最大角速率（°/s）	特技飞行可以调高，新手建议降低

警告： 调参时一定要先调内环（速率），再调外环（姿态）。顺序反了，你调出来的参数全是错的。我见过有人先调姿态P，结果飞起来像弹簧——因为内环根本没锁住。

4.4 模块整体结构图

下面这张图展示了 mc_att_control 模块的数据流和控制链路。我建议你把它打印出来贴在工位上——调试时对照着看，思路会清晰很多。

4.5 调试与验证

代码写完了，参数调好了，怎么验证？我一般用三步：

地面测试： 解锁后轻推摇杆，观察机体响应。如果推杆后机体朝反方向动——嗯，你符号搞反了。
悬停测试： 起飞到1米高，切增稳模式。看姿态角是否在±2°内稳定。如果持续震荡，降低速率环P增益。
阶跃响应： 快速打杆然后回中，看超调量和调节时间。我习惯用 logger 录下 vehicle_attitude 和 vehicle_angular_velocity，然后画图分析。

个人经验： 调参时别一次改太多。每次只改一个参数，改完飞一次。我见过有人同时改了P、I、D三个参数，结果飞起来不知道是哪个参数的问题——这种调试方式，说白了就是碰运气。

好了，这一章的内容就到这儿。你理解了 mc_att_control 的代码结构、主循环流程和参数接口，后面调参和改代码就有底了。记住那张结构图——它是我调试时的「地图」，希望你也能用上。

📚 课程目录

4. PX4姿态控制代码结构：mc_att_control模块解析

4.1 模块入口与主循环

4.2 姿态控制主循环：三步走

4.2.1 生成姿态期望

4.2.2 姿态控制律（外环）

4.2.3 速率控制律（内环）

4.3 参数调优接口

4.4 模块整体结构图

4.5 调试与验证