4. PX4姿态控制器源码解析：mc_att_control模块

姿态控制是四旋翼飞控的核心。说白了，就是让飞机按照你想要的姿态飞。PX4的mc_att_control模块，我看了不下二十遍。每次看都有新收获。今天咱们就把它拆开，看看角速度环和角度环到底是怎么耦合的。

4.1 整体架构：双环串级控制

PX4的姿态控制采用经典的串级PID结构。外环是角度环，内环是角速度环。为什么这么设计？

你想想看，角度环的输出是期望角速度，角速度环的输出是期望力矩。这种结构的好处是：内环响应快，能迅速抑制扰动；外环响应慢，保证姿态跟踪精度。

我在项目中遇到过一个问题：单环控制时，飞机遇到阵风会剧烈晃动。换成串级结构后，内环先把角速度稳住，外环再慢慢调整角度。效果立竿见影。

4.2 角度环：从四元数到期望角速度

角度环的输入是期望姿态（四元数）和当前姿态（四元数）。输出是三个轴的期望角速度。

PX4没有直接用欧拉角做误差计算。为什么？因为欧拉角有万向锁问题。四元数虽然难懂，但胜在平滑无奇点。

代码里核心就几步：

// 计算姿态误差四元数
Quatf qe = q_desired * q_current.inversed();

// 从四元数提取角度误差（简化版）
Eulerf euler_error(qe);
// 实际代码用的是更复杂的映射，这里简化说明

// P控制器输出期望角速度
rates_sp = attitude_p_gain * euler_error.xyz();

4.3 角速度环：PID控制的精髓

角速度环接收期望角速度，输出期望力矩。这里用的是完整的PID控制。

看代码：

// 角速度误差
float rate_error = rates_sp - rates_current;

// PID计算
float integral = _rate_int + rate_error * dt;
// 抗积分饱和
integral = math::constrain(integral, -lim, lim);

// 输出力矩
float torque = _rate_p * rate_error 
             + _rate_i * integral 
             + _rate_d * (rate_error - last_error) / dt;

这里有个细节：微分项用的是误差微分，不是角加速度。我刚开始没注意，以为直接用陀螺仪的角加速度就行。后来发现噪声太大，根本没法用。

4.4 耦合设计：角度环与角速度环如何配合

两个环不是独立工作的。它们通过一个关键变量连接：期望角速度。

角度环的输出限幅，直接决定了角速度环的输入范围。这个限幅值就是最大角速度。

参数	作用	典型值	我的建议
MC_ROLL_P	角度环P增益	0.15	从0.1开始，每次加0.02
MC_ROLLRATE_P	角速度环P增益	0.15	先调这个，再调角度环
MC_ROLLRATE_I	角速度环I增益	0.05	有静差再加，别加太多
MC_ROLLRATE_D	角速度环D增益	0.003	小心噪声，我一般最后调

4.5 避坑指南：我踩过的三个坑

做姿态控制调参，有几个坑我印象特别深：

• 积分饱和：角速度环的积分项一定要限幅。我曾经没限幅，飞机悬停时积分越积越大，最后电机直接满油门。嗯，从那以后我每次都会检查MC_ROLLRATE_I_LIM。
• 微分噪声：陀螺仪数据有高频噪声，微分项会放大它。我建议加个低通滤波器，截止频率设在30Hz左右。
• 角度环限幅：角度环输出的期望角速度不能太大。我一般设到每秒200度，再大飞机就失控了。

4.6 调试技巧：如何判断耦合是否合理

怎么知道两个环配合得好不好？我一般看飞行日志：

1. 看角度误差曲线：如果误差收敛快、超调小，说明角度环OK。
2. 看角速度跟踪：期望角速度和实际角速度的延迟不能太大。
3. 看力矩输出：如果力矩频繁饱和，说明角速度环增益太高。

我个人习惯用ulog2csv把日志转成CSV，然后用Python画图。一眼就能看出问题。