Merge pull request #7097 from TonyBlit/gps_rescue_gpslost

GPS Rescue: Add GPS LOST to Sanity Checks

src/main/flight/gps_rescue.c

@@ -71,6 +71,7 @@ typedef enum {
 typedef enum {
     RESCUE_HEALTHY,
     RESCUE_FLYAWAY,
+    RESCUE_GPSLOST,
     RESCUE_LOWSATS,
     RESCUE_CRASH_FLIP_DETECTED,
     RESCUE_STALLED,
@@ -97,6 +98,7 @@ typedef struct {
     float zVelocityAvg; // Up/down average in cm/s
     float accMagnitude;
     float accMagnitudeAvg;
+    bool healthy;
 } rescueSensorData_s;
 
 typedef struct {
@@ -337,6 +339,11 @@ static void performSanityChecks()
         rescueState.failure = RESCUE_CRASH_FLIP_DETECTED;
     }
 
+    // Check if GPS comms are healthy
+    if (!rescueState.sensor.healthy) {
+        rescueState.failure = RESCUE_GPSLOST;
+    }
+
     //  Things that should run at a low refresh rate (such as flyaway detection, etc)
     //  This runs at ~1hz
 
@@ -372,6 +379,7 @@ static void performSanityChecks()
 static void sensorUpdate()
 {
     rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm();
+    rescueState.sensor.healthy = gpsIsHealthy();
 
     // Calculate altitude velocity
     static uint32_t previousTimeUs;

src/main/io/gps.c

@@ -591,6 +591,11 @@ bool gpsNewFrame(uint8_t c)
     return false;
 }
 
+// Check for healthy communications
+bool gpsIsHealthy()
+{
+    return (gpsData.state == GPS_RECEIVING_DATA);
+}
 
 /* This is a light implementation of a GPS frame decoding
    This should work with most of modern GPS devices configured to output 5 frames.

src/main/io/gps.h

@@ -162,6 +162,7 @@ extern uint8_t GPS_svinfo_cno[16];         // Carrier to Noise Ratio (Signal Str
 void gpsInit(void);
 void gpsUpdate(timeUs_t currentTimeUs);
 bool gpsNewFrame(uint8_t c);
+bool gpsIsHealthy(void); // Check for healthy communications
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
 void onGpsNewData(void);