[paparazzi-commits] [4181] add counter for time since last GPS message

[Pascal Brisset]

Revision: 4181
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4181
Author:   hecto
Date:     2009-09-18 14:20:15 +0000 (Fri, 18 Sep 2009)
Log Message:
-----------
 add counter for time since last GPS message

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_gps.c
    paparazzi3/trunk/sw/airborne/booz/booz2_gps.h

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.c       2009-09-18 14:19:09 UTC 
(rev 4180)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.c       2009-09-18 14:20:15 UTC 
(rev 4181)
@@ -27,6 +27,9 @@
 
 struct Booz_gps_state booz_gps_state;
 
+uint8_t booz_gps_lost_counter; /* updated at 4Hz, reset on gps event */
+
+
 #ifdef SITL
 bool_t booz_gps_available;
 #endif

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2009-09-18 14:19:09 UTC 
(rev 4180)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2009-09-18 14:20:15 UTC 
(rev 4181)
@@ -41,7 +41,9 @@
 
 extern struct Booz_gps_state booz_gps_state;
 
+extern uint8_t booz_gps_lost_counter; /* updated at 4Hz */
 
+
 /* UBX NAV SOL */
 #define  BOOZ2_GPS_FIX_NONE 0x00
 #define  BOOZ2_GPS_FIX_3D   0x03
@@ -68,11 +70,13 @@
   booz_gps_available = TRUE;
 }
 
-#define Booz2GpsEvent(_sol_available_callback) {    \
-    if (booz_gps_available) {                       \
-      _sol_available_callback();                    \
-      booz_gps_available = FALSE;                   \
-    }                                               \
+#define Booz2GpsEvent(_sol_available_callback) {               \
+    if (booz_gps_available) {                                  \
+      if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)               \
+        booz_gps_lost_counter = 0;                              \
+      _sol_available_callback();                               \
+      booz_gps_available = FALSE;                              \
+    }                                                          \
   }
 #else /* ! SITL */
 #define Booz2GpsEvent(_sol_available_callback) {                    \
@@ -82,10 +86,12 @@
     if (ubx_msg_available) {                                        \
       booz2_gps_read_ubx_message();                                 \
       if (ubx_class == UBX_NAV_ID && ubx_id == UBX_NAV_SOL_ID) {       \
-        _sol_available_callback();                                  \
-      }                                                             \
-      ubx_msg_available = FALSE;                                    \
-    }                                                               \
+        if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)                     \
+          booz_gps_lost_counter = 0;                                    \
+       _sol_available_callback();                                      \
+      }                                                                        
\
+      ubx_msg_available = FALSE;                                       \
+    }                                                                  \
   }
 #endif
 
@@ -113,9 +119,11 @@
 extern void ubx_parse( uint8_t c );
 extern void ubx_init(void);
 
+static inline void booz_gps_periodic( void ) {
+  RunOnceEvery(128, booz_gps_lost_counter++; );
+}
 
+#define GpsIsLost() (booz_gps_lost_counter > 20) /* 4Hz -> 5s */
 
 
 #endif /* BOOZ2_GPS_H */
-
-