diff --git a/docs/Cli.md b/docs/Cli.md
index 97e5b3f3ce..cdc5930513 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -192,8 +192,8 @@ Re-apply any new defaults as desired.
| failsafe_delay | | 0 | 200 | 10 | Profile | UINT8 |
| failsafe_off_delay | | 0 | 200 | 200 | Profile | UINT8 |
| failsafe_throttle | | 1000 | 2000 | 1200 | Profile | UINT16 |
-| failsafe_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
-| failsafe_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 |
+| rx_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
+| rx_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 |
| gimbal_flags | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins | 0 | 255 | 1 | Profile | UINT8 |
| acc_hardware | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
| acc_lpf_factor | This setting controls the Low Pass Filter factor for ACC. Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter | 0 | 250 | 4 | Profile | UINT8 |
diff --git a/docs/Failsafe.md b/docs/Failsafe.md
index 30f43fc203..1901ffe293 100644
--- a/docs/Failsafe.md
+++ b/docs/Failsafe.md
@@ -85,7 +85,11 @@ When configuring the flight controller failsafe, use the following steps:
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 'failsafe_min_usec')
+b) 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.
See your receiver's documentation for direction on how to accomplish one of these.
@@ -96,6 +100,7 @@ See your receiver's documentation for direction on how to accomplish one of thes
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
@@ -120,19 +125,15 @@ Throttle level used for landing. Specify a value that causes the aircraft to de
Use standard RX usec values. See RX documentation.
-### `failsafe_min_usec`
+### `rx_min_usec`
-The shortest PWM/PPM pulse considered valid.
+The lowest channel value considered valid. e.g. PWM/PPM pulse length
-Only valid when using Parallel PWM or PPM receivers.
+### `rx_max_usec`
-### `failsafe_max_usec`
+The highest channel value considered valid. e.g. PWM/PPM pulse length
-The longest PWM/PPM pulse considered valid.
-
-Only valid when using Parallel PWM or PPM receivers.
-
-This setting helps detect when your RX stops sending any data when the RX looses signal.
+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.
diff --git a/docs/Migrating from baseflight.md b/docs/Migrating from baseflight.md
index 5c8983968d..9f8b022f17 100644
--- a/docs/Migrating from baseflight.md
+++ b/docs/Migrating from baseflight.md
@@ -55,7 +55,7 @@ Example: to use RSSI on AUX1 in Cleanflight use `set rssi_channel = 5`, since 5
### failsafe_detect_threshold
reason: improved functionality
-See `failsafe_min_usec` and `failsafe_max_usec` in Failsafe documentation.
+See `rx_min_usec` and `rx_max_usec` in Failsafe documentation.
### emfavoidance
reason: renamed to `emf_avoidance` for consistency
diff --git a/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h b/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h
index 8bf7624536..fe3ca1de14 100755
--- a/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h
+++ b/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/stm32f10x.h
@@ -15,15 +15,15 @@
* is using in the C source code, usually in main.c. This file contains:
* - Configuration section that allows to select:
* - The device used in the target application
- * - To use or not the peripheral’s drivers in application code(i.e.
- * code will be based on direct access to peripheral’s registers
+ * - To use or not the peripheral�s drivers in application code(i.e.
+ * code will be based on direct access to peripheral�s registers
* rather than drivers API), this option is controlled by
* "#define USE_STDPERIPH_DRIVER"
* - To change few application-specific parameters such as the HSE
* crystal frequency
* - Data structures and the address mapping for all peripherals
* - Peripheral's registers declarations and bits definition
- * - Macros to access peripheral’s registers hardware
+ * - Macros to access peripheral�s registers hardware
*
******************************************************************************
* @attention
diff --git a/src/main/config/config.c b/src/main/config/config.c
index fe923741fd..1ce1276ab0 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -138,7 +138,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
-static const uint8_t EEPROM_CONF_VERSION = 95;
+static const uint8_t EEPROM_CONF_VERSION = 96;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@@ -407,6 +407,9 @@ static void resetConf(void)
masterConfig.rxConfig.midrc = 1500;
masterConfig.rxConfig.mincheck = 1100;
masterConfig.rxConfig.maxcheck = 1900;
+ masterConfig.rxConfig.rx_min_usec = 985; // any of first 4 channels below this value will trigger rx loss detection
+ masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection
+
masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
@@ -474,11 +477,9 @@ static void resetConf(void)
currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
- currentProfile->failsafeConfig.failsafe_delay = 10; // 1sec
- currentProfile->failsafeConfig.failsafe_off_delay = 200; // 20sec
- currentProfile->failsafeConfig.failsafe_throttle = 1000; // default throttle off.
- currentProfile->failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe
- currentProfile->failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
+ masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec
+ masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec
+ masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off.
#ifdef USE_SERVOS
// servos
@@ -533,9 +534,9 @@ static void resetConf(void)
currentProfile->pidProfile.pidController = 3;
currentProfile->pidProfile.P8[ROLL] = 36;
currentProfile->pidProfile.P8[PITCH] = 36;
- currentProfile->failsafeConfig.failsafe_delay = 2;
- currentProfile->failsafeConfig.failsafe_off_delay = 0;
- currentProfile->failsafeConfig.failsafe_throttle = 1000;
+ masterConfig.failsafeConfig.failsafe_delay = 2;
+ masterConfig.failsafeConfig.failsafe_off_delay = 0;
+ masterConfig.failsafeConfig.failsafe_throttle = 1000;
currentControlRateProfile->rcRate8 = 130;
currentControlRateProfile->rates[FD_PITCH] = 20;
currentControlRateProfile->rates[FD_ROLL] = 20;
@@ -671,7 +672,7 @@ void activateConfig(void)
gpsUsePIDs(¤tProfile->pidProfile);
#endif
- useFailsafeConfig(¤tProfile->failsafeConfig);
+ useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs(
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 895fcf6c1a..21dff780df 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -61,6 +61,8 @@ typedef struct master_t {
rxConfig_t rxConfig;
inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
+ failsafeConfig_t failsafeConfig;
+
uint8_t retarded_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h
index 7b2ccb5103..cd68f0aaa2 100644
--- a/src/main/config/config_profile.h
+++ b/src/main/config/config_profile.h
@@ -54,9 +54,6 @@ typedef struct profile_s {
gimbalConfig_t gimbalConfig;
#endif
- // Failsafe related configuration
- failsafeConfig_t failsafeConfig;
-
#ifdef GPS
gpsProfile_t gpsProfile;
#endif
diff --git a/src/main/drivers/gpio.h b/src/main/drivers/gpio.h
index 0f6d00980b..dd08eb4ac3 100644
--- a/src/main/drivers/gpio.h
+++ b/src/main/drivers/gpio.h
@@ -106,10 +106,12 @@ typedef struct
GPIO_Speed speed;
} gpio_config_t;
+#ifndef UNIT_TEST
static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { p->BSRR = i; }
static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BRR = i; }
static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; }
static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) {return p->IDR & i; }
+#endif
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config);
void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc);
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index d8d78a4471..8edbd88e5c 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -33,6 +33,11 @@ typedef uint16_t timCCR_t;
typedef uint16_t timCCER_t;
typedef uint16_t timSR_t;
typedef uint16_t timCNT_t;
+#elif defined(UNIT_TEST)
+typedef uint32_t timCCR_t;
+typedef uint32_t timCCER_t;
+typedef uint32_t timSR_t;
+typedef uint32_t timCNT_t;
#else
# error "Unknown CPU defined"
#endif
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 85cfc10679..6ffd27dd6f 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -44,9 +44,10 @@ static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
-void failsafeReset(void)
+static void failsafeReset(void)
{
failsafeState.counter = 0;
+ failsafeState.phase = FAILSAFE_IDLE;
}
/*
@@ -58,54 +59,75 @@ void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
failsafeReset();
}
-failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig)
+void failsafeInit(rxConfig_t *intialRxConfig)
{
rxConfig = intialRxConfig;
failsafeState.events = 0;
- failsafeState.enabled = false;
+ failsafeState.monitoring = false;
- return &failsafeState;
+ return;
}
-bool failsafeIsIdle(void)
+failsafePhase_e failsafePhase()
{
- return failsafeState.counter == 0;
+ return failsafeState.phase;
}
-bool failsafeIsEnabled(void)
+#define MAX_COUNTER_VALUE_WHEN_RX_IS_RECEIVED_AFTER_RX_CYCLE 1
+
+bool failsafeIsReceivingRxData(void)
{
- return failsafeState.enabled;
+ return failsafeState.counter <= MAX_COUNTER_VALUE_WHEN_RX_IS_RECEIVED_AFTER_RX_CYCLE;
}
-void failsafeEnable(void)
+bool failsafeIsMonitoring(void)
{
- failsafeState.enabled = true;
+ return failsafeState.monitoring;
}
-bool failsafeHasTimerElapsed(void)
+bool failsafeIsActive(void)
+{
+ return failsafeState.active;
+}
+
+void failsafeStartMonitoring(void)
+{
+ failsafeState.monitoring = true;
+}
+
+static bool failsafeHasTimerElapsed(void)
{
return failsafeState.counter > (5 * failsafeConfig->failsafe_delay);
}
-bool failsafeShouldForceLanding(bool armed)
+static bool failsafeShouldForceLanding(bool armed)
{
return failsafeHasTimerElapsed() && armed;
}
-bool failsafeShouldHaveCausedLandingByNow(void)
+static bool failsafeShouldHaveCausedLandingByNow(void)
{
return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay);
}
-static void failsafeAvoidRearm(void)
+static void failsafeActivate(void)
{
- // 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 = true;
+ failsafeState.phase = FAILSAFE_LANDING;
+
+ failsafeState.events++;
}
-static void failsafeOnValidDataReceived(void)
+static void failsafeApplyControlInput(void)
+{
+ for (int i = 0; i < 3; i++) {
+ rcData[i] = rxConfig->midrc;
+ }
+ rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
+}
+
+void failsafeOnValidDataReceived(void)
{
if (failsafeState.counter > 20)
failsafeState.counter -= 20;
@@ -115,58 +137,76 @@ static void failsafeOnValidDataReceived(void)
void failsafeUpdateState(void)
{
- uint8_t i;
+ bool receivingRxData = failsafeIsReceivingRxData();
+ bool armed = ARMING_FLAG(ARMED);
- if (!failsafeHasTimerElapsed()) {
- return;
+ if (receivingRxData) {
+ failsafeState.phase = FAILSAFE_IDLE;
+ failsafeState.active = false;
}
- if (!failsafeIsEnabled()) {
- failsafeReset();
- return;
- }
- if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level
- failsafeAvoidRearm();
+ bool reprocessState;
- for (i = 0; i < 3; i++) {
- rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec)
+ do {
+ reprocessState = false;
+
+ switch (failsafeState.phase) {
+ case FAILSAFE_IDLE:
+ if (!receivingRxData && armed) {
+ failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
+
+ reprocessState = true;
+ }
+ break;
+
+ case FAILSAFE_RX_LOSS_DETECTED:
+ if (failsafeShouldForceLanding(armed)) {
+ // Stabilize, and set Throttle to specified level
+ failsafeActivate();
+
+ reprocessState = true;
+ }
+ break;
+
+ case FAILSAFE_LANDING:
+ if (armed) {
+ failsafeApplyControlInput();
+ }
+
+ if (failsafeShouldHaveCausedLandingByNow() || !armed) {
+
+ 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;
+ mwDisarm();
+ break;
+
+ default:
+ break;
}
- rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
- failsafeState.events++;
- }
+ } while (reprocessState);
- if (failsafeShouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
- mwDisarm();
- }
}
/**
- * Should be called once each time RX data is processed by the system.
+ * Should be called once when RX data is processed by the system.
*/
-void failsafeOnRxCycle(void)
+void failsafeOnRxCycleStarted(void)
{
failsafeState.counter++;
}
-
-#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
-
-// pulse duration is in micro seconds (usec)
-void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
-{
- static uint8_t goodChannelMask = 0;
-
- if (channel < 4 &&
- pulseDuration > failsafeConfig->failsafe_min_usec &&
- pulseDuration < failsafeConfig->failsafe_max_usec
- ) {
- // if signal is valid - mark channel as OK
- goodChannelMask |= (1 << channel);
- }
-
- if (goodChannelMask == REQUIRED_CHANNEL_MASK) {
- goodChannelMask = 0;
- failsafeOnValidDataReceived();
- }
-}
-
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 35a94e0a34..abe5707c87 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -23,30 +23,37 @@ 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.
- uint16_t failsafe_min_usec;
- uint16_t failsafe_max_usec;
} failsafeConfig_t;
+typedef enum {
+ FAILSAFE_IDLE = 0,
+ FAILSAFE_RX_LOSS_DETECTED,
+ FAILSAFE_LANDING,
+ FAILSAFE_LANDED
+} failsafePhase_e;
typedef struct failsafeState_s {
int16_t counter;
int16_t events;
- bool enabled;
+ bool monitoring;
+ bool active;
+ failsafePhase_e phase;
} failsafeState_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
-void failsafeEnable(void);
-void failsafeOnRxCycle(void);
-void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration);
+void failsafeStartMonitoring(void);
void failsafeUpdateState(void);
-void failsafeReset(void);
+failsafePhase_e failsafePhase();
+bool failsafeIsMonitoring(void);
+bool failsafeIsActive(void);
+bool failsafeIsReceivingRxData(void);
+
+void failsafeOnValidDataReceived(void);
+void failsafeOnRxCycleStarted(void);
+
+
-bool failsafeIsEnabled(void);
-bool failsafeIsIdle(void);
-bool failsafeHasTimerElapsed(void);
-bool failsafeShouldForceLanding(bool armed);
-bool failsafeShouldHaveCausedLandingByNow(void);
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index 93af618a43..a5c06fa7d7 100644
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -49,9 +49,9 @@ static void beep_code(char first, char second, char third, char pause);
static uint8_t toggleBeep = 0;
typedef enum {
- FAILSAFE_IDLE = 0,
- FAILSAFE_LANDING,
- FAILSAFE_FIND_ME
+ FAILSAFE_WARNING_NONE = 0,
+ FAILSAFE_WARNING_LANDING,
+ FAILSAFE_WARNING_FIND_ME
} failsafeBeeperWarnings_e;
void beepcodeInit(void)
@@ -64,7 +64,7 @@ void beepcodeUpdateState(batteryState_e batteryState)
#ifdef GPS
static uint8_t warn_noGPSfix = 0;
#endif
- static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_IDLE;
+ static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_WARNING_NONE;
//===================== BeeperOn via rcOptions =====================
if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { // unconditional beeper on via AUXn switch
@@ -74,20 +74,19 @@ void beepcodeUpdateState(batteryState_e batteryState)
}
//===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) {
- if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) {
- warn_failsafe = FAILSAFE_LANDING;
-
- if (failsafeShouldHaveCausedLandingByNow()) {
- warn_failsafe = FAILSAFE_FIND_ME;
- }
+ switch (failsafePhase()) {
+ case FAILSAFE_LANDING:
+ warn_failsafe = FAILSAFE_WARNING_LANDING;
+ break;
+ case FAILSAFE_LANDED:
+ warn_failsafe = FAILSAFE_WARNING_FIND_ME;
+ break;
+ default:
+ warn_failsafe = FAILSAFE_WARNING_NONE;
}
- if (failsafeHasTimerElapsed() && !ARMING_FLAG(ARMED)) {
- warn_failsafe = FAILSAFE_FIND_ME;
- }
-
- if (failsafeIsIdle()) {
- warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay
+ if (rxIsReceivingSignal()) {
+ warn_failsafe = FAILSAFE_WARNING_NONE;
}
}
diff --git a/src/main/io/display.c b/src/main/io/display.c
index 14953c971f..2aca4680d4 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -51,6 +51,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
+#include "flight/failsafe.h"
#ifdef GPS
#include "io/gps.h"
@@ -204,6 +205,33 @@ void updateTicker(void)
tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
}
+void updateRxStatus(void)
+{
+ i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
+ i2c_OLED_send_char(rxIsReceivingSignal() ? 'R' : '!');
+}
+
+void updateFailsafeStatus(void)
+{
+ char failsafeIndicator = '?';
+ switch (failsafePhase()) {
+ case FAILSAFE_IDLE:
+ failsafeIndicator = '-';
+ break;
+ case FAILSAFE_RX_LOSS_DETECTED:
+ failsafeIndicator = 'R';
+ break;
+ case FAILSAFE_LANDING:
+ failsafeIndicator = 'l';
+ break;
+ case FAILSAFE_LANDED:
+ failsafeIndicator = 'L';
+ break;
+ }
+ i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
+ i2c_OLED_send_char(failsafeIndicator);
+}
+
void showTitle()
{
i2c_OLED_set_line(0);
@@ -582,8 +610,11 @@ void updateDisplay(void)
#endif
}
if (!armedState) {
+ updateFailsafeStatus();
+ updateRxStatus();
updateTicker();
}
+
}
void displaySetPage(pageId_e pageId)
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index bd8cb20029..3d2e691981 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -661,7 +661,7 @@ void applyLedWarningLayer(uint8_t updateNow)
if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) {
warningFlags |= WARNING_FLAG_LOW_BATTERY;
}
- if (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed()) {
+ if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) {
warningFlags |= WARNING_FLAG_FAILSAFE;
}
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
@@ -714,6 +714,9 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState)
const ledConfig_t *ledConfig;
static const hsvColor_t *flashColor;
+ if (!rxIsReceivingSignal()) {
+ return;
+ }
if (indicatorFlashState == 0) {
flashColor = &hsv_orange;
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 4c322bae4c..0d680a7e78 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -51,6 +51,7 @@
#include "flight/pid.h"
#include "flight/navigation.h"
+#include "flight/failsafe.h"
#include "mw.h"
@@ -124,7 +125,8 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
} else {
// Disarming via ARM BOX
- if (ARMING_FLAG(ARMED)) {
+
+ if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
if (disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 1f04a983e0..0e1d4687b6 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -404,11 +404,12 @@ const clivalue_t valueTable[] = {
{ "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, 0, CONTROL_RATE_CONFIG_TPA_MAX},
{ "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX},
- { "failsafe_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_delay, 0, 200 },
- { "failsafe_off_delay", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_off_delay, 0, 200 },
- { "failsafe_throttle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
- { "failsafe_min_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
- { "failsafe_max_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
+ { "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 },
+
+ { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, 100, PWM_RANGE_MAX },
+ { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
#ifdef USE_SERVOS
{ "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index e332a4e994..f13d08f6f8 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -1003,7 +1003,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(masterConfig.escAndServoConfig.maxthrottle);
serialize16(masterConfig.escAndServoConfig.mincommand);
- serialize16(currentProfile->failsafeConfig.failsafe_throttle);
+ serialize16(masterConfig.failsafeConfig.failsafe_throttle);
#ifdef GPS
serialize8(masterConfig.gpsConfig.provider); // gps_type
@@ -1378,7 +1378,7 @@ static bool processInCommand(void)
masterConfig.escAndServoConfig.maxthrottle = read16();
masterConfig.escAndServoConfig.mincommand = read16();
- currentProfile->failsafeConfig.failsafe_throttle = read16();
+ masterConfig.failsafeConfig.failsafe_throttle = read16();
#ifdef GPS
masterConfig.gpsConfig.provider = read8(); // gps_type
diff --git a/src/main/mw.c b/src/main/mw.c
index f145f99f47..5e9bd0763a 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -509,8 +509,8 @@ void processRx(void)
if (feature(FEATURE_FAILSAFE)) {
- if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) {
- failsafeEnable();
+ if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
+ failsafeStartMonitoring();
}
failsafeUpdateState();
@@ -527,7 +527,8 @@ void processRx(void)
if (ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
&& masterConfig.auto_disarm_delay != 0
- && isUsingSticksForArming()) {
+ && isUsingSticksForArming())
+ {
if (throttleStatus == THROTTLE_LOW) {
if ((int32_t)(disarmAt - millis()) < 0) // delay is over
mwDisarm();
@@ -551,7 +552,7 @@ void processRx(void)
bool canUseHorizonMode = true;
- if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) {
+ if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
@@ -628,7 +629,7 @@ void loop(void)
static bool haveProcessedAnnexCodeOnce = false;
#endif
- updateRx();
+ updateRx(currentTime);
if (shouldProcessRx(currentTime)) {
processRx();
diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c
index 853a7bd0a3..105a30ec98 100644
--- a/src/main/rx/msp.c
+++ b/src/main/rx/msp.c
@@ -54,12 +54,10 @@ bool rxMspFrameComplete(void)
return true;
}
-bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
+void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = 8; // Limited to 8 channels due to MSP_SET_RAW_RC command.
if (callback)
*callback = rxMspReadRawRC;
-
- return true;
}
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index aa80408c01..480930af47 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -56,12 +56,19 @@ bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcRe
bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
-bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
+void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
const char rcChannelLetters[] = "AERT12345678abcdefgh";
uint16_t rssi = 0; // range: [0;1023]
+static bool rxDataReceived = false;
+static bool rxSignalReceived = false;
+static bool shouldCheckPulse = true;
+
+static uint32_t rxUpdateAt = 0;
+static uint32_t needRxSignalBefore = 0;
+
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define PPM_AND_PWM_SAMPLE_COUNT 4
@@ -70,6 +77,7 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define PULSE_MAX 2250 // maximum PWM pulse width which is considered valid
#define DELAY_50_HZ (1000000 / 50)
+#define DELAY_10_HZ (1000000 / 10)
static rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
@@ -83,7 +91,28 @@ void useRxConfig(rxConfig_t *rxConfigToUse)
rxConfig = rxConfigToUse;
}
-#define STICK_CHANNEL_COUNT 4
+#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
+
+// pulse duration is in micro seconds (usec)
+STATIC_UNIT_TESTED void rxCheckPulse(uint8_t channel, uint16_t pulseDuration)
+{
+ static uint8_t goodChannelMask = 0;
+
+ if (channel < 4 &&
+ pulseDuration >= rxConfig->rx_min_usec &&
+ pulseDuration <= rxConfig->rx_max_usec
+ ) {
+ // if signal is valid - mark channel as OK
+ goodChannelMask |= (1 << channel);
+ }
+
+ if (goodChannelMask == REQUIRED_CHANNEL_MASK) {
+ goodChannelMask = 0;
+ failsafeOnValidDataReceived();
+ rxSignalReceived = true;
+ }
+}
+
void rxInit(rxConfig_t *rxConfig)
{
@@ -179,43 +208,71 @@ uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCo
return channelToRemap;
}
-static bool rcDataReceived = false;
-
-static uint32_t rxUpdateAt = 0;
-
-
-void updateRx(void)
+bool rxIsReceivingSignal(void)
{
- rcDataReceived = false;
+ return rxSignalReceived;
+}
+
+static bool isRxDataDriven(void) {
+ return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
+}
+
+void updateRx(uint32_t currentTime)
+{
+ rxDataReceived = false;
+ shouldCheckPulse = true;
+
+ if (rxSignalReceived) {
+ if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) {
+ rxSignalReceived = false;
+ }
+ }
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
uint8_t frameStatus = serialRxFrameStatus(rxConfig);
if (frameStatus & SERIAL_RX_FRAME_COMPLETE) {
- rcDataReceived = true;
- if ((frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0 && feature(FEATURE_FAILSAFE)) {
- failsafeReset();
+ rxDataReceived = true;
+ rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0;
+ if (rxSignalReceived && feature(FEATURE_FAILSAFE)) {
+ shouldCheckPulse = false;
+
+ failsafeOnValidDataReceived();
}
+ } else {
+ shouldCheckPulse = false;
}
}
#endif
if (feature(FEATURE_RX_MSP)) {
- rcDataReceived = rxMspFrameComplete();
- if (rcDataReceived && feature(FEATURE_FAILSAFE)) {
- failsafeReset();
+ rxDataReceived = rxMspFrameComplete();
+ if (rxDataReceived) {
+
+ if (feature(FEATURE_FAILSAFE)) {
+ failsafeOnValidDataReceived();
+ }
}
}
+
+ if (feature(FEATURE_RX_SERIAL | FEATURE_RX_MSP) && rxDataReceived) {
+ needRxSignalBefore = currentTime + DELAY_10_HZ;
+ }
+
+ if (feature(FEATURE_RX_PPM)) {
+ if (isPPMDataBeingReceived()) {
+ rxSignalReceived = true;
+ needRxSignalBefore = currentTime + DELAY_10_HZ;
+ resetPPMDataReceivedState();
+ }
+ shouldCheckPulse = rxSignalReceived;
+ }
}
bool shouldProcessRx(uint32_t currentTime)
{
- return rcDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
-}
-
-static bool isRxDataDriven(void) {
- return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
+ return rxDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
}
static uint8_t rcSampleIndex = 0;
@@ -256,17 +313,6 @@ static void processRxChannels(void)
return; // rcData will have already been updated by MSP_SET_RAW_RC
}
- bool shouldCheckPulse = true;
-
- if (feature(FEATURE_FAILSAFE)) {
- if (feature(FEATURE_RX_PPM)) {
- shouldCheckPulse = isPPMDataBeingReceived();
- resetPPMDataReceivedState();
- } else {
- shouldCheckPulse = !isRxDataDriven();
- }
- }
-
for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) {
if (!rcReadRawFunc) {
@@ -279,8 +325,8 @@ static void processRxChannels(void)
// sample the channel
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
- if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
- failsafeCheckPulse(chan, sample);
+ if (shouldCheckPulse) {
+ rxCheckPulse(chan, sample);
}
// validate the range
@@ -311,9 +357,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
{
rxUpdateAt = currentTime + DELAY_50_HZ;
- if (feature(FEATURE_FAILSAFE)) {
- failsafeOnRxCycle();
- }
+ failsafeOnRxCycleStarted();
if (isRxDataDriven()) {
processDataDrivenRx();
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 8529483eb3..56389dbcb3 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -17,6 +17,8 @@
#pragma once
+#define STICK_CHANNEL_COUNT 4
+
#define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN?
#define PWM_RANGE_MIN 1000
#define PWM_RANGE_MAX 2000
@@ -79,6 +81,10 @@ typedef struct rxConfig_s {
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
+
+ uint16_t rx_min_usec;
+ uint16_t rx_max_usec;
+
} rxConfig_t;
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
@@ -94,7 +100,8 @@ void useRxConfig(rxConfig_t *rxConfigToUse);
typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
-void updateRx(void);
+void updateRx(uint32_t currentTime);
+bool rxIsReceivingSignal(void);
bool shouldProcessRx(uint32_t currentTime);
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);
diff --git a/src/test/Makefile b/src/test/Makefile
index fb5f8beb6a..5e099151ea 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -47,11 +47,13 @@ TESTS = \
battery_unittest \
flight_imu_unittest \
flight_mixer_unittest \
+ flight_failsafe_unittest \
altitude_hold_unittest \
maths_unittest \
gps_conversion_unittest \
telemetry_hott_unittest \
rc_controls_unittest \
+ rx_rx_unittest \
ledstrip_unittest \
ws2811_unittest \
encoding_unittest \
@@ -390,6 +392,30 @@ flight_mixer_unittest : \
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
+$(OBJECT_DIR)/flight/failsafe.o : \
+ $(USER_DIR)/flight/failsafe.c \
+ $(USER_DIR)/flight/failsafe.h \
+ $(GTEST_HEADERS)
+
+ @mkdir -p $(dir $@)
+ $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/failsafe.c -o $@
+
+$(OBJECT_DIR)/flight_failsafe_unittest.o : \
+ $(TEST_DIR)/flight_failsafe_unittest.cc \
+ $(USER_DIR)/flight/failsafe.h \
+ $(GTEST_HEADERS)
+
+ @mkdir -p $(dir $@)
+ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_failsafe_unittest.cc -o $@
+
+flight_failsafe_unittest : \
+ $(OBJECT_DIR)/flight/failsafe.o \
+ $(OBJECT_DIR)/flight_failsafe_unittest.o \
+ $(OBJECT_DIR)/common/maths.o \
+ $(OBJECT_DIR)/gtest_main.a
+
+ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
+
$(OBJECT_DIR)/io/serial.o : \
$(USER_DIR)/io/serial.c \
$(USER_DIR)/io/serial.h \
@@ -413,6 +439,30 @@ io_serial_unittest : \
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
+$(OBJECT_DIR)/rx/rx.o : \
+ $(USER_DIR)/rx/rx.c \
+ $(USER_DIR)/rx/rx.h \
+ $(GTEST_HEADERS)
+
+ @mkdir -p $(dir $@)
+ $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/rx/rx.c -o $@
+
+$(OBJECT_DIR)/rx_rx_unittest.o : \
+ $(TEST_DIR)/rx_rx_unittest.cc \
+ $(USER_DIR)/rx/rx.h \
+ $(GTEST_HEADERS)
+
+ @mkdir -p $(dir $@)
+ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_rx_unittest.cc -o $@
+
+rx_rx_unittest : \
+ $(OBJECT_DIR)/rx/rx.o \
+ $(OBJECT_DIR)/rx_rx_unittest.o \
+ $(OBJECT_DIR)/common/maths.o \
+ $(OBJECT_DIR)/gtest_main.a
+
+ $(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
+
test: $(TESTS)
set -e && for test in $(TESTS) ; do \
$(OBJECT_DIR)/$$test; \
diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc
new file mode 100644
index 0000000000..4a6d7e2246
--- /dev/null
+++ b/src/test/unit/flight_failsafe_unittest.cc
@@ -0,0 +1,317 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+
+#include
+
+extern "C" {
+ #include "debug.h"
+
+ #include "platform.h"
+
+ #include "common/axis.h"
+ #include "common/maths.h"
+
+ #include "config/runtime_config.h"
+
+ #include "rx/rx.h"
+ #include "flight/failsafe.h"
+
+ failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig);
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+uint32_t testFeatureMask = 0;
+
+enum {
+ COUNTER_MW_DISARM = 0,
+};
+#define CALL_COUNT_ITEM_COUNT 1
+
+static int callCounts[CALL_COUNT_ITEM_COUNT];
+
+#define CALL_COUNTER(item) (callCounts[item])
+
+void resetCallCounters(void) {
+ memset(&callCounts, 0, sizeof(callCounts));
+}
+
+#define TEST_MID_RC 1495 // something other than the default 1500 will suffice.
+
+rxConfig_t rxConfig;
+failsafeConfig_t failsafeConfig;
+
+void configureFailsafe(void)
+{
+ memset(&rxConfig, 0, sizeof(rxConfig));
+ rxConfig.midrc = TEST_MID_RC;
+
+ memset(&failsafeConfig, 0, sizeof(failsafeConfig));
+ failsafeConfig.failsafe_delay = 10; // 1 second
+ failsafeConfig.failsafe_off_delay = 50; // 5 seconds
+}
+//
+// Stepwise tests
+//
+
+TEST(FlightFailsafeTest, TestFailsafeInitialState)
+{
+ // given
+ configureFailsafe();
+ // and
+ DISABLE_ARMING_FLAG(ARMED);
+
+ // when
+ useFailsafeConfig(&failsafeConfig);
+ failsafeInit(&rxConfig);
+
+ // then
+ EXPECT_EQ(false, failsafeIsMonitoring());
+ EXPECT_EQ(false, failsafeIsActive());
+ EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+}
+
+TEST(FlightFailsafeTest, TestFailsafeStartMonitoring)
+{
+ // when
+ failsafeStartMonitoring();
+
+ // then
+ EXPECT_EQ(true, failsafeIsMonitoring());
+ EXPECT_EQ(false, failsafeIsActive());
+ EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+}
+
+TEST(FlightFailsafeTest, TestFailsafeFirstArmedCycle)
+{
+ // given
+ ENABLE_ARMING_FLAG(ARMED);
+
+ // when
+ failsafeOnRxCycleStarted();
+ failsafeOnValidDataReceived();
+
+ // and
+ failsafeUpdateState();
+
+ // then
+ EXPECT_EQ(false, failsafeIsActive());
+ 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();
+ failsafeOnValidDataReceived();
+
+ failsafeUpdateState();
+
+ // then
+ EXPECT_EQ(false, failsafeIsActive());
+ EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+ }
+}
+
+TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
+{
+
+ // given
+ ENABLE_ARMING_FLAG(ARMED);
+
+ // when
+ failsafeOnRxCycleStarted();
+ // no call to failsafeOnValidDataReceived();
+
+ failsafeUpdateState();
+
+ // then
+ EXPECT_EQ(false, failsafeIsActive());
+ EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+
+ //
+ // currently one cycle must occur (above) so that the next cycle (below) can detect the lack of an update.
+ //
+
+ // when
+ for (int i = 0; i < FAILSAFE_UPDATE_HZ - 1; 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.
+ //
+
+ // 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(1, CALL_COUNTER(COUNTER_MW_DISARM));
+ EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+
+ // given
+ DISABLE_ARMING_FLAG(ARMED);
+
+ // when
+ failsafeOnRxCycleStarted();
+ // no call to failsafeOnValidDataReceived();
+ failsafeUpdateState();
+
+ // then
+ EXPECT_EQ(false, failsafeIsActive());
+ EXPECT_EQ(FAILSAFE_LANDED, failsafePhase());
+ EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
+ EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
+
+}
+
+//
+// Additional non-stepwise tests
+//
+
+TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected)
+{
+ // given
+ configureFailsafe();
+
+ // and
+ useFailsafeConfig(&failsafeConfig);
+ failsafeInit(&rxConfig);
+
+ // and
+ DISABLE_ARMING_FLAG(ARMED);
+
+ // when
+ failsafeStartMonitoring();
+
+ // and
+ int callsToMakeToSimulateTenSeconds = FAILSAFE_UPDATE_HZ * 10;
+
+ for (int i = 0; i < callsToMakeToSimulateTenSeconds; i++) {
+ failsafeOnRxCycleStarted();
+ // no call to failsafeOnValidDataReceived();
+
+ failsafeUpdateState();
+
+ // then
+ EXPECT_EQ(true, failsafeIsMonitoring());
+ EXPECT_EQ(false, failsafeIsActive());
+ EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
+ }
+}
+
+
+// STUBS
+
+extern "C" {
+int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+uint8_t armingFlags;
+
+void delay(uint32_t) {}
+
+bool feature(uint32_t mask) {
+ return (mask & testFeatureMask);
+}
+
+void mwDisarm(void) {
+ callCounts[COUNTER_MW_DISARM]++;
+}
+
+}
diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc
index 739982a6ca..c98ace2fa7 100644
--- a/src/test/unit/ledstrip_unittest.cc
+++ b/src/test/unit/ledstrip_unittest.cc
@@ -418,6 +418,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return 0;
}
-bool failsafeHasTimerElapsed(void) { return false; }
+bool failsafeIsActive() { return false; }
+bool rxIsReceivingSignal() { return true; }
}
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index 3d0a20faf5..7841b19f9a 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -27,3 +27,21 @@
#define SERIAL_PORT_COUNT 4
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6
+
+typedef enum
+{
+ Mode_TEST = 0x0,
+} GPIO_Mode;
+
+typedef struct
+{
+ void* test;
+} GPIO_TypeDef;
+
+typedef struct
+{
+ void* test;
+} TIM_TypeDef;
+
+typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
+
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index 4e8e33e513..209110ec0c 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -711,6 +711,9 @@ void mwDisarm(void) {}
void displayDisablePageCycling() {}
void displayEnablePageCycling() {}
+bool failsafeIsActive() { return false; }
+bool rxIsReceivingSignal() { return true; }
+
uint8_t getCurrentControlRateProfile(void) {
return 0;
}
diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc
new file mode 100644
index 0000000000..dff8650653
--- /dev/null
+++ b/src/test/unit/rx_rx_unittest.cc
@@ -0,0 +1,162 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+
+#include
+
+extern "C" {
+ #include "platform.h"
+
+ #include "rx/rx.h"
+
+ void rxInit(rxConfig_t *rxConfig);
+ void rxCheckPulse(uint8_t channel, uint16_t pulseDuration);
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+enum {
+ COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED = 0,
+};
+#define CALL_COUNT_ITEM_COUNT 1
+
+static int callCounts[CALL_COUNT_ITEM_COUNT];
+
+#define CALL_COUNTER(item) (callCounts[item])
+
+void resetCallCounters(void) {
+ memset(&callCounts, 0, sizeof(callCounts));
+}
+
+typedef struct testData_s {
+ bool isPPMDataBeingReceived;
+} testData_t;
+
+static testData_t testData;
+
+TEST(RxTest, TestFailsafeInformedOfValidData)
+{
+ // given
+ resetCallCounters();
+ memset(&testData, 0, sizeof(testData));
+
+ // and
+ rxConfig_t rxConfig;
+
+ memset(&rxConfig, 0, sizeof(rxConfig));
+ rxConfig.rx_min_usec = 1000;
+ rxConfig.rx_max_usec = 2000;
+
+ // when
+ rxInit(&rxConfig);
+
+ for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
+ rxCheckPulse(channelIndex, 1500);
+ }
+
+ // then
+ EXPECT_EQ(1, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED));
+}
+
+TEST(RxTest, TestFailsafeNotInformedOfValidDataWhenStickChannelsAreBad)
+{
+ // given
+ memset(&testData, 0, sizeof(testData));
+
+ // and
+ rxConfig_t rxConfig;
+
+ memset(&rxConfig, 0, sizeof(rxConfig));
+ rxConfig.rx_min_usec = 1000;
+ rxConfig.rx_max_usec = 2000;
+
+ // and
+ uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+ memset(&channelPulses, 1500, sizeof(channelPulses));
+
+ // and
+ rxInit(&rxConfig);
+
+ // and
+
+ for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) {
+
+ // given
+ resetCallCounters();
+
+ for (uint8_t channelIndex = 0; channelIndex < STICK_CHANNEL_COUNT; channelIndex++) {
+ channelPulses[stickChannelIndex] = rxConfig.rx_min_usec;
+ }
+ channelPulses[stickChannelIndex] = rxConfig.rx_min_usec - 1;
+
+ // when
+ for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
+ rxCheckPulse(channelIndex, channelPulses[channelIndex]);
+ }
+
+ // then
+ EXPECT_EQ(0, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED));
+
+ // given
+ resetCallCounters();
+
+ for (uint8_t channelIndex = 0; channelIndex < STICK_CHANNEL_COUNT; channelIndex++) {
+ channelPulses[stickChannelIndex] = rxConfig.rx_max_usec;
+ }
+ channelPulses[stickChannelIndex] = rxConfig.rx_max_usec + 1;
+
+ // when
+ for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
+ rxCheckPulse(channelIndex, channelPulses[channelIndex]);
+ }
+
+ // then
+ EXPECT_EQ(0, CALL_COUNTER(COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED));
+ }
+}
+
+
+// STUBS
+
+extern "C" {
+ void failsafeOnRxCycleStarted() {}
+ void failsafeOnValidDataReceived() {
+ callCounts[COUNTER_FAILSAFE_ON_VALID_DATA_RECEIVED]++;
+ }
+
+ bool feature(uint32_t mask) {
+ UNUSED(mask);
+ return false;
+ }
+
+ bool isPPMDataBeingReceived(void) {
+ return testData.isPPMDataBeingReceived;
+ }
+
+ void resetPPMDataReceivedState(void) {}
+
+ bool rxMspFrameComplete(void) { return false; }
+
+ void rxMspInit(rxConfig_t *, rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
+
+ void rxPwmInit(rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
+
+
+}