1. 源由

最近因为炸机，百思不得其解。

关于炸鸡的过程，就不再展开，都是“泪”啊！想进一步了解的，请参阅前面的初步分析。

链接：iNav开源代码之严重炸机 – 危险隐患。

为了更清楚的搞清楚到底是哪里的问题，还是要从FAILSAFE的工作原理来分析，在相应FAILSAFE模式下代码逻辑是如何发生的。

先从FAILSAFE的类别，配置，阶段，以及状态机来看看是怎么个过程。

2. `FAILSAFE`类别

以下类别与Configurator中的Drop/Land/RTH一一对应，其中FAILSAFE_PROCEDURE_NONE是一个默认的状态，仅程序使用，GUI界面没有相关配置选项。

typedef enum {
    FAILSAFE_PROCEDURE_AUTO_LANDING = 0,
    FAILSAFE_PROCEDURE_DROP_IT,
    FAILSAFE_PROCEDURE_RTH,
    FAILSAFE_PROCEDURE_NONE
} failsafeProcedure_e;

3. `FAILSAFE`配置

配置参数默认在fc/settings.yaml文件中定义。在编译时，在目标板目录inav\build\src\main\target\target_name\target_name下生成settings_generated.c/h文件。

主要的配置参数如下所示：

typedef struct failsafeConfig_s {
    uint16_t failsafe_throttle_low_delay;       // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
    uint8_t failsafe_delay;                     // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
    uint8_t failsafe_recovery_delay;            // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)
    uint8_t failsafe_off_delay;                 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
    uint8_t failsafe_procedure;                 // selected full failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)

    int16_t failsafe_fw_roll_angle;             // Settings to be applies during "LAND" procedure on a fixed-wing
    int16_t failsafe_fw_pitch_angle;
    int16_t failsafe_fw_yaw_rate;
    uint16_t failsafe_stick_motion_threshold;
    uint16_t failsafe_min_distance;             // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
    uint8_t failsafe_min_distance_procedure;    // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
    int16_t failsafe_mission_delay;             // Time delay before Failsafe triggered when WP mission in progress (s)
} failsafeConfig_t;

当前代码定义为：

SETTING_FAILSAFE_DELAY_DEFAULT // 0.5 sec
SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay)
SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec
SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // 0, default throttle low delay for “just disarm” on failsafe condition
SETTING_FAILSAFE_PROCEDURE_DEFAULT, // 0, default full failsafe procedure
SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left
SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT, // 10 deg dive (yes, positive means dive)
SETTING_FAILSAFE_FW_YAW_RATE_DEFAULT, // 45 deg/s left yaw (left is negative, 8s for full turn)
SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT, // 50
SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // 0, No minimum distance for failsafe by default
SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // 1, default minimum distance failsafe procedure
SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // 0, Time delay before Failsafe activated during WP mission (s)


#define SETTING_FAILSAFE_DELAY_DEFAULT 5
#define SETTING_FAILSAFE_DELAY 88
#define SETTING_FAILSAFE_DELAY_MIN 0
#define SETTING_FAILSAFE_DELAY_MAX 200
#define SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT 5
#define SETTING_FAILSAFE_RECOVERY_DELAY 89
#define SETTING_FAILSAFE_RECOVERY_DELAY_MIN 0
#define SETTING_FAILSAFE_RECOVERY_DELAY_MAX 200
#define SETTING_FAILSAFE_OFF_DELAY_DEFAULT 200
#define SETTING_FAILSAFE_OFF_DELAY 90
#define SETTING_FAILSAFE_OFF_DELAY_MIN 0
#define SETTING_FAILSAFE_OFF_DELAY_MAX 200
#define SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT 0
#define SETTING_FAILSAFE_THROTTLE_LOW_DELAY 91
#define SETTING_FAILSAFE_THROTTLE_LOW_DELAY_MIN 0
#define SETTING_FAILSAFE_THROTTLE_LOW_DELAY_MAX 300
#define SETTING_FAILSAFE_PROCEDURE_DEFAULT 0
#define SETTING_FAILSAFE_PROCEDURE 92
#define SETTING_FAILSAFE_PROCEDURE_MIN 0
#define SETTING_FAILSAFE_PROCEDURE_MAX 0
#define SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT 50
#define SETTING_FAILSAFE_STICK_THRESHOLD 93
#define SETTING_FAILSAFE_STICK_THRESHOLD_MIN 0
#define SETTING_FAILSAFE_STICK_THRESHOLD_MAX 500
#define SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT -200
#define SETTING_FAILSAFE_FW_ROLL_ANGLE 94
#define SETTING_FAILSAFE_FW_ROLL_ANGLE_MIN -800
#define SETTING_FAILSAFE_FW_ROLL_ANGLE_MAX 800
#define SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT 100
#define SETTING_FAILSAFE_FW_PITCH_ANGLE 95
#define SETTING_FAILSAFE_FW_PITCH_ANGLE_MIN -800
#define SETTING_FAILSAFE_FW_PITCH_ANGLE_MAX 800
#define SETTING_FAILSAFE_FW_YAW_RATE_DEFAULT -45
#define SETTING_FAILSAFE_FW_YAW_RATE 96
#define SETTING_FAILSAFE_FW_YAW_RATE_MIN -1000
#define SETTING_FAILSAFE_FW_YAW_RATE_MAX 1000
#define SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT 0
#define SETTING_FAILSAFE_MIN_DISTANCE 97
#define SETTING_FAILSAFE_MIN_DISTANCE_MIN 0
#define SETTING_FAILSAFE_MIN_DISTANCE_MAX 65000
#define SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT 1
#define SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE 98
#define SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_MIN 0
#define SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_MAX 0
#define SETTING_FAILSAFE_MISSION_DELAY_DEFAULT 0
#define SETTING_FAILSAFE_MISSION_DELAY 99
#define SETTING_FAILSAFE_MISSION_DELAY_MIN -1
#define SETTING_FAILSAFE_MISSION_DELAY_MAX 600

static failsafeState_t failsafeState;

PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 3);

PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
    .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT,                                   // 0.5 sec
    .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT,                 // 0.5 seconds (plus 200ms explicit delay)
    .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT,                           // 20sec
    .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT,         // default throttle low delay for "just disarm" on failsafe condition
    .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT,                           // default full failsafe procedure
    .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT,                   // 20 deg left
    .failsafe_fw_pitch_angle = SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT,                 // 10 deg dive (yes, positive means dive)
    .failsafe_fw_yaw_rate = SETTING_FAILSAFE_FW_YAW_RATE_DEFAULT,                       // 45 deg/s left yaw (left is negative, 8s for full turn)
    .failsafe_stick_motion_threshold = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT,
    .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
    .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
    .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe activated during WP mission (s)
);

4. `FAILSAFE`阶段&状态机

FAILSAFE_PROCEDURE_AUTO_LANDING：触发FAILSAFE_LANDING
FAILSAFE_PROCEDURE_DROP_IT：触发FAILSAFE_LANDED
FAILSAFE_PROCEDURE_RTH：触发FAILSAFE_RETURN_TO_HOME

typedef enum {
    FAILSAFE_IDLE = 0,
    /* Failsafe mode is not active. All other
     * phases indicate that the failsafe flight
     * mode is active.
     */
    FAILSAFE_RX_LOSS_DETECTED,
    /* In this phase, the connection from the receiver
     * has been confirmed as lost and it will either
     * transition into FAILSAFE_RX_LOSS_RECOVERED if the
     * RX link is recovered immediately or one of the
     * recovery phases otherwise (as configured via
     * failsafe_procedure) or into FAILSAFE_RX_LOSS_IDLE
     * if failsafe_procedure is NONE.
     */
    FAILSAFE_RX_LOSS_IDLE,
    /* This phase will just do nothing else than wait
     * until the RX connection is re-established and the
     * sticks are moved more than the failsafe_stick_threshold
     * settings and then transition to FAILSAFE_RX_LOSS_RECOVERED.
     * Note that this phase is only used when
     * failsafe_procedure = NONE.
     */
    FAILSAFE_RETURN_TO_HOME,
    /* Failsafe is executing RTH. This phase is the first one
     * enabled when failsafe_procedure = RTH if an RTH is
     * deemed possible (RTH might not be activated if e.g.
     * a HOME position was not recorded or some required
     * sensors are not working at the moment). If RTH can't
     * be started, this phase will transition to FAILSAFE_LANDING.
     */
    FAILSAFE_LANDING,
    /* Performs NAV Emergency Landing using controlled descent rate if
     * altitude sensors available.
     * Otherwise Emergency Landing performs a simplified landing procedure.
     * This is done by setting throttle and roll/pitch/yaw controls
     * to a pre-configured values that will allow aircraft
     * to reach ground in somewhat safe "controlled crash" way.
     * This is the first recovery phase enabled when
     * failsafe_procedure = LAND. Once timeout expires or if a
     * "controlled crash" can't be executed, this phase will
     * transition to FAILSAFE_LANDED.
     */
    FAILSAFE_LANDED,
    /* Failsafe has either detected that the model has landed and disabled
     * the motors or either decided to drop the model because it couldn't
     * perform an emergency landing. It will disarm, prevent re-arming
     * and transition into FAILSAFE_RX_LOSS_MONITORING immediately. This is
     * the first recovery phase enabled when failsafe_procedure = DROP.
     */
    FAILSAFE_RX_LOSS_MONITORING,
    /* This phase will wait until the RX connection is
     * working for some time and if and only if switch arming
     * is used and the switch is in the unarmed position
     * will allow rearming again.
     */
    FAILSAFE_RX_LOSS_RECOVERED
    /* This phase indicates that the RX link has been re-established and
     * it will immediately transition out of failsafe mode (phase will
     * transition to FAILSAFE_IDLE.)
     */
} failsafePhase_e;

在这里插入图片描述