iNav开源代码之严重炸机 -- FAILSAFE
1. 源由
最近因为炸机,百思不得其解。
关于炸鸡的过程,就不再展开,都是“泪”啊!想进一步了解的,请参阅前面的初步分析。
为了更清楚的搞清楚到底是哪里的问题,还是要从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任务分析