4、模式切换逻辑设计：基于状态机的模式切换逻辑，优先级管理，以及模式切换的约束条件

模式切换，说白了就是飞控的大脑在决定“现在该干什么”。

我刚开始做飞控的时候，觉得切换模式不就是改个参数吗？后来炸了一次机才明白——切换时机不对，飞机就得摔。今天咱们就把这块掰开揉碎了讲。

4.1 状态机：模式切换的骨架

飞控里的模式切换，底层就是一个有限状态机（FSM）。每个模式是一个状态，触发条件满足就跳转。

我个人习惯把状态机分成三层：

• 顶层：飞行模式（手动、定高、悬停、返航等）
• 中间层：子状态（比如悬停里的“位置锁定”和“速度控制”）
• 底层：执行器状态（电机怠速、解锁、急停）

举个例子，一个最简单的悬停模式切换逻辑：

// 伪代码：悬停模式状态机
enum FlightMode {
    MANUAL,
    ALT_HOLD,
    POS_HOLD,
    RTL
};

FlightMode current_mode = MANUAL;

void mode_switch_handler(FlightMode target_mode) {
    // 检查约束条件
    if (!check_constraints(target_mode)) {
        return; // 不满足条件，拒绝切换
    }
    
    // 执行退出当前模式的动作
    exit_current_mode(current_mode);
    
    // 切换到目标模式
    current_mode = target_mode;
    
    // 执行进入新模式的初始化
    enter_new_mode(current_mode);
}

你看，逻辑很直白。但实际工程里，坑全在check_constraints()和exit_current_mode()里。

4.2 优先级管理：谁说了算？

模式之间是有优先级的。你不能在返航途中突然切到手动作动，那会出大事。

我一般把优先级分成三级：

优先级	模式举例	说明
高（不可抢占）	急停、降落、返航	一旦进入，必须完成才能切出
中（可抢占）	悬停、定高、跟随	可以被更高优先级打断
低（自由切换）	手动、特技、自稳	用户随时可以切，但受约束条件限制

举个例子：飞机正在执行返航（高优先级），这时候用户发了个“切手动”指令。怎么办？

我的做法是——忽略。返航逻辑里有个标志位 rtl_in_progress，只要它置位，任何低优先级切换请求都被丢弃，直到飞机落地或者用户通过特殊指令（比如长按急停）强制打断。

4.3 约束条件：不是你想切就能切

模式切换不是随心所欲的。每个模式都有它的前置条件。不满足条件就硬切，轻则飞控报错，重则炸机。

常见的约束条件有这几类：

• GPS状态：悬停、返航、航点飞行必须要有有效的GPS定位（HDOP < 2.0，卫星数 > 8）
• 电量：电量低于20%时，禁止切入手动模式以外的模式（防止低电量大机动导致电压骤降）
• 高度：低于2米时，禁止切入返航模式（防止误触导致飞机突然爬升撞到障碍物）
• 传感器状态：IMU未校准、磁罗盘异常时，禁止切入任何自动模式
• 遥控器信号：RC信号丢失时，强制切入失控保护模式（Failsafe）

我曾经遇到过一个问题：用户在地面切返航，飞机直接原地爬升撞到了树。后来查日志发现，GPS定位误差有5米，飞机以为自己在10米外，结果一爬升就撞了。从那以后，我在约束条件里加了一条：离地高度低于3米时，返航模式必须先爬升到安全高度再水平移动。

4.4 模式切换流程图

下面这张图，是我做飞控时最常用的一张流程图。它把模式切换的完整逻辑串起来了。

这张图的核心逻辑就一句话：先看优先级，再看约束条件，都通过了才执行切换。顺序不能乱。

4.5 实战中的避坑指南

最后，分享几个我踩过的坑：

• 不要在地面切返航：除非你确认GPS精度足够，而且周围没有障碍物。我建议在代码里加一个“离地高度低于2米禁止切返航”的保护。
• 电量低时禁止切手动：低电量下手动大机动，电压瞬间掉到3.3V以下，飞控直接重启。我一般会在电量低于15%时强制切降落模式。
• 传感器异常时锁定当前模式：磁罗盘受到干扰时，不要切到需要航向的模式（比如航点飞行）。保持当前模式，或者切到纯手动。
• RC信号丢失的处理：信号丢失后，不要立即切返航。先等3秒，确认不是短暂干扰，再执行失控保护。我曾经遇到过RC信号闪断导致飞机在悬停和返航之间反复横跳的情况。