4、自定义模式框架搭建：创建新的模式类、继承AC_Mode基类、注册模式到模式管理器

好，咱们今天来干点实在的。

前面几章我们把ArduPilot的模式体系讲了个大概，从顶层设计到调度流程都过了一遍。但说实话，光看不练是学不会的。这一章，我带你亲手搭建一个自定义模式的框架。

说白了，就是三件事：写一个类、继承基类、注册进去。听起来简单？嗯，细节里全是坑。

4.1 先搞清楚我们要继承谁

在ArduPilot里，所有飞行模式都继承自 AC_Mode 这个基类。你打开 libraries/AC_Mode/ 目录，就能看到它的真身。

这个基类干了什么？它定义了模式的生命周期：

• 初始化：进入模式时调用一次
• 运行：主循环里每帧调用
• 退出：离开模式时清理资源

我个人习惯，在写新模式之前，先看一眼基类的头文件。你不需要记住所有虚函数，但至少要搞清楚哪些是必须重写的，哪些是可选的。

4.2 创建你的第一个模式类

假设我们要做一个「悬停锁定」模式——飞机进入这个模式后，不管你怎么推杆，它都死守当前位置。听起来很酷？其实代码量不大。

新建一个头文件，比如 AC_Mode_HoverLock.h：

#pragma once

#include "AC_Mode.h"

class AC_Mode_HoverLock : public AC_Mode
{
public:
    // 构造函数，传入模式编号
    AC_Mode_HoverLock() : AC_Mode(Mode::HOVER_LOCK) {}

    // 必须重写：模式运行逻辑
    void run() override;

    // 可选重写：进入模式时的初始化
    void init() override;

    // 可选重写：退出模式时的清理
    void exit() override;

private:
    float _target_altitude;   // 目标高度
    Vector3f _target_position; // 目标位置
};

你看，结构很清晰。构造函数里传入了模式编号 Mode::HOVER_LOCK，这个编号我们后面要在枚举里定义。

然后是实现文件 AC_Mode_HoverLock.cpp：

#include "AC_Mode_HoverLock.h"

void AC_Mode_HoverLock::init()
{
    // 记录进入模式时的位置和高度
    _target_position = AP::ahrs().get_position();
    _target_altitude = AP::baro().get_altitude();

    // 设置位置控制器目标
    pos_control->set_pos_target(_target_position);
    pos_control->set_alt_target(_target_altitude);
}

void AC_Mode_HoverLock::run()
{
    // 每帧都重新锁定目标位置
    pos_control->set_pos_target(_target_position);
    pos_control->set_alt_target(_target_altitude);

    // 调用位置控制器
    pos_control->update();
}

void AC_Mode_HoverLock::exit()
{
    // 退出时，把控制权交还给位置控制器
    pos_control->clear_target();
}

嗯，这里要注意：run() 函数是每帧调用的，频率很高。所以里面不要做耗时操作，比如串口打印、文件读写。我曾经见过有人在这里面写了个 delay()，结果飞机直接炸了……

4.3 注册到模式管理器

类写好了，但ArduPilot还不知道它的存在。我们需要把它注册到模式管理器里。

模式管理器在 AP_Modes.cpp 里，它维护了一个模式对象数组。打开这个文件，找到类似这样的地方：

// 模式对象数组
AP_Mode *mode_table[] = {
    &mode_stabilize,
    &mode_althold,
    &mode_loiter,
    // ... 其他模式
};

我们需要在这里加上我们的新模式：

// 在文件顶部声明
AC_Mode_HoverLock mode_hoverlock;

// 在模式数组里注册
AP_Mode *mode_table[] = {
    &mode_stabilize,
    &mode_althold,
    &mode_loiter,
    &mode_hoverlock,   // 新注册的模式
    // ... 其他模式
};

然后，还要在模式枚举 Mode 里加上 HOVER_LOCK：

enum class Mode : uint8_t {
    STABILIZE = 0,
    ACRO = 1,
    ALT_HOLD = 2,
    AUTO = 3,
    LOITER = 4,
    HOVER_LOCK = 5,   // 新加的模式编号
    // ...
};

编号不能重复，这是常识。我建议你从 5 开始往后排，避开已有的模式编号。

4.4 框架搭建的完整流程

来，我画个图帮你理清思路。整个流程其实就三步：

你看，逻辑上就这么简单。但实际做的时候，有几个地方容易翻车。

4.5 常见翻车现场

我总结几个我踩过的坑，你注意避开：

1. 枚举编号冲突：你加了个 HOVER_LOCK = 5，但别人已经用了 5。编译不会报错，但运行时模式切换会乱套。建议先 grep 一下整个工程，确认编号没被占用。
2. 忘记加头文件：在 AP_Modes.cpp 里用了 AC_Mode_HoverLock，但没 include 头文件。编译报错是小事，关键是定位起来费时间。
3. 模式对象声明位置不对：模式对象必须是全局的，或者静态的。不能放在函数里局部声明，否则模式管理器访问不到。

4.6 验证你的模式是否生效

编译通过后，怎么知道模式注册成功了？最简单的方法：

• 用 Mission Planner 连接飞控
• 在「飞行模式」设置页面，看看下拉菜单里有没有 HOVER_LOCK
• 如果有，说明注册成功了
• 如果没有，检查枚举编号和数组注册

我曾经有一次编译了半小时，刷进去发现模式没出现。最后发现是枚举编号写成了 5，但数组里注册的是 6。这种低级错误，排查起来最头疼。

好了，框架搭建就讲到这里。你按照这个流程走一遍，应该能跑起来。下一章我们往这个空壳子里填真正的控制逻辑——让飞机真的悬停住。

---