From f4f0cf2f3b4f56f7940093048d58af6bda925f25 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 25 Sep 2023 21:52:32 +0200
Subject: [PATCH] [gps] add a timeout on gps fix lost (#3105)

---
 sw/airborne/modules/gps/gps.c     | 11 +++++++++
 sw/airborne/modules/gps/gps.h     | 38 ++++++++++++++++++++++---------
 sw/airborne/modules/gps/gps_ubx.c |  6 ++---
 3 files changed, 41 insertions(+), 14 deletions(-)

diff --git a/sw/airborne/modules/gps/gps.c b/sw/airborne/modules/gps/gps.c
index 93906f9834..d9d7a2e0a8 100644
--- a/sw/airborne/modules/gps/gps.c
+++ b/sw/airborne/modules/gps/gps.c
@@ -277,6 +277,17 @@ void gps_periodic_check(struct GpsState *gps_s)
 #endif
 }
 
+bool gps_fix_valid(void)
+{
+  bool gps_3d_timeout_valid = false;
+#ifdef GPS_FIX_TIMEOUT
+  if (get_sys_time_float() - gps_time_since_last_3dfix() < GPS_FIX_TIMEOUT) {
+    gps_3d_timeout_valid = true;
+  }
+#endif
+  return (gps.fix >= GPS_FIX_3D || gps_3d_timeout_valid);
+}
+
 
 static abi_event gps_ev;
 static void gps_cb(uint8_t sender_id,
diff --git a/sw/airborne/modules/gps/gps.h b/sw/airborne/modules/gps/gps.h
index 741ce76a19..1b93ca9235 100644
--- a/sw/airborne/modules/gps/gps.h
+++ b/sw/airborne/modules/gps/gps.h
@@ -43,11 +43,6 @@ extern "C" {
 #define GPS_FIX_DGPS 0x04     ///< DGPS fix
 #define GPS_FIX_RTK  0x05     ///< RTK GPS fix
 
-#define GpsFixValid() (gps.fix >= GPS_FIX_3D)
-#if USE_GPS
-#define GpsIsLost() !GpsFixValid()
-#endif
-
 #define GPS_VALID_POS_ECEF_BIT 0
 #define GPS_VALID_POS_LLA_BIT  1
 #define GPS_VALID_POS_UTM_BIT  2
@@ -177,11 +172,32 @@ extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data);
 extern void gps_parse_GPS_INJECT(uint8_t *buf);
 extern void gps_parse_RTCM_INJECT(uint8_t *buf);
 
-/** GPS timeout in seconds */
+/** Check if GPS fix is valid.
+ *
+ * If GPS_FIX_TIMEOUT is configured, check for 3D fix with timeout,
+ * otherwise, returns the last value from GPS module
+ */
+extern bool gps_fix_valid(void);
+
+// Compatibility macros
+#define GpsFixValid() gps_fix_valid()
+#if USE_GPS
+#define GpsIsLost() !GpsFixValid()
+#endif
+
+/** GPS timeout in seconds
+ *  in case of communication loss with GPS module
+ */
 #ifndef GPS_TIMEOUT
 #define GPS_TIMEOUT 2
 #endif
 
+/** Periodic GPS check.
+ * Marks GPS as lost when no GPS message was received for GPS_TIMEOUT seconds
+ */
+extern void gps_periodic_check(struct GpsState *gps_s);
+
+// FIXME not used, to be removed ?
 static inline bool gps_has_been_good(void)
 {
   static bool gps_had_valid_fix = false;
@@ -191,11 +207,11 @@ static inline bool gps_has_been_good(void)
   return gps_had_valid_fix;
 }
 
-
-/** Periodic GPS check.
- * Marks GPS as lost when no GPS message was received for GPS_TIMEOUT seconds
- */
-extern void gps_periodic_check(struct GpsState *gps_s);
+/** Returns the time since last 3D fix in seconds (float) */
+static inline float gps_time_since_last_3dfix(void)
+{
+  return (float)(gps.last_3dfix_time + (float)(gps.last_3dfix_ticks) / sys_time.cpu_ticks_per_sec);
+}
 
 
 /*
diff --git a/sw/airborne/modules/gps/gps_ubx.c b/sw/airborne/modules/gps/gps_ubx.c
index 3344286094..6103d1e916 100644
--- a/sw/airborne/modules/gps/gps_ubx.c
+++ b/sw/airborne/modules/gps/gps_ubx.c
@@ -166,7 +166,7 @@ static void gps_ubx_parse_nav_pvt(void)
     gps_ubx.state.fix = 4; // dgnss
   } else if(gnssFixOK) {
     gps_ubx.state.fix = 3; // 3D
-  } else{
+  } else {
     gps_ubx.state.fix = 0;
   }
 
@@ -397,14 +397,14 @@ static void gps_ubx_parse_nav_relposned(void)
     uint8_t gnssFixOK   = RTCMgetbitu(&flags, 7, 1);
 
     /* Only save the latest valid relative position */
-    if(relPosValid) {
+    if (relPosValid) {
       if (diffSoln && carrSoln == 2) {
         gps_ubx.state.fix = 5; // rtk
       } else if(diffSoln && carrSoln == 1) {
         gps_ubx.state.fix = 4; // dgnss
       } else if(gnssFixOK) {
         gps_ubx.state.fix = 3; // 3D
-      } else{
+      } else {
         gps_ubx.state.fix = 0;
       }