Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Merge pull request #11509 from ctzsnooze/failsafe-rc5-dev

Browse source 
Failsafe changes proposed for RC5
This commit is contained in:
ctzsnooze 2022-04-13 22:12:07 +10:00 • committed by GitHub
commit f4775292b6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
11 changed files with 859 additions and 361 deletions
Download patch file Download diff file Expand all files Collapse all files

227
docs/Failsafe.md
View file

@ -1,103 +1,207 @@
# Failsafe
If the radio link is lost, or the receiver fails or becomes disconnected, the pilot will have no control over their aircraft.
Betaflight provides a failsafe system to safely manage this potential hazard.
**_It is vitally important to check that your failsafe system is working properly before flying!_**
There are two types of failsafe:
1. Receiver based failsafe
2. Flight controller based failsafe
Receiver based failsafe is where you, from your transmitter and receiver, configure channels to output desired signals if your receiver detects signal loss and goes to the __failsafe mode__. The idea is that you set throttle and other controls so the aircraft descends in a controlled manner. See your receiver's documentation for this method.
__Receiver based failsafe__ is where your transmitter configures the receiver to output specific signals on specific channels if your receiver loses signal. The receiver sends 'normal-looking' data packets to the flight controller, containing the exact values that you configured, in the radio, for throttle and other channels. Normally these are configured so that the aircraft either cuts motors and falls, or descends in a controlled manner. See your receiver's documentation for this method. The flight controller will be unaware that anything bad is happening, and you will not see anything in the OSD.
Flight controller based failsafe is where the flight controller attempts to detect signal loss and/or the __failsafe mode__ of your receiver and upon detection goes to __failsafe stage 1__. The idea is that the flight controller starts using __fallback settings__ for all controls, which are set by you, using the CLI command `rxfail` (see [rxfail](Rx.md#rx-loss-configuration) section in rx documentation) or the cleanflight-configurator GUI.
__Flight controller based failsafe__ is where the flight controller attempts to detect signal loss by analysing the received data packets, then responding according to failsafe settings made in the Configurator, and provides some indication of what is going on in the OSD and in the logs. There are three phases:
It is possible to use both types at the same time, which may be desirable. Flight controller failsafe can even help if your receiver signal wires come loose, get damaged or your receiver malfunctions in a way the receiver itself cannot detect.
- signal validation
- failsafe stage 1
- failsafe stage 2
Alternatively you may configure a transmitter switch to activate failsafe mode. This is useful for fieldtesting the failsafe system and as a **_`PANIC`_** switch when you lose orientation.
After basic signal validation fails, the flight controller holds the last known good values for 300ms. It then enters __Failsafe Stage 1__, the 'Guard' period, and applies your specified __Channel Fallback Settings__ for the `failsafe_delay` period (1.5s by default) counted from the time of the last valid packet. If any valid data arrives while in Stage 1, the flight controller will respond to it immediately, and the failsafe process stops.
If the failsafe duration exceeds the __Failsafe Stage 1__ or "Guard" period, the flight controller enters __Failsafe Stage 2__. This either immediately disarms and drops, or enters a customisable Landing Mode or a GPS Rescue Mode. After Stage 2 Failsafe, the signal must be consistently good for a period of time before control is returned to the pilot.
Flight controller based failsafe relies, for its triggering signal, on the absence of valid data from the radio receiver. If the receiver is configured to keep sending 'normal' packets on signal loss, the flight controller will never enter 'flight controller failsafe'. Hence, it is essential that the receiver is configured to send __nothing__ (no packets at all) on signal loss.
Flight controller failsafe will work if your receiver signal wires come loose, break, or your receiver malfunctions in a way the receiver itself cannot detect.
A transmitter switch may be configured to immediately activate failsafe. This is useful for field testing the failsafe system and as a **_`PANIC`_** switch when you lose orientation.
## Flight controller failsafe system
This system has two stages.
This system continuously monitors the integrity of the radio link, and has two stages that apply sequentially after signal loss is confirmed.
__Stage 1__ is entered when __a flightchannel__ has an __*invalid pulse length*__, the receiver reports __*failsafe mode*__ or there is __*no signal*__ from the receiver. Fallback settings are applied to __*all channels*__ and a short amount of time is provided to allow for recovery.
### Signal Loss
__Note:__ Prior to entering __stage 1__, fallback settings are also applied to __*individual AUX channels*__ that have invalid pulses.
__Signal loss__ means:
- __no incoming data packets__, or __failsafe mode__ or __frame dropped__ packets for more than 100ms, or
- __*invalid pulse length*__ data on any flight channel for more than 300ms.
__Stage 1__ can also directly be activated when a transmitter switch that is configured to control the failsafe mode is switched ON and `failsafe_switch_mode` is set to `STAGE1`. Stage 1 will be aborted if the switch is moved to the OFF position before Stage 2 is engaged (see next).
Once signal loss is detected, the values on the bad channels, or on all channels for total packet loss, will be held at their last received value for 300ms from the last known good data, or until signal returns.
__Stage 2__ is entered when your craft is __armed__ and __stage 1__ persists longer then the configured guard time (`failsafe_delay`). All channels will remain at the applied fallback setting unless overruled by the chosen stage 2 procedure (`failsafe_procedure`).
`RXLOSS` should be displayed in the warnings field of the OSD 100ms after the last valid packet. This is an 'early warning' of significant packet loss - an indicator that the link is in a bad way.
__Stage 2__ is not activated until 5 seconds after the flight controller boots up. This is to prevent unwanted activation, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data.
When failsafe is initiated by a failsafe switch that has been configured to to enter Stage 1, the flight channels (Roll, Pitch, Yaw and Throttle), but not the auxiliary channels, are immediately set to Stage 1 values, without any delay.
__Stage 2__ can also directly be activated when a transmitter switch that is configured to control the failsafe mode is switched ON and `failsafe_switch_mode` is set to `STAGE2`.
### Stage 1 Failsafe
__Stage 2__ will be aborted when it was due to:
__Stage 1__ applies fixed values after confirmed signal loss.
* a lost RC signal and the RC signal has recovered.
* a transmitter failsafe switch was set to ON position and the switch is set to OFF position (and `failsafe_switch_mode` is _not_ set to `KILL`).
__The default Stage 1 duration__ is one second, with a minimum of 200ms. The timer for initiation of stage 1 is activated when 100ms has elapsed since the last good packet, or 300ms after any persistently bad flight channel, but it starts timing down to failsafe from the time of the last good packet. The Stage 1 duration may be customised via the "Guard time for stage 2 activation" parameter in Configurator (`failsafe_delay` in the CLI). It starts at the time of the last good packet. The units are tenths of a second.
Note that:
* At the end of the stage 2 procedure, the flight controller will be disarmed and re-arming will be locked until the signal from the receiver is restored for specific amount of time depending on the procedure (see below) AND the arming switch is in the OFF position (when an arm switch is in use).
__During Stage 1 failsafe__, all stick positions are set to the fixed 'fallback' values as specified in the 'Channel fallback settings' panel in Configurator, after the 300ms 'hold last good values' period expires. When initiated by a switch, the aux channels remain active, and there is no hold period - the effect is immediate Default behaviour is to center the sticks, set throttle stick to zero (causing the motors to idle), and hold the existing switch positions. These values may be be customised in Configurator or with the CLI command `rxfail` (see [rxfail](Rx.md#rx-loss-configuration) section in rx documentation) or v.
* Prior to starting a stage 2 intervention it is checked if the throttle position was below `min_throttle` level for the last `failsafe_throttle_low_delay` seconds. If it was, the craft is assumed to be on the ground and is only disarmed. It may be re-armed without a power cycle.
The pilot may choose to active Level mode, using an Aux channel, so that it becomes active soon as Stage 1 commences.
Some notes about **SAFETY**:
* The failsafe system will be activated regardless of current throttle position. So when the failsafe intervention is aborted (RC signal restored/failsafe switch set to OFF) the current stick position will direct the craft !
* The craft may already be on the ground with motors stopped and that motors and props could spin again - the software does not currently detect if the craft is on the ground. Take care when using `MOTOR_STOP` feature. **Props will spin up without warning**, when armed with `MOTOR_STOP` feature ON (props are not spinning) **_and_** failsafe is activated !
The flight mode field in the OSD does *not* show failsafe with `!FS!` during stage 1, but `RXLOSS` will show in the warnings section after the 100ms basic signal validation period, to provide early warning of a bad link or impending failsafe.
## Configuration
__If signal returns during either the hold or Stage 1 period__, control is immediately returned to the pilot, the failsafe system resets. `RXLOSS` should immediately disappear from the OSD.
When configuring the flight controller failsafe, use the following steps:
> Note: the PID system remains active in Stage 1.
1. Configure your receiver to do one of the following:
__Stage 1 can also be activated by a transmitter switch__. The switch should be configured (using Modes) to enable failsafe, and the `failsafe_switch_mode` should be set to `STAGE1`.
* Upon signal loss, send no signal/pulses over the channels
* Send an invalid signal over the channels (for example, send values lower than `rx_min_usec`)
The switch emulates a signal loss, but there are some differences.
and
The main difference is that channels go immediately to Stage 1 values without waiting for any hold or signal evaluation periods.
* Ensure your receiver does not send out channel data that would cause a disarm by switch or sticks to be registered by the FC. This is especially important for those using a switch to arm.
While in Stage 1, returning the switch to the OFF position will restore normal control immediately, just like re-gaining signal during Stage 1.
See your receiver's documentation for direction on how to accomplish one of these.
Note that if the switch is held ON for longer than the `failsafe_delay` period, the flight controller will enter Stage 2 (see below), and, depending on how Stage 2 is configured, may immediately drop and disarm, from which recovery will not be immediate.
* Configure one of the transmitter switches to activate the failsafe mode.
With switch-initiated failsafe, the auxiliary channels must remain responsive so that the pilot can exit failsafe with the switch Hence they are not set 'Channel fallback settings' to the Aux channels. This is different from a true 'signal lost' type failsafe, where the Aux channels are fixed, according to the Guard period settings.
2. Set `failsafe_off_delay` to an appropriate value based on how high you fly
### Stage 2 Failsafe
__Stage 2 Failsafe__ is entered when your craft is __armed__ and __stage 1__ persists longer then the configured Stage 1, or 'guard' time (`failsafe_delay`), or, by switch.
During Stage 2, all channels will remain at the fallback settings, unless overruled by the chosen Stage 2 procedure (the settings of the `failsafe_procedure`).
> During Stage 2,`!FS!` will be shown in the Flight Mode field of the OSD.
Entering Stage 2 is not possible until 5 seconds after the flight controller boots up. This is to prevent unwanted activation, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data.
Stage 2 Failsafe can be activated directly, and immediately, with a transmitter failsafe switch where the switch behaviour is set to `STAGE2` (`failsafe_switch_mode` in the CLI).
Stage 2 will also be activated if the switch is set to Stage 1 and it is held for longer than the Stage 1 time (the Stage 2 'guard time').
When the flight contoller enters Stage 2, it implements one of three (actually, four) possible Stage 2 failsafe procedures::
* __Drop__, the default, causing immediate disarm and motor stop.
* __Landing Mode__, where the sticks are centered, throttle is held at a defined value, and the aux channels are set as configured for Stage 1 (which could include configuring an aux channel to enable Level mode). These settings will apply for the Landing Time (`failsafe_off_delay` period), which defaults to 1 second, but can be longer. Landing mode can be hazardous, since the motors and PIDs are active, but you cannot control where the quad goes, and there is no way to make the motors stop if you crash. If the machine crashes and the props get stuck, they can burn out.
* __GPS Rescue__, where an appropriately configured machine will transfer stick and throttle control to the GPS controller, so that the quad should try to fly towards its starting point.
* __Just Disarm__, a 'fourth' internal mode, which applies if the throttle has been held low for at least 10 seconds before entering Stage 2 (unless the mode is set to GPS Return). This can sometimes cause confusion when testing failsafe - always test with throttle up at some point before the test. Its primary purpose is to force a disarm if the user powers down their radio after landing, but has forgotten to remove the lipo. This prevents the quad entering landing mode, for example, and spinning the props up by itself
At the end of the stage 2 procedure, the flight controller will disarm. The word `FAILSAFE` will alternate with `RXLOSS` in the warnings field. Arming will be blocked until the signal from the receiver is restored for more than the `failsafe_recovery_delay` period. This period is the same regardless of the Stage 2 mode.
Control will be returned to the pilot:
* in Landing mode, when the RC signal has recovered for longer than the `failsafe_recovery_delay` period, or
* in GPS Rescue mode, when the link has returned for `failsafe_recovery_delay` and the pilot has moved the sticks more than 30 degrees out from centre.
* a transmitter failsafe Stage 2 switch that was set to ON is turned OFF (unless the `failsafe_switch_mode` is _not_ set to `KILL`).
__There is no way to instantly recover from a Drop or Just Disarm outcome__. The quad will disarm and fall with zero motor power. The pilot must re-arm, after the `failsafe_recovery_delay` period, to regain control. The usual arming checks apply; arming switches must be off, throttle must be zero, and if the accelerometer is enabled, the quad must be within the 'small_ _angle' range.
### The default Drop mode
The default signal loss behaviour with one second of stage 1 'guard time' is:
- 300ms holding last values
- the next 1200ms at idle throttle with sticks centred (the default fallback settings)
- followed by disarm and drop at 1s.
Recovery within the Stage 1 time is immediate, but if Stage 2 completes to disarm, the defaultRecovery time of one second will block arming for one full second.
The defaults are typically used by racers and park fliers.
The `failsafe_delay`, or guard time, should be long enough that a brief Rx loss will be tolerated without leading to a disarm. It should reflect the reliability of your link, vs how long it typically takes before you hit the ground. If you crash within the guard time, the PIDs may cook your motors.
Shorter guard times will stop the motors more quickly when signal is lost. In practice the minimum is 200ms. Any shorter and you are vulnerable to false failsafes from brief signal loss.
The default of 1.5s is a good guard time.
The `failsafe_recovery_delay` is how long the signal must be 'good' for before you're allowed to re-arm after being disarmed by the Drop. By default this is one second. For many setups this time can be a lot shorter. However, some radio links can be erratic when they recover, so don't make it too short without first checking that the quad doesn't go bezerk when the signal recovers.
### Landing mode
This can be used to apply, after the Stage 1 or Guard period expires. a defined set of Aux switch settings and stick values for a set period of time.
Historically, this was used to enable Level mode, and apply sufficient throttle for a gradual fall from a typical flight altitude. It had a role when people would hover at a consistent altitude and not do much else, potentially minimising the damage from a crash from altitude.
However, it is s a potentially hazardous thing to do, and not generally recommended, because the quad will fall with active PIDs and will drift with the breeze, potentially landing almost anywhere. The motors may be active when it hits the ground, and could burn them out, and the quad could land on top of people with throttle on and fully active PIDs.
We do not recommend this anymore.
You will regain normal flight control during landing mode after signal restores for more than the `failsafe_recovery_delay` period.
You will regain the ability to re-arm after landing mode terminates and disarms, after the signal has been restored for more than the `failsafe_recovery_delay` period.
__Configuring Landing Mode.__
1. Enable Landing Mode as opposed to Drop as the failsafe procedure
2. Set `failsafe_off_delay` to an appropriate value based on how high you fly (how long you think it will take to land at the set throttle value).
3. Set `failsafe_throttle` to a value that allows the aircraft to descend at approximately one meter per second (default is 1000 which should be throttle off).
The behaviour with default one second of stage 1 'guard time' and a 10s Landing time is:
- 300ms holding last values
- the next 1,200ms at idle throttle with sticks centred (the default fallback settings)
- 10s of landing throttle
- your Stage 1 Aux switch values applied
### GPS Return mode
The full details of GPS return are covered in the wiki, and elsewhere.
You will regain during GPS Return mode only after signal restores for more than the `failsafe_recovery_delay` period AND you move the sticks more than 30 degrees out from centre.
You will regain the ability to re-arm after GPS Return terminates and disarms, after the signal has been restored for more than the `failsafe_recovery_delay` period.
### "Just Drop" mode
This is an 'invisible' mode that is always present.
It is intended to 'catch' the possibility that the pilot has landed, forgotten to disarm, and powered off their transmitter. This would result in a failsafe, and if landing mode was active, the motors could spin up.
"Just Drop" looks at the throttle position, and if it has been down for 10s before turning the transmitter off, the failsafe system will immediately disarm the quad, and not enter landing mode. This protects the pilot, so long as they have throttle low for 10s before switching the radio off.
### General Stage 2 SAFETY Considerations
* The failsafe system will activate and de-activate without regard to the current throttle or stick position. When the failsafe intervention is aborted (RC signal restored/failsafe switch set to OFF) the current throttle and stick positions will be applied to the craft!
* If you land and turn your transmitter off before powering down your quad, it could be on the ground with motors stopped, and those motors and props could spin again. Take care when using `Landing Mode`, particularly with the `MOTOR_STOP` feature. **The props may spin up without warning**, when failsafe activates in Landing mode!
* In 4.3 re-arming is possible, after a failsafe, without needing to power cycle the quad, so that if you crash after failsafe in an inaccessible area, you can perhaps get close enough to regain signal and fly out.
These are the basic steps for flight controller failsafe configuration; see Failsafe Settings below for additional settings that may be changed.
## Failsafe Settings
Failsafe delays are configured in 0.1 second steps.
1 step = 0.1sec
1 second = 10 steps
### `failsafe_delay`
Guard time for failsafe activation after signal lost. This is the amount of time the flight controller waits to see if it begins receiving a valid signal again before activating failsafe.
Guard time, or Failsafe Stage 1 period; the time before for failsafe Stage 2 activation after a lost signal. This is the amount of time the flight controller waits, after a signal loss, before activating Stage 2 failsafe.
### `failsafe_off_delay`
Delay after failsafe activates before motors finally turn off. This is the amount of time 'failsafe_throttle' is active. If you fly at higher altitudes you may need more time to descend safely.
The time from when Landing Mode activates to when the motors finally turn off. Throttle will be at 'failsafe_throttle' for this period of time. If you fly at higher altitudes you may need more time to descend safely.
### `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.
Throttle level used for landing. Specify a value that causes the aircraft to descend at about 1M/sec (relatively slowly). Default is set to 1000 which should correspond to throttle off.
### `failsafe_switch_mode`
Configure the RC switched failsafe action. It can be one of:
* `STAGE1` - activates Stage 1 failsafe. RC controls are applied as configured for Stage 1 and the `failsafe_delay` guard time will have to elapse before Stage 2 is activated. This is useful if you want to simulate with a switch the exact signal loss failsafe behavior.
* `STAGE2` - skips Stage 1 and activates the Stage 2 procedure immediately (see `failsafe_procedure`). Useful if you want to assign instant auto-landing to a switch.
* `KILL` - disarms instantly (your craft will crash). Re-arming is locked for 1 second AND until the arming switch (if used) is moved to the OFF position. Similar effect can be achieved by:
* setting `failsafe_switch_mode` to `STAGE2` and `failsafe_procedure` to `DROP`. The difference is that `DROP` locks re-arming for 3 seconds instead of 1.
* setting `failsafe_switch_mode` to `STAGE2`, `failsafe_procedure` to `AUTO-LAND`, setting `failsafe_throttle` to 1000 and `failsafe_off_delay` to 0 (basically initiates an auto-landing but cuts it short immediately). This is not preferred method, since the reaction is slower and re-arming will be locked for 30 seconds.
* using arm switch. This does not introduce re-arming locking.
* `STAGE1` - activates Stage 1 failsafe. RC controls are applied as configured for Stage 1 and the `failsafe_delay` guard time will have to elapse before Stage 2 is activated. This is useful if you want to simulate signal loss failsafe behavior. Note that there are some differences. There is no 300ms 'hold' time; the flight channels (RPY&T) go directly to Stage 1 settings the moment the switch goes ON. Also, the Auxiliary channels remain active, and are not set to the Stage 1 values. Recovery of signal immediately restores full pilot control.
* `STAGE2` - skips Stage 1 and immediately activates the selected Stage 2 procedure. Useful if you want to assign instant auto-landing, GPS Return, or Drop, to a switch.
* `KILL` - immediately disarms the quad with no delay. Your craft will crash. Note that a single glitch on the failsafe channel will immediately crash the quad. Re-arming is blocked for 1 second after signal is restored. A similar, but safer effect can be achieved by:
* setting `failsafe_switch_mode` to `STAGE2`, `failsafe_procedure` to `DROP`, and `failsafe delay` to 2. This gives a 200ms delay signal validation period, the shortest allowed, so that transient glitches on the failsafe channel will not falsely trigger a disarm. Drop recovery can be made faster than Kill by configuring a short `failsafe_recovery_delay` time (which can be as short as 200ms).
* using the arm switch. This does not introduce re-arming locking.
### `failsafe_throttle_low_delay`
@ -110,6 +214,16 @@ Use standard RX μs values. See [Rx documentation](Rx.md).
* `DROP`: Just kill the motors and disarm (crash the craft). Re-arming is locked until RC link is available for at least 3 seconds and the arm switch (if used) is in the OFF position.
* `AUTO-LAND`: Enable an auto-level mode, center the flight sticks and set the throttle to a predefined value (`failsafe_throttle`) for a predefined time (`failsafe_off_delay`). This should allow the craft to come to a safer landing. Re-arming is locked until RC link is available for at least 30 seconds and the arm switch (if used) is in the OFF position.
### `failsafe_recovery_delay`
Time for a recovered signal to be considered valid. In Failsafe Landing Mode, signal must be 'good' for at least this time for control to be returned to the pilot. In GPS Return mode, this time is required before the stick inputs will be assessed for the restoration of control.
### `failsafe_stick_threshold`
For GPS Return, the angle in degrees that the sticks must be away from centre in order to return control to the pilot, assuming the signal has already recovered.
The idea is that as the quad flies home, the pilot leaves the sticks centred. Once they get video back, and see that Rx signal has returned, moving the sticks allows the pilot to regain control at a time that suits them, rather than just immediately signal returns.
### `rx_min_usec`
The lowest channel value considered valid. e.g. PWM/PPM pulse length
@ -122,25 +236,38 @@ The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sen
With a Graupner GR-24 configured for PWM output with failsafe on channels 1-4 set to OFF in the receiver settings then this setting, at its default value, will allow failsafe to be activated.
## Testing
## Testing failsafe
**Bench test of Stage 1 - _remove the props!_.**
1. Set the Guard, or stage 1 time, to 10s (100)
1. Set the Stage 2 procedure to Drop
1. Arm the craft and throttle up briefly
1. While wiggling the sticks, listen to the motors, and throttle off
1. Confirm that motors hold RPM for 300ms, then drop to idle and stay there
1. Note that RXLOSS appears in the OSD immediately the link is lost
1. Power up the transmitter, wiggle the pitch/roll stick while it re-establishes the link
1. Confirm that the motors start responding about 1s after the link is re-established
**Bench test the failsafe system before flying - _remove props while doing so_.**
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Configure failsafe
1. Arm the craft and throttle up briefly
1. Either turn the transmitter off, or unplug the RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Observe motors turn off after configured duration.
1. Ensure that when you turn on your TX again or reconnect the RX that you cannot re-arm once the motors have stopped.
1. Power cycle the FC.
1. Arm the craft.
1. Configure a long Stage 2 Landing Time (eg 200 or 20s) and enable Landing mode
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Turn on TX or reconnect RX.
1. Observe motors spin at configured Landing throttle setting.
1. Turn on TX or reconnect RX before Landing times out
1. Ensure that your switch positions don't now cause the craft to disarm (otherwise it would fall out of the sky on regained signal).
1. Observe that normal flight behavior is resumed.
1. Disarm.
**Field test the failsafe system.**
**Field test of Landing Mode.**
1. Perform bench testing first!
1. On a calm day go to an unpopulated area away from buildings or test indoors in a safe controlled environment - e.g. inside a big net.
@ -157,6 +284,6 @@ With a Graupner GR-24 configured for PWM output with failsafe on channels 1-4 se
If craft descends too quickly then increase failsafe throttle setting.
Ensure that the duration is long enough for your craft to land at the altitudes you normally fly at.
Ensure that the duration is long enough for your craft to land from the altitudes you normally fly at.
Using a configured transmitter switch to activate failsafe mode, instead of switching off your TX, is good primary testing method in addition to the above procedure.

162
docs/Failsafe_before_4.3.md Normal file
View file

@ -0,0 +1,162 @@
# Failsafe
There are two types of failsafe:
1. Receiver based failsafe
2. Flight controller based failsafe
Receiver based failsafe is where you, from your transmitter and receiver, configure channels to output desired signals if your receiver detects signal loss and goes to the __failsafe mode__. The idea is that you set throttle and other controls so the aircraft descends in a controlled manner. See your receiver's documentation for this method.
Flight controller based failsafe is where the flight controller attempts to detect signal loss and/or the __failsafe mode__ of your receiver and upon detection goes to __failsafe stage 1__. The idea is that the flight controller starts using __fallback settings__ for all controls, which are set by you, using the CLI command `rxfail` (see [rxfail](Rx.md#rx-loss-configuration) section in rx documentation) or the cleanflight-configurator GUI.
It is possible to use both types at the same time, which may be desirable. Flight controller failsafe can even help if your receiver signal wires come loose, get damaged or your receiver malfunctions in a way the receiver itself cannot detect.
Alternatively you may configure a transmitter switch to activate failsafe mode. This is useful for fieldtesting the failsafe system and as a **_`PANIC`_** switch when you lose orientation.
## Flight controller failsafe system
This system has two stages.
__Stage 1__ is entered when __a flightchannel__ has an __*invalid pulse length*__, the receiver reports __*failsafe mode*__ or there is __*no signal*__ from the receiver. Fallback settings are applied to __*all channels*__ and a short amount of time is provided to allow for recovery.
__Note:__ Prior to entering __stage 1__, fallback settings are also applied to __*individual AUX channels*__ that have invalid pulses.
__Stage 1__ can also directly be activated when a transmitter switch that is configured to control the failsafe mode is switched ON and `failsafe_switch_mode` is set to `STAGE1`. Stage 1 will be aborted if the switch is moved to the OFF position before Stage 2 is engaged (see next).
__Stage 2__ is entered when your craft is __armed__ and __stage 1__ persists longer then the configured guard time (`failsafe_delay`). All channels will remain at the applied fallback setting unless overruled by the chosen stage 2 procedure (`failsafe_procedure`).
__Stage 2__ is not activated until 5 seconds after the flight controller boots up. This is to prevent unwanted activation, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data.
__Stage 2__ can also directly be activated when a transmitter switch that is configured to control the failsafe mode is switched ON and `failsafe_switch_mode` is set to `STAGE2`.
__Stage 2__ will be aborted when it was due to:
* a lost RC signal and the RC signal has recovered.
* a transmitter failsafe switch was set to ON position and the switch is set to OFF position (and `failsafe_switch_mode` is _not_ set to `KILL`).
Note that:
* At the end of the stage 2 procedure, the flight controller will be disarmed and re-arming will be locked until the signal from the receiver is restored for specific amount of time depending on the procedure (see below) AND the arming switch is in the OFF position (when an arm switch is in use).
* Prior to starting a stage 2 intervention it is checked if the throttle position was below `min_throttle` level for the last `failsafe_throttle_low_delay` seconds. If it was, the craft is assumed to be on the ground and is only disarmed. It may be re-armed without a power cycle.
Some notes about **SAFETY**:
* The failsafe system will be activated regardless of current throttle position. So when the failsafe intervention is aborted (RC signal restored/failsafe switch set to OFF) the current stick position will direct the craft !
* The craft may already be on the ground with motors stopped and that motors and props could spin again - the software does not currently detect if the craft is on the ground. Take care when using `MOTOR_STOP` feature. **Props will spin up without warning**, when armed with `MOTOR_STOP` feature ON (props are not spinning) **_and_** failsafe is activated !
## Configuration
When configuring the flight controller failsafe, use the following steps:
1. Configure your receiver to do one of the following:
* Upon signal loss, send no signal/pulses over the channels
* Send an invalid signal over the channels (for example, send values lower than `rx_min_usec`)
and
* Ensure your receiver does not send out channel data that would cause a disarm by switch or sticks to be registered by the FC. This is especially important for those using a switch to arm.
See your receiver's documentation for direction on how to accomplish one of these.
* Configure one of the transmitter switches to activate the failsafe mode.
2. Set `failsafe_off_delay` to an appropriate value based on how high you fly
3. Set `failsafe_throttle` to a value that allows the aircraft to descend at approximately one meter per second (default is 1000 which should be throttle off).
These are the basic steps for flight controller failsafe configuration; see Failsafe Settings below for additional settings that may be changed.
## Failsafe Settings
Failsafe delays are configured in 0.1 second steps.
1 step = 0.1sec
1 second = 10 steps
### `failsafe_delay`
Guard time for failsafe activation after signal lost. This is the amount of time the flight controller waits to see if it begins receiving a valid signal again before activating failsafe.
### `failsafe_off_delay`
Delay after failsafe activates before motors finally turn off. This is the amount of time 'failsafe_throttle' is active. If you fly at higher altitudes you may need more time to descend safely.
### `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.
### `failsafe_switch_mode`
Configure the RC switched failsafe action. It can be one of:
* `STAGE1` - activates Stage 1 failsafe. RC controls are applied as configured for Stage 1 and the `failsafe_delay` guard time will have to elapse before Stage 2 is activated. This is useful if you want to simulate with a switch the exact signal loss failsafe behavior.
* `STAGE2` - skips Stage 1 and activates the Stage 2 procedure immediately (see `failsafe_procedure`). Useful if you want to assign instant auto-landing to a switch.
* `KILL` - disarms instantly (your craft will crash). Re-arming is locked for 1 second AND until the arming switch (if used) is moved to the OFF position. Similar effect can be achieved by:
* setting `failsafe_switch_mode` to `STAGE2` and `failsafe_procedure` to `DROP`. The difference is that `DROP` locks re-arming for 3 seconds instead of 1.
* setting `failsafe_switch_mode` to `STAGE2`, `failsafe_procedure` to `AUTO-LAND`, setting `failsafe_throttle` to 1000 and `failsafe_off_delay` to 0 (basically initiates an auto-landing but cuts it short immediately). This is not preferred method, since the reaction is slower and re-arming will be locked for 30 seconds.
* using arm switch. This does not introduce re-arming locking.
### `failsafe_throttle_low_delay`
Time throttle level must have been below 'min_throttle' to _only disarm_ instead of _full failsafe procedure_.
Use standard RX μs values. See [Rx documentation](Rx.md).
### `failsafe_procedure`
* `DROP`: Just kill the motors and disarm (crash the craft). Re-arming is locked until RC link is available for at least 3 seconds and the arm switch (if used) is in the OFF position.
* `AUTO-LAND`: Enable an auto-level mode, center the flight sticks and set the throttle to a predefined value (`failsafe_throttle`) for a predefined time (`failsafe_off_delay`). This should allow the craft to come to a safer landing. Re-arming is locked until RC link is available for at least 30 seconds and the arm switch (if used) is in the OFF position.
### `rx_min_usec`
The lowest channel value considered valid. e.g. PWM/PPM pulse length
### `rx_max_usec`
The highest channel value considered valid. e.g. PWM/PPM pulse length
The `rx_min_usec` and `rx_max_usec` settings helps detect when your RX stops sending any data, enters failsafe mode or when the RX looses signal.
With a Graupner GR-24 configured for PWM output with failsafe on channels 1-4 set to OFF in the receiver settings then this setting, at its default value, will allow failsafe to be activated.
## Testing
**Bench test the failsafe system before flying - _remove props while doing so_.**
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Observe motors turn off after configured duration.
1. Ensure that when you turn on your TX again or reconnect the RX that you cannot re-arm once the motors have stopped.
1. Power cycle the FC.
1. Arm the craft.
1. Turn off transmitter or unplug RX.
1. Observe motors spin at configured throttle setting for configured duration.
1. Turn on TX or reconnect RX.
1. Ensure that your switch positions don't now cause the craft to disarm (otherwise it would fall out of the sky on regained signal).
1. Observe that normal flight behavior is resumed.
1. Disarm.
**Field test the failsafe system.**
1. Perform bench testing first!
1. On a calm day go to an unpopulated area away from buildings or test indoors in a safe controlled environment - e.g. inside a big net.
1. Arm the craft.
1. Hover over something soft (long grass, ferns, heather, foam, etc.).
1. Descend the craft and observe throttle position and record throttle value from your TX channel monitor. Ideally 1500 should be hover. So your value should be less than 1500.
1. Stop, disarm.
1. Set failsafe throttle to the recorded value.
1. Arm, hover over something soft again.
1. Turn off TX (!)
1. Observe craft descends and motors continue to spin for the configured duration.
1. Observe FC disarms after the configured duration.
1. Remove flight battery.
If craft descends too quickly then increase failsafe throttle setting.
Ensure that the duration is long enough for your craft to land at the altitudes you normally fly at.
Using a configured transmitter switch to activate failsafe mode, instead of switching off your TX, is good primary testing method in addition to the above procedure.

2
src/main/fc/core.c
View file

@ -985,7 +985,7 @@ void processRxModes(timeUs_t currentTimeUs)
}
#ifdef USE_GPS_RESCUE
if (ARMING_FLAG(ARMED) && (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE))) {
if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXGPSRESCUE)) {
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}

138
src/main/flight/failsafe.c
View file

@ -63,14 +63,14 @@ static failsafeState_t failsafeState;
PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2);
PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_throttle = 1000, // default throttle off.
.failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_delay = 10, // 1 sec, can regain control on signal recovery, at idle in drop mode
.failsafe_throttle = 1000, // default throttle off.
.failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_delay = 15, // 1.5 sec stage 1 period, can regain control on signal recovery, at idle in drop mode
.failsafe_off_delay = 10, // 1 sec in landing phase, if enabled
.failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // default failsafe switch action is identical to rc link loss
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT, // default full failsafe procedure is 0: auto-landing
.failsafe_recovery_delay = 10, // 1 sec of valid rx data needed to allow recovering from failsafe procedure
.failsafe_stick_threshold = 30 // 30 percent of stick deflection to exit GPS Rescue procedure
.failsafe_stick_threshold = 30 // 30 percent of stick deflection to exit GPS Rescue procedure
);
const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
@ -86,9 +86,9 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
*/
void failsafeReset(void)
{
failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; // time to start stage2
if (failsafeState.rxDataFailurePeriod < PERIOD_RXDATA_RECOVERY) {
// PERIOD_RXDATA_RECOVERY (200ms) - avoid transients and ensure reliable arming
failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
if (failsafeState.rxDataFailurePeriod < PERIOD_RXDATA_RECOVERY){
// avoid transients and ensure reliable arming for minimum of PERIOD_RXDATA_RECOVERY (200ms)
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_RECOVERY;
}
failsafeState.rxDataRecoveryPeriod = failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
@ -129,13 +129,6 @@ bool failsafeIsActive(void)
return failsafeState.active;
}
#ifdef USE_GPS_RESCUE
bool failsafePhaseIsGpsRescue(void)
{
return failsafeState.phase == FAILSAFE_GPS_RESCUE;
}
#endif
void failsafeStartMonitoring(void)
{
failsafeState.monitoring = true;
@ -146,19 +139,6 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
return (millis() > failsafeState.landingShouldBeFinishedAt);
}
static void failsafeActivate(void)
{
failsafeState.active = true;
failsafeState.phase = FAILSAFE_LANDING;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.events++;
}
bool failsafeIsReceivingRxData(void)
{
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
@ -176,32 +156,50 @@ void failsafeOnRxResume(void)
}
void failsafeOnValidDataReceived(void)
// runs when packets are received for more than the signal validation period (100ms)
{
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// clear RXLOSS in OSD immediately we get a good packet, and un-set its arming block
failsafeState.validRxDataReceivedAt = millis();
if ((cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod) ||
(cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > 0)) {
if (failsafeState.validRxDataFailedAt == 0) {
// after initialisation, we sometimes only receive valid packets,
// then we don't know how long the signal has been valid for
// in this setting, the time the signal first valid is also time it was last valid, so
// initialise validRxDataFailedAt to the time of the first valid data
failsafeState.validRxDataFailedAt = failsafeState.validRxDataReceivedAt;
setArmingDisabled(ARMING_DISABLED_BST);
// prevent arming until we have valid data for rxDataRecoveryPeriod after initialisation
// using the BST flag since no other suitable name....
}
if (cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod){
// rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms)
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
// Allow arming now we have an RX link
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
unsetArmingDisabled(ARMING_DISABLED_BST);
}
}
void failsafeOnValidDataFailed(void)
// runs when packets are lost for more than the signal validation period (100ms)
{
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// set RXLOSS in OSD and block arming after 100ms of signal loss (is restored in rx.c immediately signal returns)
failsafeState.validRxDataFailedAt = millis();
if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
// rxDataFailurePeriod is stage 1 guard time
// sets rxLinkState = DOWN to initiate stage 2 failsafe, if no validated signal for the stage 1 period
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
// Prevent arming with no RX link
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// show RXLOSS and block arming
}
}
void failsafeCheckDataFailurePeriod(void)
// forces link down directly from scheduler?
// runs directly from scheduler, every 10ms, to validate the link
{
if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) {
// sets link DOWN after the stage 1 failsafe period, initiating stage 2
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
// Prevent arming with no RX link
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
@ -249,16 +247,18 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus()) {
if (calculateThrottleStatus() != THROTTLE_LOW) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
// Failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
// Skip auto-landing procedure
failsafeState.active = true;
failsafeState.events++;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.phase = FAILSAFE_LANDED;
// Require 3 seconds of valid rxData
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
// go to landed immediately
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// allow re-arming 1 second after Rx recovery, customisable
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod
@ -266,13 +266,15 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
&& failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
#endif
) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds before failsafe
// protects against false arming when the Tx is powered up after the quad
failsafeActivate();
// skip auto-landing procedure
// JustDisarm if throttle was LOW for at least 'failsafe_throttle_low_delay' before failsafe
// protects against false arming when the Tx is powered up after the quad
failsafeState.active = true;
failsafeState.events++;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.phase = FAILSAFE_LANDED;
// re-arm at rxDataRecoveryPeriod - default is 1.0 seconds
// go directly to FAILSAFE_LANDED
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// allow re-arming 1 second after Rx recovery, customisable
} else {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
}
@ -294,24 +296,31 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
failsafeState.active = true;
failsafeState.events++;
switch (failsafeConfig()->failsafe_procedure) {
case FAILSAFE_PROCEDURE_AUTO_LANDING:
// Stabilize, and set Throttle to specified level
failsafeActivate();
// Enter Stage 2 with settings for landing mode
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.phase = FAILSAFE_LANDING;
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// allow re-arming 1 second after Rx recovery
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
break;
case FAILSAFE_PROCEDURE_DROP_IT:
// Drop the craft
failsafeActivate();
// re-arm at rxDataRecoveryPeriod - default is 1.0 seconds
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// skip auto-landing procedure
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.phase = FAILSAFE_LANDED;
// go directly to FAILSAFE_LANDED
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// allow re-arming 1 second after Rx recovery
break;
#ifdef USE_GPS_RESCUE
case FAILSAFE_PROCEDURE_GPS_RESCUE:
failsafeActivate();
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
failsafeState.phase = FAILSAFE_GPS_RESCUE;
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
// allow re-arming 3 seconds after Rx recovery
break;
#endif
}
@ -328,8 +337,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
beeperMode = BEEPER_RX_LOST_LANDING;
}
if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
// require 3 seconds of valid rxData
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
// to manually disarm while Landing, aux channels must be enabled
failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true;
}
@ -339,17 +347,15 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
case FAILSAFE_GPS_RESCUE:
if (receivingRxData) {
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
// hence we must allow stick inputs during FAILSAFE_GPS_RESCUE see PR #7936 for rationale
// this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
} else {
if (armed) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
beeperMode = BEEPER_RX_LOST_LANDING;
} else {
// require 3 seconds of valid rxData
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
// to manually disarm while in GPS Rescue, aux channels must be enabled
failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true;
}
@ -357,16 +363,17 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
break;
#endif
case FAILSAFE_LANDED:
// Prevent accidently rearming by an intermittent rx link
setArmingDisabled(ARMING_DISABLED_FAILSAFE);
disarm(DISARM_REASON_FAILSAFE);
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
setArmingDisabled(ARMING_DISABLED_FAILSAFE);
// prevent accidently rearming by an intermittent rx link
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
// customise receivingRxDataPeriod according to type of failsafe
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
reprocessState = true;
break;
case FAILSAFE_RX_LOSS_MONITORING:
// Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
// Monitoring the rx link, allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
if (receivingRxData) {
if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now
@ -385,6 +392,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
#ifdef USE_GPS_RESCUE
DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
#endif
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
reprocessState = true;

5
src/main/flight/failsafe.h
View file

@ -97,16 +97,13 @@ void failsafeReset(void);
void failsafeStartMonitoring(void);
void failsafeUpdateState(void);
void failsafeCheckDataFailurePeriod(void);
failsafePhase_e failsafePhase(void);
bool failsafeIsMonitoring(void);
bool failsafeIsActive(void);
bool failsafeIsReceivingRxData(void);
void failsafeCheckDataFailurePeriod(void);
void failsafeOnRxSuspend(uint32_t suspendPeriod);
void failsafeOnRxResume(void);
void failsafeOnValidDataReceived(void);
void failsafeOnValidDataFailed(void);
#ifdef USE_GPS_RESCUE
bool failsafePhaseIsGpsRescue(void);
#endif

88
src/main/rx/rx.c
View file

@ -90,8 +90,6 @@ static uint8_t rfMode = 0;
static uint16_t uplinkTxPwrMw = 0; //Uplink Tx power in mW
#endif
#define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec
#define RSSI_ADC_DIVISOR (4096 / 1024)
#define RSSI_OFFSET_SCALING (1024 / 100.0f)
@ -113,16 +111,14 @@ static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define MAX_INVALID_PULSE_TIME 300 // hold time in millisecons after bad channel or Rx link loss
#define MAX_INVALID_PULSE_TIME_MS 300 // hold time in milliseconds after bad channel or Rx link loss
// will not be actioned until the nearest multiple of 100ms
#define PPM_AND_PWM_SAMPLE_COUNT 3
#define DELAY_50_HZ (1000000 / 50)
#define DELAY_33_HZ (1000000 / 33)
#define DELAY_15_HZ (1000000 / 15)
#define DELAY_10_HZ (1000000 / 10)
#define DELAY_5_HZ (1000000 / 5)
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
#define DELAY_20_MS (20 * 1000) // 20ms in us
#define DELAY_100_MS (100 * 1000) // 100ms in us
#define DELAY_1500_MS (1500 * 1000) // 1.5 seconds in us
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
rxRuntimeState_t rxRuntimeState;
static uint8_t rcSampleIndex = 0;
@ -285,7 +281,7 @@ void rxInit(void)
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig()->midrc;
validRxSignalTimeout[i] = millis() + MAX_INVALID_PULSE_TIME;
validRxSignalTimeout[i] = millis() + MAX_INVALID_PULSE_TIME_MS;
}
rcData[THROTTLE] = (featureIsEnabled(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
@ -382,9 +378,9 @@ void suspendRxPwmPpmSignal(void)
{
#if defined(USE_PWM) || defined(USE_PPM)
if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
suspendRxSignalUntil = micros() + DELAY_1500_MS; // 1.5s
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
failsafeOnRxSuspend(DELAY_1500_MS); // 1.5s
}
#endif
}
@ -472,14 +468,13 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired;
}
FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs)
FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
bool signalReceived = false;
bool useDataDrivenProcessing = true;
// need the next packet in 2.5 x the anticipated time
timeDelta_t needRxSignalMaxDelayUs = anticipatedDeltaTime10thUs * 2 / 8;
timeDelta_t needRxSignalMaxDelayUs = DELAY_100_MS;
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, needRxSignalMaxDelayUs / 10);
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, MIN(2000, currentDeltaTimeUs / 100));
if (taskUpdateRxMainInProgress()) {
// no need to check for new data as a packet is being processed already
@ -523,13 +518,13 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
if (signalReceived) {
// true only when a new packet arrives
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
rxSignalReceived = true; // immediately process data
rxSignalReceived = true; // immediately process packet data
} else {
// watch for next packet
if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) {
// initial time to signalReceived failure is 2.5 packets then every 100ms
// initial time to signalReceived failure is 100ms, then we check every 100ms
rxSignalReceived = false;
needRxSignalBefore = currentTimeUs + (timeDelta_t)DELAY_10_HZ;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
// review rcData values every 100ms in failsafe changed them
rxDataProcessingRequired = true;
}
@ -642,30 +637,24 @@ void detectAndApplySignalLossBehaviour(void)
{
const uint32_t currentTimeMs = millis();
const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
#ifdef USE_GPS_RESCUE
const bool gpsRescue = failsafePhaseIsGpsRescue();
#endif
bool allAuxChannelsAreGood = true;
// used to record if any non-aux channel is out of range for the timeout period, assume they are good
rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch;
// set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch
for (int channel = 0; channel < rxChannelCount; channel++) {
float sample = rcRaw[channel];
const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample);
// if the packet is bad, we don't allow any channels to be good
// if the packet is bad, we don't allow any channels to be good
if (thisChannelValid) {
// reset the invalid pulse period timer for every good channel
validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME;
validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME_MS;
}
if (ARMING_FLAG(ARMED) && failsafeIsActive()) {
// apply failsafe values, until failsafe ends, or disarmed, unless in GPS Return
if (channel < NON_AUX_CHANNEL_COUNT) {
#ifdef USE_GPS_RESCUE
if (gpsRescue) {
continue;
}
#endif
if ((channel < NON_AUX_CHANNEL_COUNT) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
if (channel == THROTTLE ) {
sample = failsafeConfig()->failsafe_throttle;
} else {
@ -676,21 +665,22 @@ void detectAndApplySignalLossBehaviour(void)
sample = getRxfailValue(channel);
}
} else {
// in STAGE 1 failsafe or HOLD period.
if (!thisChannelValid) {
if (failsafeAuxSwitch) {
if (channel < NON_AUX_CHANNEL_COUNT) {
sample = getRxfailValue(channel);
// set RPYT values to Stage 1 values immediately if initiated by switch
}
} else if (!thisChannelValid) {
if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
// HOLD PERIOD is MAX_INVALID_PULSE_TIME or 300ms for invalid channels/packets
// HOLD bad channel/s for MAX_INVALID_PULSE_TIME_MS (300ms) after Rx loss
} else {
// in STAGE 1 failsafe now that hold time has expired
// then use STAGE 1 failsafe values
if (channel < NON_AUX_CHANNEL_COUNT) {
rxFlightChannelsValid = false;
// some flight channels were bad, so flag them
sample = getRxfailValue(channel);
// affected RPYT values will get Stage 1 values
} else if (!failsafeAuxSwitch) {
// for switch initiated failsafe, only the flight channels get Stage 1 values
sample = getRxfailValue(channel);
allAuxChannelsAreGood = false;
// declare signal lost after 300ms of at least one bad flight channel
}
sample = getRxfailValue(channel);
// set all channels to Stage 1 values
}
}
}
@ -708,16 +698,12 @@ void detectAndApplySignalLossBehaviour(void)
}
}
if (rxFlightChannelsValid) {
// clear RXLOSS in OSD
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// --> start the timer to exit stage 2 failsafe
if (rxFlightChannelsValid && allAuxChannelsAreGood) {
failsafeOnValidDataReceived();
// --> start the timer to exit stage 2 failsafe
} else {
// show RXLOSS in OSD (may be a little aggressive ? if so move up to Stage 1)
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// -> start timer to enter stage2 failsafe
failsafeOnValidDataFailed();
// -> start timer to enter stage2 failsafe
}
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
@ -831,7 +817,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) {
return;
}
rssiUpdateAt = currentTimeUs + DELAY_50_HZ;
rssiUpdateAt = currentTimeUs + DELAY_20_MS;
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR;
@ -850,7 +836,7 @@ void updateRSSI(timeUs_t currentTimeUs)
updateRSSIADC(currentTimeUs);
break;
case RSSI_SOURCE_MSP:
if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > DELAY_1500_MS) { // 1.5s
rssi = 0;
}
break;

2
src/main/rx/rx.h
View file

@ -177,7 +177,7 @@ extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed
void rxInit(void);
void rxProcessPending(bool state);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs);
void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);

5
src/main/scheduler/scheduler.c
View file

@ -507,10 +507,9 @@ FAST_CODE void scheduler(void)
// Check for incoming RX data. Don't do this in the checker as that is called repeatedly within
// a given gyro loop, and ELRS takes a long time to process this and so can only be safely processed
// before the checkers
rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs), getTask(TASK_RX)->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT);
rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs));
}
// Check for failsafe conditions without reliance on the RX task being well behaved
// Check for failsafe conditions every 10ms, independently of the Rx Task, which runs at 33hz.
if (cmp32(millis(), lastFailsafeCheckMs) > PERIOD_RXDATA_FAILURE) {
// This is very low cost taking less that 4us every 10ms
failsafeCheckDataFailurePeriod();

589
src/test/unit/flight_failsafe_unittest.cc
View file

@ -80,11 +80,12 @@ void configureFailsafe(void)
rxConfigMutable()->mincheck = TEST_MIN_CHECK;
failsafeConfigMutable()->failsafe_delay = 10; // 1 second
failsafeConfigMutable()->failsafe_off_delay = 50; // 5 seconds
failsafeConfigMutable()->failsafe_off_delay = 15; // 1.5 seconds
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1;
failsafeConfigMutable()->failsafe_throttle = 1200;
failsafeConfigMutable()->failsafe_throttle_low_delay = 50; // 5 seconds
failsafeConfigMutable()->failsafe_throttle_low_delay = 100; // 10 seconds
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
// NB we don't have failsafe_recovery_delay so use PERIOD_RXDATA_RECOVERY (200ms)
sysTickUptime = 0;
}
@ -143,7 +144,8 @@ TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
ENABLE_ARMING_FLAG(ARMED);
// when
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime ++;
failsafeOnValidDataFailed(); // set last invalid sample to a non-zero value
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
@ -170,80 +172,131 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
}
}
/****************************************************************************************/
//
// Clean start
//
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
{
// given
ENABLE_ARMING_FLAG(ARMED);
// and
configureFailsafe();
failsafeInit();
DISABLE_ARMING_FLAG(ARMED);
resetCallCounters();
failsafeStartMonitoring();
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
// when
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
// then
EXPECT_TRUE(failsafeIsMonitoring());
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
ENABLE_ARMING_FLAG(ARMED);
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeOnValidDataReceived();
// when
sysTickUptime = 0;
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(isArmingDisabled());
// simulate an Rx loss for the stage 1 duration
sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);;
failsafeOnValidDataFailed();
failsafeUpdateState();
// should be in stage 1
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
sysTickUptime ++; // exceed the stage 1 period by one tick
failsafeOnValidDataFailed(); // confirm that we still have no valid data
failsafeUpdateState();
// we should now be in stage 2, landing phase
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
// note this test follows on from the previous test
{
// given
sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
// exceed the stage 2 landing time
sysTickUptime += (failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND);
failsafeOnValidDataFailed(); // confirm that we still have no valid data
// when
failsafeUpdateState();
// expect to still be in landing phase
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
// adjust time to point just past the landing time
sysTickUptime++;
failsafeOnValidDataFailed(); // confirm that we still have no valid data
// when
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
// expect to be in monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
// let's wait 3 seconds and still get no signal
sysTickUptime += PERIOD_OF_3_SECONDS;
failsafeOnValidDataFailed(); // confirm that we still have no valid data
// when
failsafeUpdateState();
// then
// nothing should change
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
// now lets get a signal while in monitoring mode
failsafeOnValidDataReceived();
// we must clear the first delay in failsafeOnValidDataReceived(), for which 200ms 'works'
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
failsafeUpdateState();
// nothing should change
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
failsafeUpdateState();
// nothing should change
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// One tick later
sysTickUptime++;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// then
// we expect failsafe to finish and to revert to idle
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
@ -252,69 +305,98 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
// Just Disarm is when throttle is low for >10s before signal loss
// we should exit stage 1 directly into failsafe monitoring mode, and not enter landing mode
{
// given
DISABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
// and
failsafeStartMonitoring();
throttleStatus = THROTTLE_LOW; // throttle LOW to go for a failsafe just-disarm procedure
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
// when
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
ENABLE_ARMING_FLAG(ARMED); // armed from here (disarmed state has cleared throttleLowPeriod).
// set to normal initial state
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
sysTickUptime += PERIOD_OF_3_SECONDS; // 3s of normality
failsafeOnValidDataReceived(); // we have a valid signal
// when
failsafeUpdateState();
// then
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
// when
failsafeUpdateState();
// then
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// given
sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// then
// confirm that we are in idle mode
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(isArmingDisabled());
}
// run throttle_low for 11s
throttleStatus = THROTTLE_LOW; // for failsafe 'just-disarm' procedure
sysTickUptime += 11000;
throttleStatus = THROTTLE_LOW; // for failsafe 'just-disarm' procedure
failsafeOnValidDataReceived(); // set the last valid signal to now
failsafeUpdateState();
// check that we are still in idle mode
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(isArmingDisabled());
// simulate an Rx loss for the stage 1 duration
sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);;
failsafeOnValidDataFailed();
failsafeUpdateState();
// should remain in stage 1
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
sysTickUptime ++; // now we exceed stage 1
failsafeOnValidDataFailed(); // we still have no valid data
failsafeUpdateState();
// we should now be in stage 2 via Just Disarm, going direct to monitoring mode.
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// now lets get a signal while in monitoring mode
failsafeOnValidDataReceived();
// we must clear the first delay in failsafeOnValidDataReceived(), for which 200ms 'works'
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
failsafeUpdateState();
// nothing should change
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
failsafeUpdateState();
// nothing should change
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// One tick later
sysTickUptime++;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we expect failsafe to finish and to revert to idle
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(isArmingDisabled());}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
@ -324,36 +406,26 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
resetCallCounters();
failsafeStartMonitoring();
// and
// set to normal initial state
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_KILL;
activateBoxFailsafe();
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
sysTickUptime = PERIOD_RXDATA_FAILURE + 1; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
// when
failsafeUpdateState(); // kill switch handling should come first
// then
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to
failsafeOnValidDataReceived(); // cause a recovered link
deactivateBoxFailsafe();
sysTickUptime += PERIOD_OF_3_SECONDS; // 3s of normality
failsafeOnValidDataReceived(); // we have a valid signal
// when
failsafeUpdateState();
// confirm that we are in idle mode
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
// given
activateBoxFailsafe(); // activate the Kill swith
// when
failsafeUpdateState(); // should failsafe immediately the kill switch is hit
// then
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
@ -361,110 +433,254 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived(); // the link is active the whole time
// deactivate the kill switch
deactivateBoxFailsafe(); // receivingRxData is immediately true
// we should go to failsafe monitoring mode, via Landing
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// since we didn't enter stage 2, we have one rxDataRecoveryPeriod delay to deal with
// while within rxDataRecoveryPeriod in monitoring mode...
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// one tick later
sysTickUptime ++;
// when
failsafeUpdateState();
// then
// we should now have exited failsafe
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(isArmingDisabled());
}
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Drop)
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage1OrStage2Drop)
// should immediately stop motors and go to monitoring mode diretly
{
// given
// given a clean start
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
failsafeStartMonitoring();
// and
// and set initial states
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;
sysTickUptime = 0; // restart time from 0
activateBoxFailsafe();
failsafeOnValidDataFailed(); // box failsafe causes data to be invalid
failsafeOnValidDataReceived(); // we have a valid signal
sysTickUptime += 3000; // 3s of normality
// when
failsafeUpdateState(); // should activate stage2 immediately
failsafeUpdateState();
// confirm that we are in idle mode
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
// given
activateBoxFailsafe(); // activate the stage 2 Drop switch
failsafeOnValidDataFailed(); // immediate stage 2 switch sets failsafeOnValidDataFailed
// when
failsafeUpdateState(); // should activate stage2 immediately, even though signal is good
// expect to be in monitoring mode, and to have disarmed
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(isArmingDisabled());
// deactivate the failsafe switch
deactivateBoxFailsafe();
// receivingRxData is immediately true
// we go directly to failsafe monitoring mode, via Landing
// because the switch also forces rxFlightChannelsValid false, emulating real failsafe
// we have two delays to deal with before we can re-arm
// then
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time
deactivateBoxFailsafe();
failsafeOnValidDataReceived(); // inactive box failsafe gives valid data
// handle the first delay in rxDataRecoveryPeriod
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// handle the second delay
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// one tick later
sysTickUptime ++;
// when
failsafeUpdateState();
// then
// we should now have exited failsafe
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(isArmingDisabled());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
{
// given
// given a clean start
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
failsafeStartMonitoring();
// and
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfigMutable()->failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE2;
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING;
sysTickUptime = 0; // restart time from 0
activateBoxFailsafe();
failsafeOnValidDataFailed(); // box failsafe causes data to be invalid
failsafeOnValidDataReceived(); // we have a valid signal
sysTickUptime += 3000; // 3s of normality
// when
failsafeUpdateState();
// confirm that we are in idle mode
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
// given
activateBoxFailsafe(); // activate the stage 2 Drop switch
failsafeOnValidDataFailed(); // immediate stage 2 switch sets failsafeOnValidDataFailed
// when
failsafeUpdateState(); // should activate stage2 immediately
// then
// expect to immediately be in landing mode, and not disarmed
EXPECT_TRUE(failsafeIsActive()); // stick induced failsafe allows re-arming
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
// should stay in landing for failsafe_off_delay (stage 2 period) of 1s
sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
// when
failsafeUpdateState();
// then
EXPECT_TRUE(failsafeIsActive()); // stick induced failsafe allows re-arming
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
// one tick later, landing time has elapsed
sysTickUptime ++;
// when
failsafeUpdateState();
// now should be in monitoring mode, with switch holding signalReceived false
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
// given
sysTickUptime += PERIOD_OF_3_SECONDS; // hang around a bit waiting for switch change
// when
failsafeUpdateState();
// should still be in monitoring mode because switch is still forcing receivingRxData false
EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
// deactivate the failsafe switch
deactivateBoxFailsafe();
// receivingRxData is immediately true
// we go directly to failsafe monitoring mode, via Landing
// however, because the switch also forces rxFlightChannelsValid false, emulating real failsafe
// we have two delays to deal with before we can re-arm
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
// handle the first delay in rxDataRecoveryPeriod
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// and
deactivateBoxFailsafe();
failsafeOnValidDataReceived(); // inactive box failsafe gives valid data
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// handle the second delay
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// we should still be in failsafe monitoring mode
EXPECT_TRUE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
// one tick later
sysTickUptime ++;
// when
failsafeUpdateState();
// then
// we should now have exited failsafe
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
@ -474,7 +690,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
/****************************************************************************************/
//
// Additional non-stepwise tests
// Clean start
//
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
@ -486,66 +702,65 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
// and
failsafeInit();
// and
// and ** WE ARE DISARMED **
DISABLE_ARMING_FLAG(ARMED);
// when
// when failsafe starts
failsafeStartMonitoring();
// and
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
// enter stage 1 failsafe
for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
failsafeOnValidDataReceived(); // we have a valid signal
sysTickUptime += 3000; // 3s of normality
// when
failsafeUpdateState();
// then
EXPECT_TRUE(failsafeIsMonitoring());
// confirm that we are in idle mode
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(isArmingDisabled()); // arming not blocked in stage 1
// add enough time to enter stage 2
sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
// and that arming is not disabled
EXPECT_FALSE(isArmingDisabled());
// simulate an Rx loss for the stage 1 duration
sysTickUptime += (failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND);;
failsafeOnValidDataFailed();
// when
failsafeUpdateState();
// then
EXPECT_TRUE(isArmingDisabled()); // arming blocked until recovery time
EXPECT_FALSE(failsafeIsActive()); // failsafe is still not active
// within stage 1 time from a true failsafe while disarmed, stage 2 should normally not be active
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
// given valid data is received for the recovery time, allow re-arming
uint32_t sysTickTarget = sysTickUptime + PERIOD_RXDATA_RECOVERY;
for (; sysTickUptime < sysTickTarget; sysTickUptime++) {
failsafeOnValidDataReceived();
failsafeUpdateState();
// arming is disabled due to setArmingDisabled in stage 1 due to failsafeOnValidDataFailed()
EXPECT_FALSE(failsafeIsActive());
EXPECT_TRUE(isArmingDisabled());
}
// given
sysTickUptime++; // adjust time to point just past stage 1
failsafeOnValidDataFailed(); // would normally enter stage 2, but we are disarmed
failsafeUpdateState();
// and
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataReceived(); // cause link recovery
// expect that we do not enter failsafe stage 2, ie no change from above
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
// then
// the lock on the aux channels persists at fixed stage 1 values until recovery time
EXPECT_TRUE(isArmingDisabled());
// allow signal received for the recovery time
sysTickUptime += PERIOD_RXDATA_RECOVERY;
failsafeOnValidDataReceived();
failsafeUpdateState();
// no change in failsafe state (still idle)
EXPECT_FALSE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
// but now arming is possible
EXPECT_FALSE(isArmingDisabled());
}

1
src/test/unit/rx_ranges_unittest.cc
View file

@ -116,6 +116,7 @@ bool failsafeIsReceivingRxData(void) { return true; }
bool taskUpdateRxMainInProgress() { return true; }
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
uint16_t flightModeFlags = 0;
uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }

1
src/test/unit/rx_rx_unittest.cc
View file

@ -217,6 +217,7 @@ extern "C" {
bool failsafeIsActive(void) { return false; }
bool failsafeIsReceivingRxData(void) { return true; }
uint32_t failsafeFailurePeriodMs(void) { return 400; }
uint16_t flightModeFlags = 0;
uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }