diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index b3ef23a4ba..120234c74c 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -113,6 +113,7 @@ typedef struct { rescueSensorData_s sensor; rescueIntent_s intent; bool isFailsafe; + bool isAvailable; } rescueState_s; #define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate @@ -406,6 +407,54 @@ static void sensorUpdate() } } +// This function checks the following conditions to determine if GPS rescue is available: +// 1. sensor healthy - GPS data is being received. +// 2. GPS has a valid fix. +// 3. GPS number of satellites is less than the minimum configured for GPS rescue. +// Note: this function does not take into account the distance from homepoint etc. (gps_rescue_min_dth) and +// is also independent of the gps_rescue_sanity_checks configuration +static bool gpsRescueIsAvailable(void) +{ + static uint32_t previousTimeUs = 0; // Last time LowSat was checked + const uint32_t currentTimeUs = micros(); + static int8_t secondsLowSats = 0; // Minimum sat detection + static bool lowsats = false; + static bool noGPSfix = false; + bool result = true; + + if (!gpsIsHealthy() ) { + return false; + } + + // Things that should run at a low refresh rate >> ~1hz + const uint32_t dTime = currentTimeUs - previousTimeUs; + if (dTime < 1000000) { //1hz + if (noGPSfix || lowsats) { + result = false; + } + return result; + } + + previousTimeUs = currentTimeUs; + + if (!STATE(GPS_FIX)) { + result = false; + noGPSfix = true; + } else { + noGPSfix = false; + } + + secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < gpsRescueConfig()->minSats) ? 1 : -1), 0, 2); + if (secondsLowSats == 2) { + lowsats = true; + result = false; + } else { + lowsats = false; + } + + return result; +} + /* Determine what phase we are in, determine if all criteria are met to move to the next phase */ @@ -423,6 +472,8 @@ void updateGPSRescueState(void) sensorUpdate(); + rescueState.isAvailable = gpsRescueIsAvailable(); + switch (rescueState.phase) { case RESCUE_IDLE: idleTasks(); @@ -546,5 +597,10 @@ bool gpsRescueIsConfigured(void) { return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE); } + +bool isGPSRescueAvailable(void) +{ + return rescueState.isAvailable; +} #endif diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index d77de91da1..7dad69fddd 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -45,3 +45,4 @@ void rescueNewGpsData(void); float gpsRescueGetYawRate(void); float gpsRescueGetThrottle(void); bool gpsRescueIsConfigured(void); +bool isGPSRescueAvailable(void); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 73c7417b47..0441489ccd 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -1013,6 +1013,7 @@ const clivalue_t valueTable[] = { { "osd_warn_visual_beeper", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_VISUAL_BEEPER, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, { "osd_warn_crash_flip", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CRASH_FLIP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, { "osd_warn_esc_fail", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_ESC_FAIL, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, + { "osd_warn_no_gps_rescue", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_GPS_RESCUE_UNAVAILABLE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, #ifdef USE_ADC_INTERNAL { "osd_warn_core_temp", VAR_UINT16 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CORE_TEMPERATURE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)}, #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2942001ac4..791e05931c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -976,6 +976,17 @@ static bool osdDrawSingleElement(uint8_t item) break; } +#ifdef USE_GPS_RESCUE + if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) && + ARMING_FLAG(ARMED) && + gpsRescueIsConfigured() && + !isGPSRescueAvailable()) { + osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC"); + SET_BLINK(OSD_WARNINGS); + break; + } +#endif + // Show warning if in HEADFREE flight mode if (FLIGHT_MODE(HEADFREE_MODE)) { osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "HEADFREE"); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index bc5b1fac1e..a0bdf9c0ec 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -199,6 +199,7 @@ typedef enum { OSD_WARNING_RC_SMOOTHING, OSD_WARNING_FAIL_SAFE, OSD_WARNING_LAUNCH_CONTROL, + OSD_WARNING_GPS_RESCUE_UNAVAILABLE, OSD_WARNING_COUNT // MUST BE LAST } osdWarningsFlags_e;