From: Olivier Matz <zer0@droids-corp.org>
Date: Tue, 9 Sep 2014 17:07:34 +0000 (+0200)
Subject: gps: erase global position struct if position invalid during 200ms
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=8922822e5bbfc41ee71bf22cf1d75eb7d6a4b163;p=fpv.git

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

diff --git a/imuboard/gps_venus.c b/imuboard/gps_venus.c
index bd9a774..82f6525 100644
--- 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;
 }