3、PX4姿态控制架构：软件架构、模块划分、数据流图、关键文件与函数

好，咱们今天来聊聊PX4姿态控制器的“骨架”。说白了，就是看看这套代码到底是怎么组织起来的。我刚开始接触PX4的时候，面对那一堆文件夹和源文件，说实话有点懵。但摸清楚架构之后，你会发现它其实非常清晰，像搭积木一样。

3.1 整体软件架构：一个分层的思想

PX4的姿态控制，不是一个大泥球。它分成了几个清晰的层次。我个人习惯把它理解成“司令部-作战部-执行部”的关系。

• 最上层（司令部）： 位置控制器。它告诉你“往哪飞”，输出期望的姿态（比如期望的滚转角、俯仰角）。
• 中间层（作战部）： 姿态控制器。这是我们今天的重点。它接收司令部的指令，计算出“该用多大的力气”来调整姿态，输出期望的力矩（扭矩）。
• 最下层（执行部）： 混控器 + 执行机构。它把力矩需求，换算成每个电机该转多快，或者舵面该偏多少。

这种分层设计的好处很明显：每一层只关心自己的事。你想优化姿态控制，完全不用动位置控制和混控器。我在项目中遇到过几次，就是因为分层清晰，我们才能快速定位问题——原来是中间层某个参数没调好。

3.2 模块划分：谁负责什么？

在PX4的源码里，姿态控制相关的代码主要集中在 src/modules/mc_att_control/ 这个目录下。嗯，这里要注意，mc 代表多旋翼（Multicopter）。

主要模块可以分成这几块：

模块	核心职责	关键文件
姿态估计	融合IMU、磁力计等数据，算出当前真实的姿态（四元数/欧拉角）。	EKF2、attitude_estimator_q
姿态控制器	根据期望姿态和当前姿态，计算期望角速率。	mc_att_control_main.cpp
角速率控制器	根据期望角速率和当前角速率，计算期望力矩。	mc_att_control_main.cpp
混控器	将期望力矩映射到各个电机的PWM信号。	mixer_multirotor.generated.h

你看，姿态控制本身又分成了“外环”和“内环”。外环是姿态环（角度环），内环是角速率环。为什么这么分？因为角速率环响应更快，能先抑制外界的扰动。我曾经调试一架大四轴，就是先调好了内环，外环随便给个参数都能飞得挺稳。

3.3 数据流图：信号是怎么跑的？

光说模块有点干巴巴的，咱们画个图看看数据是怎么流动的。我手绘了一个SVG图，你感受一下这个流程。

你看，数据从位置控制器下来，先经过姿态外环，算出一个期望的角速率。然后这个期望角速率和传感器反馈的当前角速率一比较，进入内环PID控制器，输出期望力矩。最后混控器把力矩拆成每个电机的转速。

3.4 关键文件与函数：代码里找什么？

如果你想去读源码，别从头读到尾。我建议你直接找这几个关键文件：

• mc_att_control_main.cpp：这是姿态控制的主逻辑。核心函数是 run()，它里面调用了 control_attitude() 和 control_rate()。
• AttitudeControl.hpp：定义了姿态控制器的类。里面有个 update() 方法，就是外环的P控制器。
• RateControl.hpp：角速率控制器的类。里面的 update() 方法实现了PID控制。

举个例子，外环的P控制代码大概长这样（简化版）：

// 姿态外环：期望姿态 - 当前姿态 = 姿态误差
// 然后乘以比例系数，得到期望角速率
math::Vector<3> rate_sp = attitude_p_gain * attitude_error;

内环的PID控制：

// 角速率内环：期望角速率 - 当前角速率 = 角速率误差
// 经过PID运算，得到期望力矩
torque_sp = rate_p_gain * rate_error 
          + rate_i_gain * rate_integral 
          + rate_d_gain * rate_derivative;

3.5 避坑指南：我踩过的几个坑

最后，分享几个我实际项目中遇到的坑，希望能帮你省点时间：

• 坑一： 我曾经以为外环P参数越大越好，结果飞机起飞时直接翻了个跟头。后来才明白，外环P太大，内环根本追不上，系统就发散。
• 坑二： 内环积分项（I）不要给太大。有一次我为了消除静差，把I调得很大，结果飞机悬停时开始低频晃动，像“喝醉了酒”。
• 坑三： 注意传感器延时。IMU数据从采集到被控制器使用，中间有延时。如果延时太大，内环的微分项（D）会放大噪声。我建议在D项前面加个低通滤波器。

嗯，关于架构就先聊这么多。你只要把这张图印在脑子里，读PX4代码就会轻松很多。记住：外环管角度，内环管角速度，混控器管分配。这三层各司其职，缺一不可。

---