From 888c0e48f69445cf49c4a85db42a1e30201fc45b Mon Sep 17 00:00:00 2001
From: Tony Cabello <tcabello@blitsoftware.com>
Date: Fri, 16 Nov 2018 15:01:24 +0100
Subject: [PATCH] GPS Rescue: Add GPS LOST to Sanity Checks

---
 src/main/flight/gps_rescue.c | 8 ++++++++
 src/main/io/gps.c            | 5 +++++
 src/main/io/gps.h            | 1 +
 3 files changed, 14 insertions(+)

diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 2e00e1a70c..eb3b129c6f 100644
--- a/src/main/flight/gps_rescue.c
+++ b/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;
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index d5c389ad04..d631b69557 100644
--- a/src/main/io/gps.c
+++ b/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.
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 58b6d4978e..df81de8801 100644
--- a/src/main/io/gps.h
+++ b/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);