第4章源码结构解析

4. 源码结构解析：深入分析PX4固件源码目录结构，定位飞行模式相关代码

好，咱们直接进入正题。这一章，我带大家把PX4的源码目录翻个底朝天。说白了，就是让你知道飞行模式相关的代码到底藏在哪，长什么样。

我记得刚接触PX4那会儿，面对src目录下几十个文件夹，整个人是懵的。后来摸清了门道，发现其实就几个核心模块在干活。今天咱们就聚焦这三个：Navigator、Commander、MC_pos_control。

4.1 顶层目录结构速览

先看个大概。PX4固件的源码主要在src目录下。我习惯把它分成三层：

modules：模块层，每个模块独立运行，通过uORB通信
lib：库层，数学工具、控制算法、EKF等
drivers：驱动层，传感器、执行器、GPS等

飞行模式相关的代码，基本都集中在src/modules里。你想想看，飞行模式本质上就是一系列状态机的组合，而状态机就散落在各个模块中。

核心路径速查：

src/modules/commander — 模式切换的总控
src/modules/navigator — 自主导航逻辑
src/modules/mc_pos_control — 多旋翼位置控制
src/modules/mc_att_control — 多旋翼姿态控制
src/modules/fw_pos_control — 固定翼位置控制

4.2 Commander：模式切换的“大脑”

Commander这个模块，说白了就是整个飞控的调度中心。它负责接收遥控器信号、地面站指令、以及内部状态变化，然后决定要不要切换飞行模式。

它的核心文件就两个：

commander.cpp — 主逻辑，处理模式切换请求
state_machine_helper.cpp — 状态机实现，检查切换条件

我给大家看一段关键代码，这是模式切换的入口：

// commander.cpp 中处理模式切换的核心函数
void Commander::handle_command(const vehicle_command_s &cmd)
{
    switch (cmd.command) {
    case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE:
        // 检查当前状态是否允许切换
        if (transition_result == TRANSITION_DENIED) {
            // 拒绝切换，发送失败应答
            publish_mode_switch_result(false);
            return;
        }
        // 执行模式切换
        set_nav_state(static_cast<nav_state_t>(cmd.param1));
        break;
    // ... 其他命令处理
    }
}

嗯，这里要注意。Commander不会直接执行控制逻辑，它只负责“下命令”。真正的控制代码在别的模块里。这就像项目经理只负责分配任务，具体干活的是工程师。

个人经验：我曾经在调试一个自定义模式时，发现模式切换总是失败。查了两天，最后发现是Commander里的transition_result检查太严格了。有些条件在特定场景下其实可以放宽，但默认代码是保守的。所以如果你要加新模式，记得先看看state_machine_helper.cpp里的检查逻辑。

4.3 Navigator：自主导航的“大脑”

Navigator模块负责处理自主飞行任务。比如“飞到这个点”、“绕这个圈转”、“降落到这里”。它不直接控制电机，而是生成期望的位置和速度，然后发给位置控制器。

它的目录结构是这样的：

navigator_main.cpp — 主循环，调度各个任务
mission.cpp — 航点任务执行
loiter.cpp — 悬停/绕圈逻辑
rtl.cpp — 返航逻辑
takeoff.cpp — 起飞逻辑
land.cpp — 降落逻辑

每个飞行模式，在Navigator里基本都有一个对应的文件。比如你切到“Mission模式”，Navigator就会调用mission.cpp里的逻辑。

我画了一张图，帮你理清Navigator和Commander的关系：

4.4 MC_pos_control：位置控制的“肌肉”

这个模块负责把期望位置/速度，转化成实际的推力指令。它运行在很高的频率（250Hz-1kHz），是真正的实时控制环。

核心文件：

PositionControl.cpp — 位置环和速度环PID
ControlMath.cpp — 数学工具，比如限幅、坐标变换
mc_pos_control_main.cpp — 主循环，订阅uORB话题

我给大家看一段位置控制的核心逻辑：

// PositionControl.cpp 中的位置环计算
void PositionControl::_positionControl()
{
    // 计算位置误差
    Vector3f pos_error = _pos_sp - _pos;
    
    // 位置环P控制，生成速度期望
    Vector3f vel_sp = pos_error.emult(_param_mpc_xy_p.get());
    
    // 限幅到最大速度
    float vel_norm = vel_sp.norm();
    if (vel_norm > _param_mpc_xy_vel_max.get()) {
        vel_sp = vel_sp / vel_norm * _param_mpc_xy_vel_max.get();
    }
    
    // 加上前馈速度
    vel_sp += _vel_sp_ff;
}

你想想看，这里有个关键点：位置环的输出是速度期望，然后速度环再输出加速度期望，最后转成推力。这就是经典的串级PID结构。

避坑指南：我曾经在调参时发现飞机在悬停模式下会缓慢漂移。查了半天，发现是_pos_sp（位置期望）没有正确更新。原来Navigator在悬停模式下，位置期望是锁死的，但如果GPS有漂移，实际位置会慢慢偏离。解决办法是在位置控制里加一个“位置保持”的积分项，或者定期用GPS校正期望点。

4.5 三个模块如何协同工作

说白了，这三个模块就是一条流水线：

Commander 决定“我们要干什么”（模式切换）
Navigator 决定“我们要去哪”（生成期望轨迹）
MC_pos_control 决定“怎么去”（执行控制算法）

它们之间通过uORB消息传递数据。我列个表，把关键话题说清楚：

uORB话题	发布者	订阅者	内容说明
`vehicle_status`	Commander	Navigator, MC_pos_control	当前飞行模式、系统状态
`vehicle_command`	地面站/遥控器	Commander	模式切换指令
`position_setpoint_triplet`	Navigator	MC_pos_control	期望位置/速度/加速度
`vehicle_local_position`	EKF	MC_pos_control, Navigator	当前估计位置
`actuator_controls`	MC_pos_control	混控器	最终推力/力矩指令

个人建议：如果你想自定义一个飞行模式，不要急着改Commander。先看看Navigator里有没有类似的任务逻辑。比如你想做一个“环绕飞行”，可以直接复用loiter.cpp里的绕圈逻辑，改一下半径和速度参数就行。我当年做“扇形扫描”模式就是这么干的，省了不少事。

4.6 总结一下

这一章我们翻遍了PX4的源码目录，找到了飞行模式相关的三个核心模块。记住它们的职责：

Commander — 模式切换的决策者
Navigator — 任务逻辑的执行者
MC_pos_control — 底层控制的执行者

嗯，其实还有个MC_att_control（姿态控制）也很重要，但咱们今天先聚焦位置控制。下一章我们会深入Navigator的代码，看看航点任务到底是怎么一步步执行的。

对了，如果你在源码里迷路了，记住一个技巧：先看CMakeLists.txt，那里列出了模块的所有源文件。我每次接手新项目，第一件事就是打开这个文件，快速摸清代码结构。

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

📚 课程目录

4. 源码结构解析：深入分析PX4固件源码目录结构，定位飞行模式相关代码

4.1 顶层目录结构速览

4.2 Commander：模式切换的“大脑”

4.3 Navigator：自主导航的“大脑”

4.4 MC_pos_control：位置控制的“肌肉”

4.5 三个模块如何协同工作

4.6 总结一下