ardupilot/Ardupilot V4.3.4 源码解析：mode.h

#pragma once

#include <AP_Param/AP_Param.h>

#include <AP_Common/Location.h>

#include <stdint.h>

#include <AP_Soaring/AP_Soaring.h>

#include <AP_ADSB/AP_ADSB.h>

#include <AP_Vehicle/ModeReason.h>

#include "quadplane.h"

class AC_PosControl;

class AC_AttitudeControl_Multi;

class AC_Loiter;

class Mode

{

public:

    /* 不允许拷贝 */

    Mode(const Mode &other) = delete;

    Mode &operator=(const Mode&) = delete;

    // 自动驾驶模式

    // ----------------

    enum Number : uint8_t {

        MANUAL        = 0,

        CIRCLE        = 1,

        STABILIZE     = 2,

        TRAINING      = 3,

        ACRO          = 4,

        FLY_BY_WIRE_A = 5,

        FLY_BY_WIRE_B = 6,

        CRUISE        = 7,

        AUTOTUNE      = 8,

        AUTO          = 10,

        RTL           = 11,

        LOITER        = 12,

        TAKEOFF       = 13,

        AVOID_ADSB    = 14,

        GUIDED        = 15,

        INITIALISING  = 16,

#if HAL_QUADPLANE_ENABLED

        QSTABILIZE    = 17,

        QHOVER        = 18,

        QLOITER       = 19,

        QLAND         = 20,

        QRTL          = 21,

#if QAUTOTUNE_ENABLED

        QAUTOTUNE     = 22,

#endif

        QACRO         = 23,

#endif

        THERMAL       = 24,

#if HAL_QUADPLANE_ENABLED

        LOITER_ALT_QLAND = 25,

#endif

    };

    // 构造函数

    Mode();

    // 进入此模式，始终返回true/success

    bool enter();

    // 执行所需要的任何清理工作。

    void exit();

    // 运行适用于此模式的控制器。

    virtual void run() {};

    // 返回一个与此模式相关的唯一编号.

    virtual Number mode_number() const = 0;

    // 返回完整的文本名称

    virtual const char *name() const = 0;

 

    // 返回此飞行模式的字符串，确切地说是4个字节。

    virtual const char *name4() const = 0;

    // returns true if the vehicle can be armed in this mode

    // 如果该飞行器可以在此模式下解锁，则返回true

    virtual bool allows_arming() const { return true; }

    //

    //子类应重写的方法以影响飞行器在此模式下的运动

    virtual void update() = 0;

    // 将用户输入转换为目标，以实现该模式的高级控制

    // 对所有的旋翼模式返回true

    virtual bool is_vtol_mode() const { return false; }

    virtual bool is_vtol_man_throttle() const;

    virtual bool is_vtol_man_mode() const { return false; }

    // 引导或ADSB模式

    virtual bool is_guided_mode() const { return false; }

    // 如果该模式可以通过开关关闭地形跟踪，则返回true

    virtual bool allows_terrain_disable() const { return false; }

    // 如果支持自动切换到热能模式，则返回true

    virtual bool does_automatic_thermal_switch() const {return false; }

    // 如果子类需要导航，则重写该方法

    virtual void navigate() { return; }

   

    // 单个飞行模式中允许特定的油门值，取决于延迟

    /*这个描述说明了在特定的飞行模式下，控制系统会根据airspeed_nudge_cm（气速修正参数）混合遥控器输入和油门控制。

     在某些飞行模式中，控制系统可以根据当前飞机的空速修正参数，

     对遥控器输入和油门信号进行混合控制。具体的混合方式可能因飞行控制系统的设计和设置而有所不同，

     但通常是为了在实现特定飞行任务时，使飞机保持稳定或达到特定的飞行性能要求。

     例如，在需要维持特定空速的模式下，如果飞机的实际空速与目标空速存在偏差，

     控制系统可以根据airspeed_nudge_cm的值，将修正量与遥控器输入进行混合控制，

     以调整油门位置来修正飞机的空速。这样可以确保飞机在不同气流条件下保持稳定的空速并执行所需的任务。

     需要注意的是，具体的实现细节会因不同飞行控制系统和飞机平台而有所差异。

     这取决于飞行控制器的软件和硬件架构，以及特定飞行模式的定义和要求。*/

    virtual bool allows_throttle_nudging() const { return false; }

    // true if the mode sets the vehicle destination, which controls

    // whether control input is ignored with STICK_MIXING=0

    // 如果模式设置了飞行器目标点，则控制输入在STICK_MIXING=0下被忽略

    /*如果某个飞行模式设置了飞行器的目标位置（即航点），那么STICK_MIXING=0将控制是否忽略控制输入。

    如果STICK_MIXING=0，则控制输入可能会被忽略，因为目标位置可能已经设置，飞行器将按照预定路径自动导航。

    当进入一个需要导航到特定目标的飞行模式时，例如自动驾驶或航点导航模式，

    控制系统可能会忽略来自遥控器的控制输入，因为此时飞行器会自动按照事先设定的航线或航点进行飞行。

    在这种情况下，STICK_MIXING=0的设置会告知控制系统，控制输入不应该被采纳，以免干扰自动导航操作。*/

    virtual bool does_auto_navigation() const { return false; }

 

    // 如果模式设置了飞行器目标点，则控制输入在STICK_MIXING=0下被忽略

    /*当使用STICK_MIXING=0时，如果某个飞行模式设置了航行器的目标位置，则该模式会控制是否忽略控制输入。

    也就是说，在这种情况下，如果飞行模式将航行器的目标位置设定好了，那么控制输入可能会被忽略。

    简而言之，当某个飞行模式设置了航行器的目标位置时，

    这可能意味着飞行器将自动导航到目标位置，并在此期间忽略遥控器的控制输入。*/

    virtual bool does_auto_throttle() const { return false; }

    // method for mode specific target altitude profiles

    // 特定飞行模式下特定目标高度配置文件的方法

    /*实现飞行模式特定的目标高度轮廓的方法可以基于特定的飞行控制系统和硬件平台。下面是一般的方法概述：

      确定飞行模式：首先，需要确定要定义目标高度轮廓的特定飞行模式，例如手动飞行、定高模式或自动驾驶模式等。

      定义高度轮廓：针对每个飞行模式，定义适当的目标高度轮廓。

      例如，手动飞行模式下可能没有特定的高度轮廓，而定高模式下可能需要设定一个固定的目标高度，

      而自动驾驶模式可能涉及到预先规划的航线和航点，每个航点可能有一个固定的目标高度。

      控制算法：开发相应的控制算法来实现目标高度轮廓。

      这可以涉及到气压高度传感器、GPS或雷达测距仪等传感器的使用，以获取飞机的当前高度信息。

      控制算法将根据当前高度与目标高度之间的差异，生成合适的控制指令来控制飞机的爬升或下降。

      控制命令生成：基于控制算法和高度差异，生成适当的控制指令，

      例如油门输出或控制舵面以调整飞机的爬升或下降速率。

      这可以通过PID控制器、状态估计器或其他控制技术来实现。

      控制律执行：将生成的控制指令应用于飞行器的执行机构，

      如电调、电机、舵机等，以实现相应的高度控制操作。*/

    virtual bool update_target_altitude() { return false; }

    // 处理来自GCS(地面控制站)的引导目标请求

    virtual bool handle_guided_request(Location target_loc) { return false; }

protected:

    // subclasses override this to perform checks before entering the mode

    // 在进入模式前，子类按照此方法重写以执行相关的检查

    virtual bool _enter() { return true; }

    // subclasses override this to perform any required cleanup when exiting the mode

    // 在退出模式时，子类按照此方法重写以执行所需的任何清理

    virtual void _exit() { return; }

#if HAL_QUADPLANE_ENABLED

    // 旋翼和固定翼飞机的快捷方式引用

    AC_PosControl*& pos_control;

    AC_AttitudeControl_Multi*& attitude_control;

    AC_Loiter*& loiter_nav;

    QuadPlane& quadplane;

    QuadPlane::PosControlState &poscontrol;

#endif

};

 

class ModeAcro : public Mode

{

public:

    Mode::Number mode_number() const override { return Mode::Number::ACRO; }

    const char *name() const override { return "ACRO"; }

    const char *name4() const override { return "ACRO"; }

    // 在这种模式下，影响飞机运动的方法。

    void update() override;

protected:

    bool _enter() override;

};

class ModeAuto : public Mode

{

public:

    Number mode_number() const override { return Number::AUTO; }

    const char *name() const override { return "AUTO"; }

    const char *name4() const override { return "AUTO"; }

    bool does_automatic_thermal_switch() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_navigation() const override;

    bool does_auto_throttle() const override;

protected:

    bool _enter() override;

    void _exit() override;

};

 

class ModeAutoTune : public Mode

{

public:

    Number mode_number() const override { return Number::AUTOTUNE; }

    const char *name() const override { return "AUTOTUNE"; }

    const char *name4() const override { return "ATUN"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

protected:

    bool _enter() override;

    void _exit() override;

};

class ModeGuided : public Mode

{

public:

    Number mode_number() const override { return Number::GUIDED; }

    const char *name() const override { return "GUIDED"; }

    const char *name4() const override { return "GUID"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    virtual bool is_guided_mode() const override { return true; }

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_navigation() const override { return true; }

    bool does_auto_throttle() const override { return true; }

    // 处理来自GCS的引导目标请求

    bool handle_guided_request(Location target_loc) override;

protected:

    bool _enter() override;

};

class ModeCircle: public Mode

{

public:

    Number mode_number() const override { return Number::CIRCLE; }

    const char *name() const override { return "CIRCLE"; }

    const char *name4() const override { return "CIRC"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    bool does_auto_navigation() const override { return true; }

    bool does_auto_throttle() const override { return true; }

protected:

    bool _enter() override;

};

class ModeLoiter : public Mode

{

public:

    Number mode_number() const override { return Number::LOITER; }

    const char *name() const override { return "LOITER"; }

    const char *name4() const override { return "LOIT"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);

    bool isHeadingLinedUp_cd(const int32_t bearing_cd);

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_navigation() const override { return true; }

    bool does_auto_throttle() const override { return true; }

   

    bool allows_terrain_disable() const override { return true; }

protected:

    bool _enter() override;

};

#if HAL_QUADPLANE_ENABLED

class ModeLoiterAltQLand : public ModeLoiter

{

public:

    Number mode_number() const override { return Number::LOITER_ALT_QLAND; }

    const char *name() const override { return "Loiter to QLAND"; }

    const char *name4() const override { return "L2QL"; }

    // 处理来自GCS的引导目标请求

    bool handle_guided_request(Location target_loc) override;

protected:

    bool _enter() override;

    void navigate() override;

private:

    void switch_qland();

};

#endif // HAL_QUADPLANE_ENABLED

class ModeManual : public Mode

{

public:

    Number mode_number() const override { return Number::MANUAL; }

    const char *name() const override { return "MANUAL"; }

    const char *name4() const override { return "MANU"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

};

 

class ModeRTL : public Mode

{

public:

    Number mode_number() const override { return Number::RTL; }

    const char *name() const override { return "RTL"; }

    const char *name4() const override { return "RTL "; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_navigation() const override { return true; }

    bool does_auto_throttle() const override { return true; }

protected:

    bool _enter() override;

private:

    // 如果启用并且处于半径范围内，则切换到QRTL模式。

    bool switch_QRTL();

};

class ModeStabilize : public Mode

{

public:

    Number mode_number() const override { return Number::STABILIZE; }

    const char *name() const override { return "STABILIZE"; }

    const char *name4() const override { return "STAB"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

};

class ModeTraining : public Mode

{

public:

    Number mode_number() const override { return Number::TRAINING; }

    const char *name() const override { return "TRAINING"; }

    const char *name4() const override { return "TRAN"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

};

class ModeInitializing : public Mode

{

public:

    Number mode_number() const override { return Number::INITIALISING; }

    const char *name() const override { return "INITIALISING"; }

    const char *name4() const override { return "INIT"; }

    bool _enter() override { return false; }

    // 在这种模式下，影响飞机运动的方法

    void update() override { }

    bool allows_arming() const override { return false; }

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_throttle() const override { return true; }

};

class ModeFBWA : public Mode

{

public:

    Number mode_number() const override { return Number::FLY_BY_WIRE_A; }

    const char *name() const override { return "FLY_BY_WIRE_A"; }

    const char *name4() const override { return "FBWA"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

};

class ModeFBWB : public Mode

{

public:

    Number mode_number() const override { return Number::FLY_BY_WIRE_B; }

    const char *name() const override { return "FLY_BY_WIRE_B"; }

    const char *name4() const override { return "FBWB"; }

    bool allows_terrain_disable() const override { return true; }

    bool does_automatic_thermal_switch() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    bool does_auto_throttle() const override { return true; }

protected:

    bool _enter() override;

};

class ModeCruise : public Mode

{

public:

    Number mode_number() const override { return Number::CRUISE; }

    const char *name() const override { return "CRUISE"; }

    const char *name4() const override { return "CRUS"; }

    bool allows_terrain_disable() const override { return true; }

    bool does_automatic_thermal_switch() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    bool get_target_heading_cd(int32_t &target_heading) const;

    bool does_auto_throttle() const override { return true; }

protected:

    bool _enter() override;

    bool locked_heading;

    int32_t locked_heading_cd;

    uint32_t lock_timer_ms;

};

#if HAL_ADSB_ENABLED

class ModeAvoidADSB : public Mode

{

public:

    Number mode_number() const override { return Number::AVOID_ADSB; }

    const char *name() const override { return "AVOID_ADSB"; }

    const char *name4() const override { return "AVOI"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    virtual bool is_guided_mode() const override { return true; }

    bool does_auto_throttle() const override { return true; }

protected:

    bool _enter() override;

};

#endif

#if HAL_QUADPLANE_ENABLED

class ModeQStabilize : public Mode

{

public:

    Number mode_number() const override { return Number::QSTABILIZE; }

    const char *name() const override { return "QSTABILIZE"; }

    const char *name4() const override { return "QSTB"; }

    bool is_vtol_mode() const override { return true; }

    bool is_vtol_man_throttle() const override { return true; }

    virtual bool is_vtol_man_mode() const override { return true; }

    bool allows_throttle_nudging() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    // 用作所有垂起模式的基类

    bool _enter() override;

    void run() override;

protected:

private:

    void set_tailsitter_roll_pitch(const float roll_input, const float pitch_input);

    void set_limited_roll_pitch(const float roll_input, const float pitch_input);

};

class ModeQHover : public Mode

{

public:

    Number mode_number() const override { return Number::QHOVER; }

    const char *name() const override { return "QHOVER"; }

    const char *name4() const override { return "QHOV"; }

    bool is_vtol_mode() const override { return true; }

    virtual bool is_vtol_man_mode() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void run() override;

protected:

    bool _enter() override;

};

class ModeQLoiter : public Mode

{

friend class QuadPlane;

friend class ModeQLand;

public:

    Number mode_number() const override { return Number::QLOITER; }

    const char *name() const override { return "QLOITER"; }

    const char *name4() const override { return "QLOT"; }

    bool is_vtol_mode() const override { return true; }

    virtual bool is_vtol_man_mode() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void run() override;

protected:

    bool _enter() override;

};

class ModeQLand : public Mode

{

public:

    Number mode_number() const override { return Number::QLAND; }

    const char *name() const override { return "QLAND"; }

    const char *name4() const override { return "QLND"; }

    bool is_vtol_mode() const override { return true; }

 

    // 判断是否为垂直起降模式

    void update() override;

    void run() override;

    bool allows_arming() const override { return false; }

protected:

    bool _enter() override;

};

class ModeQRTL : public Mode

{

public:

    Number mode_number() const override { return Number::QRTL; }

    const char *name() const override { return "QRTL"; }

    const char *name4() const override { return "QRTL"; }

    bool is_vtol_mode() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void run() override;

    bool allows_arming() const override { return false; }

    bool does_auto_throttle() const override { return true; }

    bool update_target_altitude() override;

    bool allows_throttle_nudging() const override;

protected:

    bool _enter() override;

private:

    enum class SubMode {

        climb,

        RTL,

    } submode;

};

class ModeQAcro : public Mode

{

public:

    Number mode_number() const override { return Number::QACRO; }

    const char *name() const override { return "QACO"; }

    const char *name4() const override { return "QACRO"; }

    bool is_vtol_mode() const override { return true; }

    bool is_vtol_man_throttle() const override { return true; }

    virtual bool is_vtol_man_mode() const override { return true; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void run() override;

protected:

    bool _enter() override;

};

#if QAUTOTUNE_ENABLED

class ModeQAutotune : public Mode

{

public:

    Number mode_number() const override { return Number::QAUTOTUNE; }

    const char *name() const override { return "QAUTOTUNE"; }

    const char *name4() const override { return "QATN"; }

    bool is_vtol_mode() const override { return true; }

    virtual bool is_vtol_man_mode() const override { return true; }

    void run() override;

    // 在这种模式下，影响飞机运动的方法

    void update() override;

protected:

    bool _enter() override;

    void _exit() override;

};

#endif  // QAUTOTUNE_ENABLED

#endif  // HAL_QUADPLANE_ENABLED

class ModeTakeoff: public Mode

{

public:

    ModeTakeoff();

    Number mode_number() const override { return Number::TAKEOFF; }

    const char *name() const override { return "TAKEOFF"; }

    const char *name4() const override { return "TKOF"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    void navigate() override;

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_navigation() const override { return true; }

    bool does_auto_throttle() const override { return true; }

    // var_info 用以保存参数信息

    static const struct AP_Param::GroupInfo var_info[];

protected:

    AP_Int16 target_alt;

    AP_Int16 target_dist;

    AP_Int16 level_alt;

    AP_Int8 level_pitch;

    bool takeoff_started;

    Location start_loc;

    bool _enter() override;

};

#if HAL_SOARING_ENABLED

class ModeThermal: public Mode

{

public:

    Number mode_number() const override { return Number::THERMAL; }

    const char *name() const override { return "THERMAL"; }

    const char *name4() const override { return "THML"; }

    // 在这种模式下，影响飞机运动的方法

    void update() override;

    // 更新热成像追踪和退出逻辑。

    void update_soaring();

    void navigate() override;

    bool allows_throttle_nudging() const override { return true; }

    bool does_auto_navigation() const override { return true; }

    // 如果处于自动油门模式（auto-throttle mode），

    // 这意味着我们需要运行速度/高度控制器，则返回true。

    bool does_auto_throttle() const override { return true; }

protected:

    bool exit_heading_aligned() const;

    void restore_mode(const char *reason, ModeReason modereason);

    bool _enter() override;

};

#endif