Merge pull request #7256 from pkruger/6839-feature-add-rescue-unreliable-now-OSD-indication-for-risky-flights

Add GPS rescue unreliable now warning in OSD

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
 

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);

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

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");

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;