diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 21c11aaba..3ead43209 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -71,7 +71,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .throttleMin = 1200, .throttleMax = 1600, .throttleHover = 1280, - .sanityChecks = 0, + .sanityChecks = RESCUE_SANITY_ON, .minSats = 8 ); @@ -123,6 +123,19 @@ void updateGPSRescueState(void) hoverThrottle = gpsRescueConfig()->throttleHover; } + // Minimum distance detection (100m). Disarm regardless of sanity check configuration. Rescue too close is never a good idea. + if (rescueState.sensor.distanceToHome < 100) { + // Never allow rescue mode to engage as a failsafe within 100 meters or when disarmed. + if (rescueState.isFailsafe || !ARMING_FLAG(ARMED)) { + rescueState.failure = RESCUE_TOO_CLOSE; + setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); + disarm(); + } else { + // Leave it up to the sanity check setting + rescueState.failure = RESCUE_TOO_CLOSE; + } + } + rescueState.phase = RESCUE_ATTAIN_ALT; FALLTHROUGH; case RESCUE_ATTAIN_ALT: @@ -284,11 +297,6 @@ void performSanityChecks() if (msI == 5) { rescueState.failure = RESCUE_FLYAWAY; } - - // Minimum distance detection (100m) - if (rescueState.sensor.distanceToHome < 100) { - rescueState.failure = RESCUE_TOO_CLOSE; - } } void rescueStart()