iNav飞控之FAILSAFE机制
- 1. 源由
- 2. 设计
- 2.1 触发场景
- 2.1.1 上锁时触发
- 2.1.2 解锁时触发
- 2.2 FAILSAFE策略
- 2.2.1 DROP
- 2.2.2 LAND
- 2.2.3 SET-THR
- 2.2.4 RTH
- 2.2.5 NONE
- 2.3 异常场景
- 2.3.1 救援上锁
- 2.3.2 救援后解锁
- 2.3.3 `FAILSAFE`地面预判
- 2.3.4 RTH丢失定位
- 2.3.5 RC链路恢复
- 3. 重要参数
- 3.1 `failsafe`相关参数
- 3.2 `RTH`相关参数
- 3.3 `LAND/SET-THR`相关参数
- 4. 状态机 & 触发流程
- 4.1 事件状态机
- 4.2 触发流程
- 4.3 `FAILSAFE`状态机
- 5. 总结
- 6. 参考资料
1. 源由
之前对航模飞控之FAILSAFE机制做了一个简单的探讨，并根据自己的理解做了简单的需求整理。
为了更好的了解现行iNav开源飞控对上述情况的处理方法，以及采用何种策略进行救援，能将损失减到最小。
接下来一起来看下iNav的设计逻辑。
注：以下内容主要来自Wiki文档以及部分代码的理解，因个人的理解，以及时间的变化，可能实际代码会有所变更。如果发现有出入的地方，也请小伙伴们能评论给出大家的意见和建议，谢谢！
2. 设计
在逻辑上分为几个步骤：
- Step 1：signal validation：触发源验证
- Step 2：failsafe stage 1
- Step 3：failsafe stage 2
注1：某些FAILSAFE策略执行过程呈现时间段性质，中途可能存在变数，就会出现不同的异常情况。
注2：笔者之前遇到的就是在RTL时，出现了GPS信号丢失，导致失控坠落的问题。
从软件设计角度，实现功能还不是最难的，而最为可贵的是对异常场景的分析，以及软件应对的方法。这就对设计者提出更高的要求。
现在的神经网络、AI算法等，其实更多是去解决多维度、多因素情况下的解决方案。从另一个层面，也可以看出对于这种问题的建模凸显了研发人员对于业务的抽象理解能力。这也就是为什么很多研发人员只会用已有的模型，无法通过模型调优或者重新建模来解决实际问题，因为这里最为关键的一环是业务模型的理解和抽象。
在讨论设计之前，这里根据iNav Failsafe文档，首先澄清一些场景和概念。
2.1 触发场景
鉴于iNav是FPV飞控开源软件，其作为第一人称视角的飞行控制，主要控制设备是遥控器。
Any flight channel (pitch, roll, throttle or yaw) sends no pulses
Any channel is outside the valid range between rx_min_usec and rx_max_usec
The FAILSAFE aux mode is activated
- 遥控开关触发
- 遥控信号丢失
注：当接收机故障，输出信号和正常遥控器连接时输出信号一样的时候，飞控将无法感知遥控信号丢失的场景。
2.1.1 上锁时触发
当飞控处于上锁状态时，如果触发了FAILSAFE，此时该状态将会阻止解锁。
注：这是一个安全措施，因为近距离的时候，RC有的时候确实存在闪断的情况，如果此时触发，有可能出现飞机自动起飞的情况。
2.1.2 解锁时触发
当飞控处于解锁状态时，如果触发了FAILSAFE，此时该状态将会依据failsafe_procedure的配置进行相应的流程处理。
在解锁状态下，通常有两种情况：
- 遥控开关触发
The FAILSAFE aux mode is activated
- 遥控信号丢失
Any flight channel (pitch, roll, throttle or yaw) sends no pulses
Any channel is outside the valid range between rx_min_usec and rx_max_usec
2.2 FAILSAFE策略
通过文档研读，目前有五种策略：
- DROP
- LAND (replaces SET-THR from INAV 4.0)
- SET-THR (Pre INAV 4.0)
- RTH (Return To Home)
- NONE
2.2.1 DROP
Just kill the motors and disarm (crash the craft).
简单的关闭动力，对于多旋翼来说，那就是灾难，直接会从天上掉下来，而固定翼可以滑翔一段距离。
2.2.2 LAND
Performs an Emergency Landing.
- Enables auto-level mode (for multirotor) or
- enters a preconfigured roll/pitch/yaw spiral down (for airplanes).
固定翼和多旋翼的降落方式存在一定差异，多旋翼垂直降落；而固定翼是螺旋高度下降。
Other than using altitude sensors for an actively controlled descent it doesn’t require any extra sensors other than basic gyros and accelerometers.
上述降落方式，通常需要IMU和baro两个传感数据，通常也是飞控板上的元器件工作正常即可(甚至baro的数据可以不用)。
If altitude sensors are working an actively controlled descent is performed using the Emergency Landing descent speed (
nav_emerg_landing_speed).
高度有效的情况下，以给定下降率的方式下降着陆。
If altitude sensors are unavailable descent is performed with the throttle set to a predefined value (
failsafe_throttle).
The descent can be limited to a predefined time (failsafe_off_delay) after which the craft disarms.
This is meant to get the craft to a safe-ish landing (or more realistically, a controlled crash).
高度无效的情况下，以给定油门的情况下，通过给定时间的设定进行盲降或者可控坠落。
注：此时如果高度过高，在给定时间无法接地，将会在时间片用完的情况下，上锁，失去动力，按照DROP方式硬着陆。
2.2.3 SET-THR
该模式就是iNav4.0之前采用的盲降方式，参考2.2.2内容。
The SET-THR procedure doesn’t control descent in any way other than setting a fixed throttle.
This is also the case for the LAND procedure when altitude sensors are unavailable.
Thus, appropriate testing must be performed to find the right throttle value.
Consider that a constant throttle setting will yield different thrusts depending on battery voltage,
so when you evaluate the throttle value do it with a loaded battery. Failure to do so may cause a flyaway.
要特别注意的是对failsafe_throttle设置，至少需考虑如下因素：
- 电池电压状态(初始时动力强劲，耗尽时动力减弱)
- 负载情况(实际使用和测试需考虑测试是否有负载，比如：GoPro)
如果测试不充分，可能会导致该油门飞机不会下降反而上升，此时飞机就称为"窜天猴"了。
2.2.4 RTH
One of the key features of INAV, it automatically navigates the craft back to the home position and (optionally) lands it.
Similarly to all other automated navigation methods, it requires GPS for any type of craft, plus compass and barometer for multicopters.
这种巡航返回起飞点的模式需要比较多的传感器件：
- GPS
- compass
- barometer
- IMU
2.2.5 NONE
Do nothing. This is meant to let the craft perform a fully automated flight (eg. waypoint flight) outside of radio range. Highly unsafe when used with manual flight.
这个手动控制的情况下是比较可怕的，但是在巡航模式倒是一种选择，即使遥控失联的情况下，飞机会继续预先设定的巡航路线完成巡航任务。
2.3 异常场景
2.3.1 救援上锁
- DROP 自动上锁
- LAND/SET-THR after
failsafe_off_delay - RTH with
nav_disarm_on_landingON
2.3.2 救援后解锁
re-arming will be locked until the signal from the receiver is restored for 30 seconds AND the arming switch is in the OFF position (when an arm switch is in use).
- 当救援上锁后，在获得RC控制权前，需要阻止上锁。
- 在RC获取稳定控制权后，锁控制开关必须首先切到OFF状态，才能解锁。
注：上述动作都是为了避免意外情况发生。
2.3.3 FAILSAFE地面预判
Prior to starting failsafe it is checked if the throttle position has been below
min_throttlefor the lastfailsafe_throttle_low_delayseconds.
If it was, the craft is assumed to be on the ground and is simply disarmed.
这个地面预判能够起到一定作用，比如：飞机尚未起飞，但是由于某种原因，遥控突然断链，飞机触发Failsafe，此时如果知道飞机在地面尚未起飞，那么就可以直接上锁。
This feature can be disabled completely by setting
failsafe_throttle_low_delayto zero, which may be necessary to do if the craft may fly long with zero throttle (eg. gliders).
实际情况并非那么理想，不推油门不见得就是在地面上，比如：固定翼的滑翔机。
2.3.4 RTH丢失定位
If any required navigation sensor becomes unavailable during a Return to Home (eg. loss of GPS fix), an emergency landing, as used by the LAND procedure, will be performed after a short timeout.
An emergency landing would also be performed right when the failsafe is triggered if any required sensor is reported as unavailable.
如果RTH过程中，遇到GPS信号丢失或者失去定位信息，此时将会触发紧急降落。
这个就是笔者遇到的炸机情况，iNav开源代码之严重炸机 – 紧急降落。
2.3.5 RC链路恢复
When the failsafe mode is aborted (RC signal restored/failsafe switch set to OFF), the current stick positions will be enforced immediately. Be ready to react quickly.
当RC链路恢复后，取消failsafe模式，就会进入当前遥控杆位操作，此时需要马上相应遥控操作。
3. 重要参数
针对一些参数做了一些简单说明，请注意，这里主要针对多旋翼的情况，因为有些默认参数已经给多旋翼定义了很好的默认配置。
因此这里简单分三大类：
- 【必要参数】务必确认或调整的参数
- 【值得关注】根据飞行场景，可能会遇到问题的参数
- 常规默认参数，暂不展开，但并非不重要
3.1 failsafe相关参数
- 【必要参数】
failsafe_procedure
Selects the failsafe procedure. Valid procedures are DROP, LAND/SET-THR, RTH and NONE. See above for a description of each one.
配置救援所需的功能：DROP, LAND/SET-THR, RTH and NONE
failsafe_delay
Guard time for failsafe activation when rx channel data is lost or invalid.
This is the amount of time the flight controller waits to see if it begins receiving a valid signal again before activating failsafe.
Note: Does not apply when activating the FAILSAFE aux mode.
Stage 1阶段持续的时间
failsafe_recovery_delay
Guard time for failsafe de-activation after signal is recovered.
This is the amount of time the flight controller waits to see if the signal is consistent before turning off failsafe procedure.
Usefull to avoid swithing in and out of failsafe RTH.
Note: Does not apply when disactivating the FAILSAFE aux mode.
信号稳定恢复监测时间
failsafe_stick_threshold
This parameter defines recovery from failsafe by stick motion. When set to zero failsafe procedure will be cleared as soon as RC link is recovered.
When this is set to a non-zero value - failsafe won’t clear immediately when if RC link is recovered,
you will have to move any of Roll/Pitch/Yaw sticks more than this value to exit failsafe.
信号恢复激活遥控链路的门限值
failsafe_throttle_low_delay
Time throttle level must have been below ‘min_throttle’ to only disarm instead of full failsafe procedure. Set to zero to disable.
低油门持续时间，判定是否是接地状态。注意：滑翔机需要disable这个功能，因为滑翔机可能长时间低油门的情况。
failsafe_min_distance
If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure,
but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken.
最短距离定义，将会采用最短距离内的Stage 2特殊的FAILSAFE策略
failsafe_min_distance_procedure
What failsafe procedure to initiate in Stage 2 when craft is closer to home than
failsafe_min_distance.
配置最短距离内Failsafe策略
rx_min_usec
The lowest channel value considered valid.
rx_max_usec
The highest channel value considered valid.
3.2 RTH相关参数
nav_min_rth_distance
If the failsafe happens while the craft is within this distance from the home position, the home position is considered immediately reached.
HOME位置的允许误差范围，在该范围内触发，直接降落。
- 【值得关注】
nav_rth_climb_first
If ON the craft rises to
nav_rth_altitudebefore heading to home position. if OFF the craft climbs on the way to home position.
返回前是先爬升到指定高度，还是返回途中慢慢爬升到指定高度。
- 【值得关注】
nav_rth_climb_ignore_emerg
When this option is OFF (default) and when you initiate RTH without GPS fix - aircraft will initiate emergency descent and go down.
If you set this option to ON - aircraft will reach the RTH target altitude before initiating emergency descent.
This is done for cases where GPS coverage is poor (i.e. in the mountains) -
allowing UAV to climb up might improve GPS coverage and allow safe return instead of landing in a place where UAV might be impossible to recover.
避免GPS信号覆盖问题导致炸机，爬升到更高的高度然后再行降落。
nav_rth_tail_first
Only relevant for multirotors. If this is OFF the craft will head to home position head first, if ON it’ll be tail first
返航机头指向设置(仅仅对多旋翼有效)
nav_rth_altitude
The altitude used as reference for the RTH procedure.
返航高度
nav_rth_alt_mode
How and when to reach
nav_rth_altitude
详见：Navigation modes
nav_rth_abort_threshold
If the craft increases its distance from the point the failsafe was triggered first by this amount,
RTH procedure is aborted and an emergency landing is initiated.
It’s meant to avoid flyaways due to navigation issues, like strong winds.
避免导航错误，导致不是飞回来，而是飞远离。
- 【必要参数】
nav_rth_allow_landing
Enables landing when home position is reached. If OFF the craft will hover indefinitely over the home position.
是否使能RTH后直接降落，还是悬停在上方。
- 【必要参数】
nav_disarm_on_landing
Instructs the flight controller to disarm the craft when landing is detected
接地后，是否disarm标志。
3.3 LAND/SET-THR相关参数
- 【必要参数】
failsafe_off_delay
Delay after failsafe activates before motors finally turn off. This is the amount of time
failsafe_throttleis active.
If you fly at higher altitudes you may need more time to descend safely. Set to zero to keepfailsafe_throttleactive indefinitely.
使用failsafe_throttle持续时间，随后disarm。
nav_emerg_landing_speed
(LAND only) Actively controlled descent speed when altitude sensors are available.
If altitude sensors aren’t available landing descent falls back to using the fixed thottle settingfailsafe_throttleso ensure this is also set correctly.
当高度传感器有效时，紧急降落下降率。
- 【必要参数】
failsafe_throttle
Throttle level used for landing. Specify a value that causes the aircraft to descend at about 1M/sec. Default is set to 1000 which should correspond to throttle off.
SET-THR默认油门值1000，相当于没有油门输出。如果airmode没有使能情况下，飞机姿态无法维持。
- 【值得关注】
failsafe_fw_roll_angle
This parameter defines the amount of roll angle (in 1/10 deg units) to execute on failsafe. Negative = LEFT
- 【值得关注】
failsafe_fw_pitch_angle
This parameter defines the amount of pitch angle (in 1/10 deg units) to execute on failsafe for an airplane. Negative = CLIMB
failsafe_fw_yaw_rate
This parameter defines the amount of yaw rate (in deg per second units) to execute on failsafe for an airplane. Negative = LEFT
4. 状态机 & 触发流程
4.1 事件状态机
iNav采用事件的方式来处理状态的变迁，其中与Failsafe相关的主要是以下几个状态：
-
NAV_STATE_IDLE
-
NAV_STATE_RTH_INITIALIZE
-
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
-
NAV_STATE_RTH_TRACKBACK
-
NAV_STATE_RTH_HEAD_HOME
-
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
-
NAV_STATE_RTH_HOVER_ABOVE_HOME
-
NAV_STATE_RTH_LANDING
-
NAV_STATE_RTH_FINISHING
-
NAV_STATE_RTH_FINISHED
-
NAV_STATE_EMERGENCY_LANDING_INITIALIZE
-
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS
-
NAV_STATE_EMERGENCY_LANDING_FINISHED
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
/** Idle state ******************************************************/
[NAV_STATE_IDLE] = {
.persistentId = NAV_PERSISTENT_ID_IDLE,
.onEntry = navOnEnteringState_NAV_STATE_IDLE,
.timeoutMs = 0,
.stateFlags = 0,
.mapToFlightModes = 0,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
/** ALTHOLD mode ***************************************************/
[NAV_STATE_ALTHOLD_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT,
.mapToFlightModes = NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_ALTHOLD_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT | NAV_RC_ALT,
.mapToFlightModes = NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
/** POSHOLD_3D mode ************************************************/
[NAV_STATE_POSHOLD_3D_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_INFINIT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_POSHOLD_3D_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_INFINIT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
/** CRUISE_HOLD mode ************************************************/
[NAV_STATE_COURSE_HOLD_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_COURSE_HOLD_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_COURSE_HOLD_ADJUSTING,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_COURSE_HOLD_ADJUSTING] = {
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING,
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING,
.timeoutMs = 10,
.stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS,
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_ADJUSTING,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
/** CRUISE_3D mode ************************************************/
[NAV_STATE_CRUISE_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_CRUISE_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_CRUISE_ADJUSTING,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_CRUISE_ADJUSTING] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_ADJUSTING,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
/** RTH_3D mode ************************************************/
[NAV_STATE_RTH_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_RTH_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_START,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK] = NAV_STATE_RTH_TRACKBACK,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_RTH_CLIMB_TO_SAFE_ALT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
.onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_CLIMB,
.mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_RTH_TRACKBACK] = {
.persistentId = NAV_PERSISTENT_ID_RTH_TRACKBACK,
.onEntry = navOnEnteringState_NAV_STATE_RTH_TRACKBACK,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_ENROUTE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_TRACKBACK, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_RTH_HEAD_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_ENROUTE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
.timeoutMs = 500,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_SETTLE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_RTH_LANDING] = {
.persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_RTH_FINISHING] = {
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_RTH_FINISHED] = {
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
/** WAYPOINT mode ************************************************/
[NAV_STATE_WAYPOINT_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
}
},
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
}
},
[NAV_STATE_WAYPOINT_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_WP_ENROUTE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_WAYPOINT_REACHED] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_REACHED,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_REACHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_WAYPOINT_HOLD_TIME] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_TIMED,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_WAYPOINT_RTH_LAND] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
[NAV_STATE_WAYPOINT_NEXT] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_NEXT,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_NEXT,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
}
},
[NAV_STATE_WAYPOINT_FINISHED] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_WP_ENROUTE,
.mwError = MW_NAV_ERROR_FINISH,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, // re-process state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
}
},
/** EMERGENCY LANDING ************************************************/
[NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.mapToFlightModes = 0,
.mwState = MW_NAV_STATE_EMERGENCY_LANDING,
.mwError = MW_NAV_ERROR_LANDING,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
[NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.mapToFlightModes = 0,
.mwState = MW_NAV_STATE_EMERGENCY_LANDING,
.mwError = MW_NAV_ERROR_LANDING,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
[NAV_STATE_EMERGENCY_LANDING_FINISHED] = {
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
.mapToFlightModes = 0,
.mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_LANDING,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_LAUNCH_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_LAUNCH_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_LAUNCH_WAIT] = {
.persistentId = NAV_PERSISTENT_ID_LAUNCH_WAIT,
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
.timeoutMs = 10,
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_LAUNCH_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_LAUNCH_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
};
4.2 触发流程
与iNav开源代码之EmergencyLanding的情况类似。
taskUpdateRxMain
└──> processRx
└──> failsafeUpdateState
4.3 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;
failsafeUpdateState
├──> <!failsafeIsMonitoring() || failsafeIsSuspended()> return
├──> [status update]
│ ├──> const bool receivingRxDataAndNotFailsafeMode = failsafeIsReceivingRxData() && !IS_RC_MODE_ACTIVE(BOXFAILSAFE);
│ ├──> const bool armed = ARMING_FLAG(ARMED);
│ ├──> const bool sticksAreMoving = failsafeCheckStickMotion();
│ └──> beeperMode_e beeperMode = BEEPER_SILENCE;
├──> <!receivingRxDataAndNotFailsafeMode && ARMING_FLAG(WAS_EVER_ARMED)> beeperMode = BEEPER_RX_LOST
├──> [failsafe update]
│ ├──> FAILSAFE_IDLE
│ │ ├──> 1. <armed> Track throttle command below minimum time
│ │ └──> 2. <When NOT armed> show rxLinkState of failsafe switch in GUI (failsafe mode)
│ ├──> FAILSAFE_RX_LOSS_DETECTED
│ │ ├──> <receivingRxDataAndNotFailsafeMode> FAILSAFE_RX_LOSS_RECOVERED; return
│ │ └──> FAILSAFE_PROCEDURE_AUTO_LANDING/FAILSAFE_PROCEDURE_DROP_IT/FAILSAFE_PROCEDURE_RTH/FAILSAFE_PROCEDURE_NONE
│ ├──> FAILSAFE_RX_LOSS_IDLE
│ │ ├──> <receivingRxDataAndNotFailsafeMode && sticksAreMoving> FAILSAFE_RX_LOSS_RECOVERED; return
│ │ └──> <failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE> FAILSAFE_RX_LOSS_DETECTED; return
│ ├──> FAILSAFE_RETURN_TO_HOME
│ │ ├──> <receivingRxDataAndNotFailsafeMode && sticksAreMoving> abortForcedRTH && FAILSAFE_RX_LOSS_RECOVERED; return
│ │ ├──> [getStateOfForcedRTH()]
│ │ │ ├──> <RTH_IN_PROGRESS>
│ │ │ ├──> <RTH_HAS_LANDED>
│ │ │ └──> <RTH_IDLE> // This shouldn't happen. If RTH was somehow aborted during failsafe - fallback to FAILSAFE_LANDING procedure
│ │ │ └──> abortForcedRTH/failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING)/failsafeActivate(FAILSAFE_LANDING)
│ │ └──> <rthLanded || !armed>
│ │ ├──> failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
│ │ └──> failsafeState.phase = FAILSAFE_LANDED;
│ ├──> FAILSAFE_LANDING
│ │ ├──> <receivingRxDataAndNotFailsafeMode && sticksAreMoving> abortForcedEmergLanding && FAILSAFE_RX_LOSS_RECOVERED; return
│ │ ├──> [getStateOfForcedEmergLanding()]
│ │ │ ├──> <EMERG_LAND_IN_PROGRESS>
│ │ │ ├──> <EMERG_LAND_HAS_LANDED>
│ │ │ └──> <EMERG_LAND_IDLE>
│ │ │ └──> abortForcedEmergLanding()/failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_DROP_IT)/failsafeActivate(FAILSAFE_LANDED);
│ │ └──> <emergLanded || failsafeShouldHaveCausedLandingByNow() || !armed>
│ │ ├──> failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
│ │ └──> failsafeState.phase = FAILSAFE_LANDED
│ ├──> FAILSAFE_LANDED
│ │ ├──> ENABLE_ARMING_FLAG(ARMING_DISABLED_FAILSAFE_SYSTEM); // To prevent accidently rearming by an intermittent rx link
│ │ ├──> disarm(DISARM_FAILSAFE);
│ │ ├──> failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
│ │ ├──> failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
│ │ └──> failsafeState.controlling = false; // Failsafe no longer in control of the machine - release control to pilot
│ ├──> FAILSAFE_RX_LOSS_MONITORING
│ │ └──> [Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.]
│ └──> FAILSAFE_RX_LOSS_RECOVERED
│ /* Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
│ * This is to prevent that JustDisarm is activated on the next iteration.
│ */ Because that would have the effect of shutting down failsafe handling on intermittent connections.
│ ├──> failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
│ ├──> failsafeState.phase = FAILSAFE_IDLE;
│ ├──> failsafeState.active = false;
│ ├──> failsafeState.controlling = false;
│ └──> DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
└──> <beeperMode != BEEPER_SILENCE>
└──> beeper(beeperMode)
5. 总结
根据FAILSAFE策略、触发流程、状态机，以及异常场景的分析，当iNav执行救援时，如果配置恰当，炸机风险相对来说是可控，但是要注意参数的设置，尤其是【值得关注】的参数。
6. 参考资料
【1】iNav开源代码之严重炸机 – 危险隐患
【2】iNav开源代码之严重炸机 – FAILSAFE
【3】iNav开源代码之严重炸机 – 紧急降落
【4】iNav开源代码之EmergencyLanding
【5】iNav开源代码之Filters
【6】航模飞控之FAILSAFE机制
