第二章 源码入口分析：mc_att_control_main.cpp 主线程与调度逻辑

好，我们直接进入正题。这一章咱们要啃的是 mc_att_control_main.cpp，也就是多旋翼姿态控制的主线程入口。很多初学者一上来就扎进姿态解算或者PID公式里，结果连控制循环怎么跑起来的都没搞明白。我个人习惯是，先看骨架，再看血肉。

说白了，这个文件就是整个姿态控制模块的“大管家”。它负责初始化、注册UORB主题、启动主循环、处理参数更新，以及最重要的——按固定频率调用姿态控制的核心算法。

2.1 类结构概览：MulticopterAttitudeControl

整个文件的核心是一个类：MulticopterAttitudeControl。它继承自 ModuleBase，这是PX4里所有模块的基类。嗯，这里要注意，PX4的模块化设计做得相当干净，每个功能模块都是一个独立的类，通过UORB通信。

我给大家画了一张图，先看整体脉络：

这张图把主线程的调度逻辑讲得很清楚了。从上到下依次是：类实例化 → 初始化 → 进入主循环 → 循环内三步走。你想想看，是不是很清晰？

2.2 主线程入口：到底谁调用了谁？

很多同学问我：飞控上电后，这个 mc_att_control 是怎么跑起来的？答案藏在 module_base 的机制里。

在 main() 函数中，实际调用的是：

// 伪代码示意，实际在 module_base 中封装
int MulticopterAttitudeControl::main(int argc, char *argv[])
{
    MulticopterAttitudeControl *obj = new MulticopterAttitudeControl();
    obj->init();  // 初始化参数、传感器等
    obj->run();   // 进入主循环
    delete obj;
    return 0;
}

这里有个细节：init() 里做了什么？我打开源码给你们看关键部分：

bool MulticopterAttitudeControl::init()
{
    // 订阅参数更新
    _params_handles = ...;
    _parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
    
    // 订阅传感器数据
    _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
    _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    
    // 发布控制输出主题
    _attitude_control_pub = orb_advertise(ORB_ID(vehicle_attitude_control), &_attitude_control);
    
    return true;
}

我曾经在调试一个原型机时，发现姿态控制完全不响应。查了半天，原来是 init() 里少订阅了一个 vehicle_attitude_setpoint 主题。嗯，这种低级错误，犯过一次就记住了。

2.3 主循环调度：250Hz 的节拍

主循环的核心逻辑在 run() 方法里。PX4 的调度方式有两种：一种是基于定时器中断，另一种是轮询等待。姿态控制用的是后者。

看这段核心代码：

void MulticopterAttitudeControl::run()
{
    // 设置循环频率：250Hz，即4ms一个周期
    _loop_perf = perf_alloc(PC_ELAPSED, MODULE_NAME ": run");
    
    while (!should_exit()) {
        // 等待新数据到来
        orb_check(_vehicle_attitude_sub, &updated);
        
        if (updated) {
            // 读取最新姿态数据
            orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
            
            // 检查参数更新
            bool params_updated = false;
            orb_check(_parameter_update_sub, &params_updated);
            if (params_updated) {
                update_params();
            }
            
            // 核心：姿态控制计算
            generate_attitude_setpoint(attitude_setpoint, _att, _rates_sp);
            
            // 发布控制输出
            orb_publish(ORB_ID(vehicle_attitude_control), _attitude_control_pub, &_attitude_control);
        }
        
        // 精确控制循环周期
        _cycle_counter++;
    }
}

这里有个关键点：orb_check 是非阻塞的。如果数据没更新，它会直接跳过本次循环。但实际运行中，传感器数据通常以 1kHz 的频率发布，所以姿态控制基本能稳定在 250Hz。

2.4 参数更新机制：热插拔式调参

调参时最怕什么？怕改了参数要重启飞控。PX4 的设计就很人性化——参数热更新。

看这段：

void MulticopterAttitudeControl::update_params()
{
    // 从参数系统读取最新值
    param_get(_params_handles.roll_p, &_params.roll_p);
    param_get(_params_handles.roll_i, &_params.roll_i);
    param_get(_params_handles.roll_d, &_params.roll_d);
    // ... 其他参数
    
    // 更新滤波器系数
    update_gain();
}

每次主循环都会检查 parameter_update 主题是否有新消息。如果有，就重新读取所有参数。这意味着你在 QGC 上拖动滑块改 PID，下一秒控制循环就用上了新值。

2.5 调度优先级与实时性

PX4 跑在 NuttX 实时操作系统上。姿态控制线程的优先级是多少？我翻一下源码：

// 在 module_base 中设置
int ret = px4_task_spawn_cmd(name, SCHED_DEFAULT, SCHED_PRIORITY_ATTITUDE_CONTROL, 
                             stack_size, (px4_main_t)&run_trampoline, (char *const *)argv);

SCHED_PRIORITY_ATTITUDE_CONTROL 的优先级通常设为 150 左右（范围 0-255）。这个优先级高于日志记录、状态估计等任务，但低于传感器驱动和角速度控制环。

为什么这么设计？因为姿态控制需要及时响应，但不能抢占传感器数据采集。你想想看，如果姿态控制把传感器驱动给阻塞了，那控制质量反而会下降。

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

讲到这里，我分享几个实战中容易翻车的地方：

1. UORB 主题未更新：我曾经在移植代码时，忘记在 init() 里订阅 vehicle_attitude_setpoint，结果控制输出一直是零。排查了整整一下午。
2. 循环频率不对：有次我把 _loop_perf 的周期设成了 10ms（100Hz），结果飞起来后飞机像喝醉了一样晃。后来才发现是频率太低，姿态响应跟不上。
3. 参数更新时机：在 update_params() 里直接修改积分限幅值，导致积分器瞬间饱和。建议在参数更新后，重置一下积分器状态。

2.7 本章小结

这一章我们拆解了 mc_att_control_main.cpp 的主线程逻辑。核心就三件事：初始化、循环等待、参数热更新。记住这张调度图，后面讲姿态解算和PID控制时，你就能清楚地知道每个函数是在哪个环节被调用的。

下一章我们会深入 generate_attitude_setpoint() 这个核心函数，看看姿态控制算法到底是怎么把期望姿态转化成角速度指令的。嗯，那才是真正的硬核内容。

---

公众号:蓝海资料掘金营，微信deep3321