3、模式切换核心机制：mode.cpp源码导读、switch_mode()函数执行流程、模式初始化与退出回调

模式切换，说白了就是飞控的「换挡」动作。你想想看，无人机从起飞到降落，中间要经历多少种状态？解锁、悬停、航线飞行、返航、降落……每一种状态背后，都是一套完全不同的控制逻辑。而把这些逻辑串起来的，就是 mode.cpp 里的那套切换机制。

我刚开始啃这部分代码时，觉得不就是个状态机嘛，能有多复杂？结果第一次在 SITL 里手动切模式，飞机直接翻了……嗯，从那以后我再也不敢小看 switch_mode() 这个函数了。

3.1 mode.cpp 源码结构概览

先看看 mode.cpp 在代码里扮演什么角色。它位于 ArduCopter 主目录下，是所有飞行模式的基类。说白了，Loiter、RTL、Auto 这些具体模式，都是它的子类。

核心文件路径：

ArduCopter/mode.cpp —— 基类实现
ArduCopter/mode.h —— 基类声明
ArduCopter/mode_*.cpp —— 各模式具体实现（如 mode_loiter.cpp）

我习惯把 mode.cpp 比作一个「模式管理器」。它不关心你具体在做什么动作，它只负责两件事：切换和 生命周期管理。

组件	职责	我踩过的坑
`Mode` 基类	定义通用接口：init()、run()、exit()	忘记在子类重写 exit()，导致资源泄漏
`mode()` 全局指针	指向当前活跃模式对象	指针悬空时直接调用 run() 会崩
`switch_mode()`	执行切换逻辑，调用回调	在中断里调它，死锁过两次
`mode_table[]`	模式编号到对象的映射表	索引越界，查到了空指针

3.2 switch_mode() 执行流程

这个函数是整个切换逻辑的入口。每次你在地面站点一下模式切换，或者飞控自动触发返航，最终都会走到这里。

我把它拆成 5 个关键步骤，你跟着走一遍就明白了：

前置检查 —— 判断当前是否允许切换。比如还在校准、还在解锁过程中，直接拒绝。
调用旧模式的 exit() —— 告诉当前模式：「你要下岗了，把资源交出来」。
更新全局 mode 指针 —— 把指针指向新模式对象。
调用新模式的 init() —— 新模式初始化，比如重置积分项、设置目标位置。
后置处理 —— 更新状态机标志位、通知 GCS、记录日志。

一个小技巧： 我在调试 SITL 时，经常在 switch_mode() 入口加一条 printf，打印当前时间和切换前后的模式编号。这样一旦飞机行为异常，我就能从日志里定位到是哪一次切换出了问题。

下面是简化后的核心代码，我删掉了锁和异常处理，只保留主干：

bool Copter::switch_mode(const uint8_t new_mode, const ModeReason reason)
{
    // 1. 前置检查
    if (!mode_allows_arming(new_mode) && arming.is_armed()) {
        return false;  // 不允许在 armed 状态下切到某些模式
    }

    // 2. 调用旧模式的退出回调
    if (mode != nullptr) {
        mode->exit();  // 清理资源、停止控制输出
    }

    // 3. 更新全局模式指针
    Mode *old_mode = mode;
    mode = mode_table[new_mode];

    // 4. 调用新模式的初始化回调
    if (!mode->init()) {
        // 初始化失败，回滚到旧模式
        mode = old_mode;
        return false;
    }

    // 5. 后置处理
    set_mode_reason(reason);
    gcs().send_message(MSG_HEARTBEAT);
    logger.Write_Mode(new_mode, reason);

    return true;
}

注意第 4 步的 init() 返回值。如果新模式初始化失败（比如 GPS 信号不好导致 Loiter 无法进入），switch_mode() 会回滚到旧模式。这个机制救过我一次——有一次在 SITL 里模拟 GPS 丢失，飞机自动切到 Land，但 Land 的 init() 因为高度数据异常失败了，结果飞机稳稳地保持在原来的 AltHold 模式，没有失控。

3.3 模式初始化与退出回调

每个模式都有两个生命周期回调：init() 和 exit()。它们分别在模式「进入时」和「离开时」被调用。

3.3.1 init() —— 进入时的准备工作

我举个例子，Loiter 模式的 init() 会做这几件事：

检查 GPS 状态和定位精度
记录当前经纬度作为悬停点
重置位置控制器的积分项
设置目标速度为 0

如果 GPS 状态不满足要求，init() 返回 false，切换就会被拒绝。你想想看，这其实是一种保护——与其让飞机在定位不准的情况下乱飘，不如保持原模式。

注意： 我在项目中遇到过一个问题：某个自定义模式在 init() 里申请了动态内存，但 exit() 里忘了释放。结果反复切换几次后，内存耗尽，飞控直接重启。所以我的建议是：init() 和 exit() 一定要成对出现，申请的资源必须在 exit() 里释放干净。

3.3.2 exit() —— 离开时的清理工作

exit() 的职责正好和 init() 相反。它要确保当前模式退出后，不会给下一个模式留下「烂摊子」。

常见的清理动作包括：

停止所有电机输出（防止控制指令残留）
释放占用的硬件资源（如串口、PWM 通道）
重置内部状态变量（如航点索引、计时器）
通知其他模块（如导航库）模式已变更

我见过一个比较隐蔽的 bug：某个模式在 exit() 里没有把目标航点索引归零，导致切回 Auto 模式后，飞机直接飞向了上一个任务中已经执行过的航点。嗯，这种问题在 SITL 里很容易复现，但在真机上可能酿成大祸。

3.4 模式切换的完整流程图

下面这张图把整个流程串起来了。我建议你把它打印出来贴在工位上，调试时对照着看，思路会清晰很多。

3.5 我在 SITL 中调试模式切换的经验

最后分享几个实战技巧，都是我亲手踩过的坑：

用 --speedup 参数加速测试：SITL 默认是实时仿真，但模式切换逻辑往往在几毫秒内完成。我习惯把速度调到 10 倍，这样 10 分钟的测试 1 分钟就跑完了。
在 init() 里加超时保护：有些模式的初始化需要等待传感器数据，比如 Auto 模式要等航点加载。如果数据一直不来，init() 会卡住。我一般加一个 500ms 的超时，超时直接返回 false。
日志是调试的上帝视角：每次模式切换，我都会把 new_mode、old_mode、reason、init() 返回值 全部打出来。用 mavlink inspector 看日志，比在终端里看 printf 舒服多了。

一个实用的小工具： 我在 SITL 里写了一个 Python 脚本，可以自动循环切换所有模式，每次切换后悬停 3 秒，然后记录飞控状态。这样跑一晚上，基本能覆盖所有边界情况。代码我放在 GitHub 上了，需要的同学可以找我要链接。

模式切换看起来简单，但它是飞控稳定性的基石。你想想看，如果切换过程中有一个锁没处理好，或者一个指针没更新，飞机可能瞬间失控。所以，switch_mode() 里的每一行代码，都值得你认真对待。

📚 课程目录

3、模式切换核心机制：mode.cpp源码导读、switch_mode()函数执行流程、模式初始化与退出回调

3.1 mode.cpp 源码结构概览

3.2 switch_mode() 执行流程

3.3 模式初始化与退出回调

3.3.1 init() —— 进入时的准备工作

3.3.2 exit() —— 离开时的清理工作

3.4 模式切换的完整流程图

3.5 我在 SITL 中调试模式切换的经验