diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 52e44b2b51..561c6cc163 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -747,6 +747,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/config.c b/src/main/fc/config.c index e03d46828e..f4bd394116 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -319,9 +319,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); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index a598907e8d..3ffb428e45 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..2ea825d96d 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -70,6 +70,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs); 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 d1a8556a30..0a3b804e56 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -69,13 +69,17 @@ 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] = { "AUTO-LAND", "DROP", - "GPS-RESCUE" +#ifdef USE_GPS_RESCUE + "GPS-RESCUE", +#endif }; /* @@ -84,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; @@ -141,11 +146,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; @@ -172,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); } @@ -227,7 +233,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 @@ -263,13 +273,13 @@ 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; +#ifdef USE_GPS_RESCUE case FAILSAFE_PROCEDURE_GPS_RESCUE: failsafeActivate(); failsafeState.phase = FAILSAFE_GPS_RESCUE; - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; break; +#endif } } reprocessState = true; @@ -290,10 +300,13 @@ void failsafeUpdateState(void) reprocessState = true; } break; +#ifdef USE_GPS_RESCUE case FAILSAFE_GPS_RESCUE: if (receivingRxData) { - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; + if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; + } } if (armed) { failsafeApplyControlInput(); @@ -304,6 +317,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 ae6b09c496..739fed6117 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); @@ -61,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; @@ -78,6 +82,7 @@ typedef struct failsafeState_s { bool monitoring; bool active; 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 diff --git a/src/test/Makefile b/src/test/Makefile index 3dbee62b4b..e6c2818d9d 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -129,6 +129,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; }