rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updates and feature additions to failsafe system. 

- Added failsafe flightmode and rc control box.

To make failsafe procedure a separate flight mode and make it possible
to trigger failsafe with an AUX switch.

- Failsafe mode is activated when failsafe is active.

RC link lost is simulated with the failsafe AUX switch.
When NOT armed: failsafe switch to failsafe mode is shown in GUI (mode
tab).

- Activate failsafe mode with AUX switch.

- Prevent arming when failsafe via AUX switch is active (safety issue).

- Make failsafe disarm if motors armed and throttle was LOW (2D & 3D)
for `failsafe_throttle_low_delay` time (__JustDisarmEvent__).

Applied code changes to effectively add pull request: Make failsafe
disarm if motors armed and throttle low #717.

- Use failsafeIsMonitoring() to actually start monitoring.

- Added `failsafe_kill_switch` to code.

When set to 1 (0 is default), the failsafe switch will instantly disarm
(__KillswitchEvent__) instead of executing the landings procedure.
Arming is NOT locked after
this, so the craft could be re-armed if needed.
This is intended for racing quads where damage and danger must be
minimized in case of a pilot error.

- Added `failsafe_throttle_low_delay`, adapted documentation.

Used to adjust the time throttle level must have been LOW
to _only disarm_ instead of _full failsafe procedure_
(__JustDisarmEvent__).

- Updated the failsafe documentation.

- Re-enable arming at end of failsafe procedure.

At the end of a handled failsafe event, that means: auto-landing,
__JustDisarmEvent__ or __KillswitchEvent__, the RX link is monitored for
valid data.
Monitoring is a part of the failsafe handling, which means the craft is
still in failsafe mode while this is done.
Arming is re-enabled (allowed) when there is a valid RX link for more
then XX seconds, where XX depends on the handled event like this:
1. XX = 30 seconds after auto landing.
2. XX = 3 seconds after __JustDisarmEvent__.
3. XX = 0 seconds after __KillswitchEvent__.

NOTE: When armed via an AUX switch, you will have to switch to the
disarmed position at the very end to be able to re-arm.
The failsafe mode will not end until you do.

- __KillswitchEvent__ has now priority over __JustDisarmEvent__

- Apply rxfail values instantly when failsafe switch is ON

- Added missing cases to display.c

Show M when failsafe is monitoring for RX recovery (AND disarming when
armed with a switch).

===

Reworked the code from counter-based to time-based.

- AUX failsafe switch now has identical behavior to RX loss.

- Added RX failure and RX recovery timing.

- __KillswitchEvent__ skips RX failure detection delay (direct disarm).

===

[UNIT TESTS]

Adapted failsafe related unittests from counter-based to time-based

- Added failsafeOnValidDataFailed() to some tests

- Removed duplicate test setup from rc_controls_unittest.cc

- Removed magic numbers from rx_ranges_unittest.cc and rx_rx_unittest.cc

- Reworked all test-cases for flight_failsafe_unittest.cc
This commit is contained in:
ProDrone 2015-05-05 16:11:05 +02:00
parent f7530df974
commit f0681de53d
17 changed files with 542 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
docs/Failsafe.md
View File

@ -5,77 +5,39 @@ 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.
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 __rx-failsafe-state__.
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 from your receiver.
Flight controller based failsafe is where the flight controller attempts to detect signal loss and/or the __rx-failsafe-state__ of your receiver and upon detection goes to __fc-failsafe-state__. The idea is that the flight controller starts using substitutes for all controls, which are set by you, using the CLI command `rxfail` (see `rxfail` document) 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
The failsafe system is not activated until 5 seconds after the flight controller boots up. This is to prevent failsafe from activating, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data. Note that you need to activate the 'FAILSAFE' feature in order to activate failsafe on flight controller.
The __failsafe-auto-landing__ system is not activated until 5 seconds after the flight controller boots up. This is to prevent __failsafe-auto-landing__ from activating, as in the case of TX/RX gear with long bind procedures, before the RX sends out valid data.
After the failsafe has forced a landing, the flight controller cannot be armed and has to be reset.
The failsafe system attempts to detect when your receiver loses signal. It then attempts to prevent your aircraft from flying away uncontrollably by enabling an auto-level mode and setting the throttle that should allow the craft to come to a safer landing.
The __failsafe-detection__ system attempts to detect when your receiver loses signal *continuously* but the __failsafe-auto-landing__ starts *only when your craft is __armed__*. It then attempts to prevent your aircraft from flying away uncontrollably by enabling an auto-level mode and setting the throttle that should allow the craft to come to a safer landing.
The failsafe is activated when:
**The failsafe is activated when the craft is armed and either:**
Either:
* The control (stick) channels do not have valid signals AND the failsafe guard time specified by `failsafe_delay` has elapsed.
* A transmitter switch that is configured to control the failsafe mode is switched ON (and 'failsafe_kill_switch' is set to 0).
a) no valid channel data from the RX is received via Serial RX.
Failsafe intervention will be aborted when it was due to:
b) the first 4 channels do not have valid signals.
And when:
c) the failsafe guard time specified by `failsafe_delay` has elapsed.
* 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_kill_switch' is set to 0).
Note that:
* At the end of a failsafe intervention, the flight controller will be disarmed and re-arming will be locked. From that moment on it is no longer possible to abort or re-arm and the flight controller has to be reset.
* When 'failsafe_kill_switch' is set to 1 and the rc switch configured for failsafe is set to ON, the craft is instantly disarmed (but re-arming is not locked). Similar effect can be achieved by setting 'failsafe_throttle' to 1000 and 'failsafe_off_delay' to 0 (but arming is locked).
* Prior to starting a failsafe 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.
d) The failsafe system will be activated regardless of current throttle position.
e) 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.
### 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.
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
@ -83,27 +45,27 @@ When configuring the flight controller failsafe, use the following steps:
1. Configure your receiver to do one of the following:
a) Upon signal loss, send no signal/pulses over the channels
b) Send an invalid signal over the channels (for example, send values lower than 'rx_min_usec')
* 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
c) 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.
* 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).
4. Enable 'FAILSAFE' feature in Cleanflight GUI or via CLI using `feature FAILSAFE`
These are the basic steps for flight controller failsafe configuration; see Failsafe Settings below for additional settings that may be changed.
##Failsafe Settings
## Failsafe Settings
Failsafe delays are configured in 0.1 second steps.
@ -123,5 +85,63 @@ Delay after failsafe activates before motors finally turn off. This is the amou
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.
Use standard RX usec values. See RX documentation.
### `failsafe_kill_switch`
Configure the rc switched failsafe action: the same action as when the rc link is lost (set to 0) or disarms instantly (set to 1). Also see above.
### `failsafe_throttle_low_delay`
Time throttle level must have been below 'min_throttle' to _only disarm_ instead of _full failsafe procedure_.
Use standard RX usec values. See Rx documentation.
### `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.

7
src/main/config/config.c
View File

@ -348,6 +348,11 @@ static void setControlRateProfile(uint8_t profileIndex)
currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex];
}
uint16_t getCurrentMinthrottle(void)
{
return masterConfig.escAndServoConfig.minthrottle;
}
// Default settings
static void resetConf(void)
{
@ -483,6 +488,8 @@ static void resetConf(void)
masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec
masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec
masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off.
masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss
masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition
#ifdef USE_SERVOS
// servos

1
src/main/config/config.h
View File

@ -73,3 +73,4 @@ void changeControlRateProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);
uint16_t getCurrentMinthrottle(void);

1
src/main/config/runtime_config.h
View File

@ -41,6 +41,7 @@ typedef enum {
AUTOTUNE_MODE = (1 << 7),
PASSTHRU_MODE = (1 << 8),
SONAR_MODE = (1 << 9),
FAILSAFE_MODE = (1 << 10),
} flightModeFlags_e;
extern uint16_t flightModeFlags;

179
src/main/flight/failsafe.c
View File

@ -23,10 +23,12 @@
#include "common/axis.h"
#include "rx/rx.h"
#include "drivers/system.h"
#include "io/beeper.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "flight/failsafe.h"
@ -47,10 +49,19 @@ static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC
static void failsafeReset(void)
{
failsafeState.counter = 0;
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND;
failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0;
failsafeState.landingShouldBeFinishedAt = 0;
failsafeState.receivingRxDataPeriod = 0;
failsafeState.receivingRxDataPeriodPreset = 0;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
/*
@ -62,10 +73,11 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
failsafeReset();
}
void failsafeInit(rxConfig_t *intialRxConfig)
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
{
rxConfig = intialRxConfig;
deadband3dThrottle = deadband3d_throttle;
failsafeState.events = 0;
failsafeState.monitoring = false;
@ -77,13 +89,6 @@ failsafePhase_e failsafePhase()
return failsafeState.phase;
}
#define FAILSAFE_COUNTER_THRESHOLD 20
bool failsafeIsReceivingRxData(void)
{
return failsafeState.counter <= FAILSAFE_COUNTER_THRESHOLD;
}
bool failsafeIsMonitoring(void)
{
return failsafeState.monitoring;
@ -99,25 +104,17 @@ void failsafeStartMonitoring(void)
failsafeState.monitoring = true;
}
static bool failsafeHasTimerElapsed(void)
{
return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
}
static bool failsafeShouldForceLanding(bool armed)
{
return failsafeHasTimerElapsed() && armed;
}
static bool failsafeShouldHaveCausedLandingByNow(void)
{
return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
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++;
}
@ -130,27 +127,41 @@ static void failsafeApplyControlInput(void)
rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
}
bool failsafeIsReceivingRxData(void)
{
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
}
void failsafeOnValidDataReceived(void)
{
if (failsafeState.counter > FAILSAFE_COUNTER_THRESHOLD)
failsafeState.counter -= FAILSAFE_COUNTER_THRESHOLD;
else
failsafeState.counter = 0;
failsafeState.validRxDataReceivedAt = millis();
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
}
}
void failsafeOnValidDataFailed(void)
{
failsafeState.validRxDataFailedAt = millis();
if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
}
void failsafeUpdateState(void)
{
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (receivingRxData) {
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.active = false;
} else {
beeperMode = BEEPER_RX_LOST;
if (!failsafeIsMonitoring()) {
return;
}
bool receivingRxData = failsafeIsReceivingRxData();
bool armed = ARMING_FLAG(ARMED);
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (!receivingRxData) {
beeperMode = BEEPER_RX_LOST;
}
bool reprocessState;
@ -159,50 +170,100 @@ void failsafeUpdateState(void)
switch (failsafeState.phase) {
case FAILSAFE_IDLE:
if (!receivingRxData && armed) {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) {
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
} else {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
}
reprocessState = true;
}
} else {
// When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
if (failsafeSwitchIsOn) {
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
} else {
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
}
// Throttle low period expired (= low long enough for JustDisarm)
failsafeState.throttleLowPeriod = 0;
}
break;
case FAILSAFE_RX_LOSS_DETECTED:
if (failsafeShouldForceLanding(armed)) {
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
// Stabilize, and set Throttle to specified level
failsafeActivate();
reprocessState = true;
}
reprocessState = true;
break;
case FAILSAFE_LANDING:
if (receivingRxData) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
if (armed) {
failsafeApplyControlInput();
beeperMode = BEEPER_RX_LOST_LANDING;
}
if (failsafeShouldHaveCausedLandingByNow() || !armed) {
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true;
}
break;
case FAILSAFE_LANDED:
if (!armed) {
break;
}
// This will prevent the automatic rearm if failsafe shuts it down and prevents
// to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
ENABLE_ARMING_FLAG(PREVENT_ARMING);
failsafeState.active = false;
ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link
mwDisarm();
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
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.
if (receivingRxData) {
if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now, when arming via ARM switch, it must be OFF first
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
DISABLE_ARMING_FLAG(PREVENT_ARMING);
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}
}
} else {
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
}
break;
case 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;
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
reprocessState = true;
break;
default:
@ -214,11 +275,3 @@ void failsafeUpdateState(void)
beeper(beeperMode);
}
}
/**
* Should be called once when RX data is processed by the system.
*/
void failsafeOnRxCycleStarted(void)
{
failsafeState.counter++;
}

30
src/main/flight/failsafe.h
View File

@ -18,26 +18,50 @@
#pragma once
#define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
#define MILLIS_PER_TENTH_SECOND 100
#define MILLIS_PER_SECOND 1000
#define PERIOD_OF_1_SECONDS 1 * MILLIS_PER_SECOND
#define PERIOD_OF_3_SECONDS 3 * MILLIS_PER_SECOND
#define PERIOD_OF_30_SECONDS 30 * MILLIS_PER_SECOND
#define PERIOD_RXDATA_FAILURE 200 // millis
#define PERIOD_RXDATA_RECOVERY 200 // millis
typedef struct failsafeConfig_s {
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint8_t failsafe_kill_switch; // failsafe switch action is 0: identical to rc link loss, 1: disarms instantly
uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure".
} failsafeConfig_t;
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_RX_LOSS_DETECTED,
FAILSAFE_LANDING,
FAILSAFE_LANDED
FAILSAFE_LANDED,
FAILSAFE_RX_LOSS_MONITORING,
FAILSAFE_RX_LOSS_RECOVERED
} failsafePhase_e;
typedef enum {
FAILSAFE_RXLINK_DOWN = 0,
FAILSAFE_RXLINK_UP
} failsafeRxLinkState_e;
typedef struct failsafeState_s {
int16_t counter;
int16_t events;
bool monitoring;
bool active;
uint32_t rxDataFailurePeriod;
uint32_t validRxDataReceivedAt;
uint32_t validRxDataFailedAt;
uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period
uint32_t landingShouldBeFinishedAt;
uint32_t receivingRxDataPeriod; // period for the required period of valid rxData
uint32_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
failsafePhase_e phase;
failsafeRxLinkState_e rxLinkState;
} failsafeState_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
@ -51,7 +75,7 @@ bool failsafeIsActive(void);
bool failsafeIsReceivingRxData(void);
void failsafeOnValidDataReceived(void);
void failsafeOnRxCycleStarted(void);
void failsafeOnValidDataFailed(void);

6
src/main/io/display.c
View File

@ -231,6 +231,12 @@ void updateFailsafeStatus(void)
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
case FAILSAFE_RX_LOSS_MONITORING:
failsafeIndicator = 'M';
break;
case FAILSAFE_RX_LOSS_RECOVERED:
failsafeIndicator = 'r';
break;
}
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(failsafeIndicator);

1
src/main/io/rc_controls.h
View File

@ -47,6 +47,7 @@ typedef enum {
BOXSERVO2,
BOXSERVO3,
BOXBLACKBOX,
BOXFAILSAFE,
CHECKBOX_ITEM_COUNT
} boxId_e;

2
src/main/io/serial_cli.c
View File

@ -433,6 +433,8 @@ const clivalue_t valueTable[] = {
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_kill_switch, 0, 1 },
{ "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle_low_delay, 0, 300 },
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, PWM_PULSE_MIN, PWM_PULSE_MAX },

10
src/main/io/serial_msp.c
View File

@ -131,7 +131,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
@ -346,6 +346,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -698,6 +699,10 @@ void mspInit(serialConfig_t *serialConfig)
}
#endif
if (feature(FEATURE_FAILSAFE)){
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
}
memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig);
}
@ -819,7 +824,8 @@ static bool processOutCommand(uint8_t cmdMSP)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));
if (flag)

4
src/main/main.c
View File

@ -104,7 +104,7 @@ void telemetryInit(void);
void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled);
void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig);
void failsafeInit(rxConfig_t *intialRxConfig);
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
#ifdef USE_SERVOS
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers);
@ -404,7 +404,7 @@ void init(void)
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig);
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);

3
src/main/mw.c
View File

@ -342,6 +342,9 @@ void mwArm(void)
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
headFreeModeHold = heading;

6
src/main/rx/rx.c
View File

@ -441,10 +441,11 @@ static void detectAndApplySignalLossBehaviour(void)
rxFlightChannelsValid = rxHaveValidFlightChannels();
if (rxFlightChannelsValid) {
if ((rxFlightChannelsValid) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
failsafeOnValidDataReceived();
} else {
rxSignalReceived = false;
failsafeOnValidDataFailed();
for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
rcData[channel] = getRxfailValue(channel);
@ -457,8 +458,6 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
{
rxUpdateAt = currentTime + DELAY_50_HZ;
failsafeOnRxCycleStarted();
if (!feature(FEATURE_RX_MSP)) {
// rcData will have already been updated by MSP_SET_RAW_RC
@ -469,7 +468,6 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
readRxChannelsApplyRanges();
detectAndApplySignalLossBehaviour();
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig)

362
src/test/unit/flight_failsafe_unittest.cc
View File

@ -31,6 +31,7 @@ extern "C" {
#include "config/runtime_config.h"
#include "io/beeper.h"
#include "io/rc_controls.h"
#include "rx/rx.h"
#include "flight/failsafe.h"
@ -42,6 +43,9 @@ extern "C" {
#include "gtest/gtest.h"
uint32_t testFeatureMask = 0;
uint16_t flightModeFlags = 0;
uint16_t testMinThrottle = 0;
throttleStatus_e throttleStatus = THROTTLE_HIGH;
enum {
COUNTER_MW_DISARM = 0,
@ -56,24 +60,34 @@ void resetCallCounters(void) {
memset(&callCounts, 0, sizeof(callCounts));
}
#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
#define TEST_MIN_CHECK 1100;
#define PERIOD_OF_10_SCONDS 10000
#define DE_ACTIVATE_ALL_BOXES 0
rxConfig_t rxConfig;
failsafeConfig_t failsafeConfig;
uint32_t sysTickUptime;
void configureFailsafe(void)
{
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.midrc = TEST_MID_RC;
rxConfig.mincheck = TEST_MIN_CHECK;
memset(&failsafeConfig, 0, sizeof(failsafeConfig));
failsafeConfig.failsafe_delay = 10; // 1 second
failsafeConfig.failsafe_off_delay = 50; // 5 seconds
failsafeConfig.failsafe_kill_switch = false;
failsafeConfig.failsafe_throttle = 1200;
failsafeConfig.failsafe_throttle_low_delay = 50; // 5 seconds
sysTickUptime = 0;
}
//
// Stepwise tests
//
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeInitialState)
{
// given
@ -91,6 +105,7 @@ TEST(FlightFailsafeTest, TestFailsafeInitialState)
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
{
// when
@ -102,14 +117,16 @@ TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
{
// given
ENABLE_ARMING_FLAG(ARMED);
// when
failsafeOnRxCycleStarted();
failsafeOnValidDataReceived();
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
// and
failsafeUpdateState();
@ -119,47 +136,11 @@ TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
/*
* FIXME failsafe assumes that calls to failsafeUpdateState() happen at a set frequency (50hz)
* but that is NOT the case when using a RX_SERIAL or RX_MSP as in that case the rx data is processed as soon
* as it arrives which may be more or less frequent.
*
* Since the failsafe uses a counter the counter would not be updated at the same frequency that the maths
* in the failsafe code is expecting the failsafe will either be triggered to early or too late when using
* RX_SERIAL or RX_MSP.
*
* uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
*
* static bool failsafeHasTimerElapsed(void)
* {
* return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
* }
*
* static bool failsafeShouldHaveCausedLandingByNow(void)
* {
* return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
* }
*
* void failsafeOnValidDataReceived(void)
* {
* if (failsafeState.counter > 20)
* failsafeState.counter -= 20;
* else
* failsafeState.counter = 0;
* }
*
* 1000ms / 50hz = 20
*/
#define FAILSAFE_UPDATE_HZ 50
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
{
// when
int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
failsafeOnRxCycleStarted();
for (sysTickUptime = 0; sysTickUptime < PERIOD_OF_10_SCONDS; sysTickUptime++) {
failsafeOnValidDataReceived();
failsafeUpdateState();
@ -170,108 +151,213 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenReceivingData)
}
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
{
// given
ENABLE_ARMING_FLAG(ARMED);
//
// currently 20 cycles must occur before the lack of an update triggers RX loss detection.
//
// FIXME see comments about RX_SERIAL/RX_MSP above, the test should likely deal with time rather than counters.
int failsafeCounterThreshold = 20;
// and
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 (int i = 0; i < failsafeCounterThreshold; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig.failsafe_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_FAILURE); sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(false, failsafeIsActive());
}
// when
for (int i = 0; i < FAILSAFE_UPDATE_HZ - failsafeCounterThreshold; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_RX_LOSS_DETECTED, failsafePhase());
EXPECT_EQ(false, failsafeIsActive());
}
//
// one more cycle currently needed before the counter is re-checked.
//
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
// when
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
EXPECT_EQ(true, failsafeIsActive());
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
{
// given
sysTickUptime += failsafeConfig.failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
sysTickUptime++;
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
}
TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
{
// given
int callsToMakeToSimulateFiveSeconds = FAILSAFE_UPDATE_HZ * 5;
// when
for (int i = 0; i < callsToMakeToSimulateFiveSeconds - 1; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
EXPECT_EQ(true, failsafeIsActive());
}
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
DISABLE_ARMING_FLAG(ARMED);
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
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived();
// when
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
{
// given
DISABLE_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_EQ(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).
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// 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
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
{
// given
ENABLE_ARMING_FLAG(ARMED);
resetCallCounters();
failsafeStartMonitoring();
// and
throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure
failsafeConfig.failsafe_kill_switch = 1; // configure AUX switch as kill switch
ACTIVATE_RC_MODE(BOXFAILSAFE); // and activate it
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_EQ(true, failsafeIsActive());
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
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
rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be off (kill switch)
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
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
failsafeOnValidDataReceived();
// when
failsafeUpdateState();
// then
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
/****************************************************************************************/
//
// Additional non-stepwise tests
//
/****************************************************************************************/
TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
{
// given
@ -288,28 +374,58 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
failsafeStartMonitoring();
// and
int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
sysTickUptime = 0; // restart time from 0
failsafeOnValidDataReceived(); // set last valid sample at current time
for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
failsafeOnRxCycleStarted();
// no call to failsafeOnValidDataReceived();
// when
for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
failsafeOnValidDataFailed();
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
}
}
// given
sysTickUptime++; // adjust time to point just past the failure time to
failsafeOnValidDataFailed(); // cause a lost link
// when
failsafeUpdateState();
// then
EXPECT_EQ(true, failsafeIsMonitoring());
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
}
// STUBS
extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t armingFlags;
int16_t rcCommand[4];
uint32_t rcModeActivationMask;
int16_t debug[DEBUG16_VALUE_COUNT];
bool isUsingSticksToArm = true;
// Return system uptime in milliseconds (rollover in 49 days)
uint32_t millis(void)
{
return sysTickUptime;
}
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
UNUSED(rxConfig);
UNUSED(deadband3d_throttle);
return throttleStatus;
}
void delay(uint32_t) {}
@ -325,4 +441,26 @@ void beeper(beeperMode_e mode) {
UNUSED(mode);
}
uint16_t enableFlightMode(flightModeFlags_e mask)
{
flightModeFlags |= (mask);
return flightModeFlags;
}
uint16_t disableFlightMode(flightModeFlags_e mask)
{
flightModeFlags &= ~(mask);
return flightModeFlags;
}
uint16_t getCurrentMinthrottle(void)
{
return testMinThrottle;
}
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;
}
}

20
src/test/unit/rc_controls_unittest.cc
View File

@ -298,6 +298,26 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle)
TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0,0,0},
.dynThrPID = 0,
.rcYawExpo8 = 0,
.tpa_breakpoint = 0
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
// and
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig);
// and

8
src/test/unit/rx_ranges_unittest.cc
View File

@ -31,7 +31,11 @@ extern "C" {
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DE_ACTIVATE_ALL_BOXES 0
extern "C" {
uint32_t rcModeActivationMask;
extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfiguration_t range);
}
@ -39,6 +43,8 @@ extern uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig
TEST(RxChannelRangeTest, TestRxChannelRanges)
{
rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF
// No signal, special condition
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1000, 2000)), 0);
EXPECT_EQ(applyRxChannelRangeConfiguraton(0, RANGE_CONFIGURATION(1300, 1700)), 0);
@ -184,7 +190,7 @@ void failsafeOnValidDataReceived(void)
{
}
void failsafeOnRxCycleStarted(void)
void failsafeOnValidDataFailed(void)
{
}

7
src/test/unit/rx_rx_unittest.cc
View File

@ -25,6 +25,8 @@ extern "C" {
#include "rx/rx.h"
uint32_t rcModeActivationMask;
void rxInit(rxConfig_t *rxConfig);
void rxResetFlightChannelStatus(void);
bool rxHaveValidFlightChannels(void);
@ -35,6 +37,8 @@ extern "C" {
#include "unittest_macros.h"
#include "gtest/gtest.h"
#define DE_ACTIVATE_ALL_BOXES 0
typedef struct testData_s {
bool isPPMDataBeingReceived;
bool isPWMDataBeingReceived;
@ -46,6 +50,7 @@ TEST(RxTest, TestValidFlightChannels)
{
// given
memset(&testData, 0, sizeof(testData));
rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF
// and
rxConfig_t rxConfig;
@ -131,7 +136,7 @@ TEST(RxTest, TestInvalidFlightChannels)
// STUBS
extern "C" {
void failsafeOnRxCycleStarted() {}
void failsafeOnValidDataFailed() {}
void failsafeOnValidDataReceived() {}
bool feature(uint32_t mask) {