gps: erase global position struct if position invalid during 200ms

diff --git a/imuboard/gps_venus.c b/imuboard/gps_venus.c
index bd9a7745abc2f83e6e4cf994e3ec0a2897310561..82f65258b63fa35cf10beec97c3122b9c68826cd 100644 (file)
--- a/imuboard/gps_venus.c
+++ b/imuboard/gps_venus.c
@@ -18,11 +18,14 @@
 #include "sd_raw_config.h"
 #include "sd_log.h"
 
+/* note: enabling debug will make the code fail due to rx queue full */
 #define debug_printf(fmt, a...) do { } while (0)
 /* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */
 
 #define GPS_UART 0
 
+#define GPS_PERIOD_MS 2
+
 /*
   https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
  */
@@ -456,6 +459,8 @@ static int decode_gps_pos(uint8_t *buf, uint16_t len)
 /* display current GPS position stored in the global variable */
 static void display_gps(void)
 {
+       /* no need to call ntohs/ntohl because the frame is already fixed by
+        * decode_gps_pos() */
        printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
                        "tow %.10"PRIu32"\t"),
                gps_pos.msg_id,
@@ -489,30 +494,52 @@ static void display_gps(void)
                (double)gps_pos.ecef_vz/100.);
 }
 
+/* called on timer event */
 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 {
+       static int16_t last_valid = 0;
+       static uint8_t is_valid = 0;
+       int16_t ms, diff;
        int16_t c;
-       int ret;
+       int8_t ret;
+       uint8_t irq_flags;
 
        (void)cm;
        (void)tim;
        (void)arg;
 
+       IRQ_LOCK(irq_flags);
+       ms = global_ms;
+       IRQ_UNLOCK(irq_flags);
+
        while (1) {
                c = uart_recv_nowait(GPS_UART);
                if (c < 0) /* no more char */
-                       goto resched;
+                       break;
 
                ret = recv_cb(c);
                if (ret == 0) {
                        decode_gps_pos(rxframe.data, rxframe.len);
                        if (0)
                                display_gps();
+
+                       last_valid = ms;
+                       is_valid = 1;
                }
        }
 
- resched:
-       callout_schedule(cm, tim, 2);
+       diff = ms - last_valid;
+       if (is_valid == 1 && diff > 100) {
+
+               is_valid = 0;
+               /* update global structure */
+               IRQ_LOCK(irq_flags);
+               memset(&gps_pos, 0, sizeof(gps_pos));
+               gps_pos.latitude = 0xffffffff;
+               IRQ_UNLOCK(irq_flags);
+       }
+
+       callout_schedule(cm, tim, GPS_PERIOD_MS);
 }
 
 static void venus634_configure(void)
@@ -550,7 +577,7 @@ int gps_venus_init(void)
        venus634_configure();
 
        callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
-       callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
+       callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
 
        return 0;
 }