iNav开源代码之严重炸机 -- FAILSAFE

lida2003

已于 2023-07-22 06:33:54 修改

阅读量345

点赞数

分类专栏： xFlight 文章标签： iNav

于 2023-07-16 21:04:51 首次发布

本文链接：https://blog.csdn.net/lida2003/article/details/131754780

版权

xFlight 专栏收录该内容

102 篇文章 107 订阅

订阅专栏

iNav开源代码之严重炸机 -- FAILSAFE

1. 源由
2. `FAILSAFE`类别
3. `FAILSAFE`配置
4. `FAILSAFE`阶段&状态机
5. 假设分析
6. 参考资料

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`阶段&状态机

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;

正常情况下，系统处在FAILSAFE_IDLE状态，INAV只有在RC链路出现断链的情况才会触发FAILSAFE，这个从另外一个侧面证明了该固件是航模，而非无人机，因为RC控制是最直接的航模控制方式。

当RC链路断链，进入FAILSAFE_RX_LOSS_DETECTED后，测试Inav Configurator配置的三种FAILSAFE方式：

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

在这里插入图片描述
本次炸机的情况发生在RC链路断链后，FAILSAFE_PROCEDURE_RTH触发FAILSAFE_RETURN_TO_HOME，GPS信号丢失从而进入FAILSAFE_LANDING阶段，此时发生了坠落。

因此，这里最为纠结的问题是到底Inav是如何处理Emergency Landing的。

目前这块代码尚未进行研读，但是从资料以及反馈信息可以看出，最早的设计是使用failsafe_throttle在有限的failsafe_off_delay时间段内进行降落。

目前，从设计上看，有baro高度、nav_mc_hover_thr等有效数据，airmode模式的IMU姿态保持方法，是否已经有更新、可靠、安全的Emergency Landig有待明确。

注：有时间，后续对这部分代码进行研读，以便了解真实原因。如有兴趣的朋友可以继续关注What will happen, when RC & GPS signal lost? – inav 6.1.1 #9167。

5. 假设分析

如果按照最早的Emergency Landing方式考虑，由于笔者使用的是默认值，failsafe_throttle = 1000，会出现一些如文档中所述的意外情况。

笔者认为如果上述情况坠落，大概率是"prop stall"，也就是所谓的“失速坠落”。没听错哦，螺旋桨失速会导致多轴飞机失去平衡，最终导致坠落。

注：当然这里是假设的分析，毕竟这个文档是有点过期的，最终还是要看代码。

在这里插入图片描述
相关描述链接：setting-up-failsafe-with-return-to-home

6. 参考资料

【1】iNav开源代码之严重炸机 – 危险隐患
【2】BetaFlight模块设计之二十八：MainPidLoop任务分析

lida2003

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
iNav开源代码之严重炸机 -- FAILSAFE

关于炸鸡的过程，就不再展开，都是“泪”啊！想进一步了解的，请参阅前面的初步分析。以下类别与Configurator中的Drop/Land/RTH一一对应，其中。是一个默认的状态，仅程序使用，GUI界面没有相关配置选项。的类别，配置，阶段，以及状态机来看看是怎么个过程。为了更清楚的搞清楚到底是哪里的问题，还是要从。在编译时，在目标板目录。最近因为炸机，百思不得其解。模式下代码逻辑是如何发生的。的工作原理来分析，在相应。
复制链接

扫一扫