From d82956317944ff9d12dfacdcc3496330606a2749 Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Tue, 9 Apr 2019 06:30:35 +0200 Subject: [PATCH 1/4] GPS Rescue procedure can be aborted by moving sticks, with a configurable delay after recovering rx --- src/main/cli/settings.c | 2 ++ src/main/fc/core.c | 5 +++-- src/main/fc/core.h | 5 +++++ src/main/flight/failsafe.c | 20 +++++++++++++------- src/main/flight/failsafe.h | 8 +++++--- src/test/Makefile | 3 +++ src/test/unit/flight_failsafe_unittest.cc | 5 +++++ 7 files changed, 36 insertions(+), 12 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 683213a42b..343201d7a7 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -746,6 +746,8 @@ const clivalue_t valueTable[] = { { "failsafe_switch_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE_SWITCH_MODE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_switch_mode) }, { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 300 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle_low_delay) }, { "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_procedure) }, + { "failsafe_recovery_delay", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_recovery_delay) }, + { "failsafe_stick_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_stick_threshold) }, // PG_BOARDALIGNMENT_CONFIG { "align_board_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -180, 360 }, PG_BOARD_ALIGNMENT, offsetof(boardAlignment_t, rollDegrees) }, diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 85d33fb75d..b6e81373bf 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -579,7 +579,7 @@ static bool canUpdateVTX(void) } #endif -#ifdef USE_RUNAWAY_TAKEOFF +#if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE) // determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here. bool areSticksActive(uint8_t stickPercentLimit) { @@ -601,8 +601,9 @@ bool areSticksActive(uint8_t stickPercentLimit) } return false; } +#endif - +#ifdef USE_RUNAWAY_TAKEOFF // allow temporarily disabling runaway takeoff prevention if we are connected // to the configurator and the ARMING_DISABLED_MSP flag is cleared. void runawayTakeoffTemporaryDisable(uint8_t disableFlag) diff --git a/src/main/fc/core.h b/src/main/fc/core.h index 20f92a44d8..2fa7770afe 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -67,6 +67,11 @@ void updateArmingStatus(void); void taskMainPidLoop(timeUs_t currentTimeUs); +#if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE) +// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here. +bool areSticksActive(uint8_t stickPercentLimit); +#endif + bool isFlipOverAfterCrashActive(void); int8_t calculateThrottlePercent(void); uint8_t calculateThrottlePercentAbs(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index d1a8556a30..a35c05cc5b 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -69,7 +69,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_delay = 4, // 0,4sec .failsafe_off_delay = 10, // 1sec .failsafe_switch_mode = 0, // default failsafe switch action is identical to rc link loss - .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing + .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing + .failsafe_recovery_delay = 20, // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure + .failsafe_stick_threshold = 30 // 30 percent of stick deflection to exit GPS Rescue procedure ); const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = { @@ -263,12 +265,10 @@ void failsafeUpdateState(void) // Drop the craft failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData break; case FAILSAFE_PROCEDURE_GPS_RESCUE: failsafeActivate(); failsafeState.phase = FAILSAFE_GPS_RESCUE; - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; break; } } @@ -277,8 +277,10 @@ void failsafeUpdateState(void) case FAILSAFE_LANDING: if (receivingRxData) { - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; + if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; + } } if (armed) { failsafeApplyControlInput(); @@ -292,8 +294,12 @@ void failsafeUpdateState(void) break; case FAILSAFE_GPS_RESCUE: if (receivingRxData) { - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; + if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { + if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; + } + } } if (armed) { failsafeApplyControlInput(); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index ae6b09c496..9eff9eba05 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -39,6 +39,8 @@ typedef struct failsafeConfig_s { uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) uint8_t failsafe_switch_mode; // failsafe switch action is 0: stage1 (identical to rc link loss), 1: disarms instantly, 2: stage2 uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it + uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure + uint8_t failsafe_stick_threshold; // Stick deflection percentage to exit GPS Rescue procedure } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -77,9 +79,9 @@ typedef struct failsafeState_s { int16_t events; bool monitoring; bool active; - uint32_t rxDataFailurePeriod; - uint32_t validRxDataReceivedAt; - uint32_t validRxDataFailedAt; + int32_t rxDataFailurePeriod; + int32_t validRxDataReceivedAt; + int32_t validRxDataFailedAt; uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period uint32_t landingShouldBeFinishedAt; uint32_t receivingRxDataPeriod; // period for the required period of valid rxData diff --git a/src/test/Makefile b/src/test/Makefile index 751a8e1da3..00a8c3e80c 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -127,6 +127,9 @@ flight_failsafe_unittest_SRC := \ $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/flight/failsafe.c +flight_failsafe_unittest_DEFINES := \ + USE_GPS_RESCUE= + flight_imu_unittest_SRC := \ $(USER_DIR)/common/bitarray.c \ diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index b98df40c4d..b8c1609656 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -589,6 +589,11 @@ bool isUsingSticksForArming(void) return isUsingSticksToArm; } +bool areSticksActive(uint8_t stickPercentLimit) { + UNUSED(stickPercentLimit); + return false; +} + void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); } bool crashRecoveryModeActive(void) { return false; } From 7480552a624ffac8b6c77c72b776da4eba666e7d Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Sun, 14 Apr 2019 14:11:21 +0200 Subject: [PATCH 2/4] USE_GPS_RESCUE condition is now used in failsafe.c --- src/main/fc/core.h | 6 +----- src/main/flight/failsafe.c | 19 +++++++++++++++---- src/main/flight/failsafe.h | 2 ++ 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/main/fc/core.h b/src/main/fc/core.h index 2fa7770afe..2ea825d96d 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -67,14 +67,10 @@ void updateArmingStatus(void); void taskMainPidLoop(timeUs_t currentTimeUs); -#if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE) -// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here. -bool areSticksActive(uint8_t stickPercentLimit); -#endif - bool isFlipOverAfterCrashActive(void); int8_t calculateThrottlePercent(void); uint8_t calculateThrottlePercentAbs(void); +bool areSticksActive(uint8_t stickPercentLimit); void runawayTakeoffTemporaryDisable(uint8_t disableFlag); bool isAirmodeActivated(); timeUs_t getLastDisarmTimeUs(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a35c05cc5b..58991c6e63 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -76,8 +76,10 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = { "AUTO-LAND", - "DROP", - "GPS-RESCUE" + "DROP" +#ifdef USE_GPS_RESCUE + , "GPS-RESCUE" +#endif }; /* @@ -143,11 +145,12 @@ static void failsafeActivate(void) static void failsafeApplyControlInput(void) { +#ifdef USE_GPS_RESCUE if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); - return; } +#endif for (int i = 0; i < 3; i++) { rcData[i] = rxConfig()->midrc; @@ -229,7 +232,11 @@ void failsafeUpdateState(void) failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData reprocessState = true; } else if (!receivingRxData) { - if (millis() > failsafeState.throttleLowPeriod && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE) { + if (millis() > failsafeState.throttleLowPeriod +#ifdef USE_GPS_RESCUE + && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE +#endif + ) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure @@ -266,10 +273,12 @@ void failsafeUpdateState(void) failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure break; +#ifdef USE_GPS_RESCUE case FAILSAFE_PROCEDURE_GPS_RESCUE: failsafeActivate(); failsafeState.phase = FAILSAFE_GPS_RESCUE; break; +#endif } } reprocessState = true; @@ -292,6 +301,7 @@ void failsafeUpdateState(void) reprocessState = true; } break; +#ifdef USE_GPS_RESCUE case FAILSAFE_GPS_RESCUE: if (receivingRxData) { if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { @@ -310,6 +320,7 @@ void failsafeUpdateState(void) reprocessState = true; } break; +#endif case FAILSAFE_LANDED: setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link disarm(); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 9eff9eba05..da32184878 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -63,7 +63,9 @@ typedef enum { typedef enum { FAILSAFE_PROCEDURE_AUTO_LANDING = 0, FAILSAFE_PROCEDURE_DROP_IT, +#ifdef USE_GPS_RESCUE FAILSAFE_PROCEDURE_GPS_RESCUE, +#endif FAILSAFE_PROCEDURE_COUNT // must be last } failsafeProcedure_e; From 0a38ef67b427d7d18b94d1286a2f06c9e6379740 Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Sun, 14 Apr 2019 19:07:05 +0200 Subject: [PATCH 3/4] Added a missing USE_GPS_RESCUE --- src/main/fc/config.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index cf72a214e8..1461a81214 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -317,9 +317,12 @@ static void validateAndFixConfig(void) || true #endif ) { + +#ifdef USE_GPS_RESCUE if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) { failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT; } +#endif if (isModeActivationConditionPresent(BOXGPSRESCUE)) { removeModeActivationCondition(BOXGPSRESCUE); From b9360eef4e97e39cf66c32cadea8821f7e9fd9f2 Mon Sep 17 00:00:00 2001 From: Tony Cabello Date: Tue, 16 Apr 2019 04:27:23 +0200 Subject: [PATCH 4/4] Simplified logic for recovery delay --- src/main/flight/failsafe.c | 21 +++++++++------------ src/main/flight/failsafe.h | 7 ++++--- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 58991c6e63..0a3b804e56 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -76,9 +76,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = { "AUTO-LAND", - "DROP" + "DROP", #ifdef USE_GPS_RESCUE - , "GPS-RESCUE" + "GPS-RESCUE", #endif }; @@ -88,6 +88,7 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = { void failsafeReset(void) { failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND; failsafeState.validRxDataReceivedAt = 0; failsafeState.validRxDataFailedAt = 0; failsafeState.throttleLowPeriod = 0; @@ -177,7 +178,7 @@ void failsafeOnRxResume(void) void failsafeOnValidDataReceived(void) { failsafeState.validRxDataReceivedAt = millis(); - if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) { + if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) { failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); } @@ -286,10 +287,8 @@ void failsafeUpdateState(void) case FAILSAFE_LANDING: if (receivingRxData) { - if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; - } + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; } if (armed) { failsafeApplyControlInput(); @@ -304,11 +303,9 @@ void failsafeUpdateState(void) #ifdef USE_GPS_RESCUE case FAILSAFE_GPS_RESCUE: if (receivingRxData) { - if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { - if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; - } + if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; } } if (armed) { diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index da32184878..739fed6117 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -81,9 +81,10 @@ typedef struct failsafeState_s { int16_t events; bool monitoring; bool active; - int32_t rxDataFailurePeriod; - int32_t validRxDataReceivedAt; - int32_t validRxDataFailedAt; + uint32_t rxDataFailurePeriod; + uint32_t rxDataRecoveryPeriod; + uint32_t validRxDataReceivedAt; + uint32_t validRxDataFailedAt; uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period uint32_t landingShouldBeFinishedAt; uint32_t receivingRxDataPeriod; // period for the required period of valid rxData