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 *) {} + + +}