diff --git a/docs/Failsafe.md b/docs/Failsafe.md index b1ae95b06..4084aad4e 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -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. diff --git a/src/main/config/config.c b/src/main/config/config.c index 6764ad4ab..4550fd1aa 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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 diff --git a/src/main/config/config.h b/src/main/config/config.h index fabc135da..0de14d60d 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -73,3 +73,4 @@ void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); +uint16_t getCurrentMinthrottle(void); diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 090a57ca7..c5a1bff6a 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -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; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a1090883a..f02b318f8 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -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++; -} diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index abe5707c8..4e2c4b52d 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -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); diff --git a/src/main/io/display.c b/src/main/io/display.c index f921d24cf..e7e0a4d66 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -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); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 024933426..8be258648 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -47,6 +47,7 @@ typedef enum { BOXSERVO2, BOXSERVO3, BOXBLACKBOX, + BOXFAILSAFE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960a..086349736 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ce80091e0..b6e1c1578 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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) diff --git a/src/main/main.c b/src/main/main.c index a9e48be90..0357fa68c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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); diff --git a/src/main/mw.c b/src/main/mw.c index 5a376f8f2..d9c0d7b4a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 78016c43b..1af91509d 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -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) diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 66981e605..cee78f665 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -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; +} + } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 3c5bebccb..073fed324 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -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 diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 021a2ad90..d559d5269 100755 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -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) { } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index c7cb0ad2c..db3e0f2f1 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -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) {