From 8922822e5bbfc41ee71bf22cf1d75eb7d6a4b163 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 9 Sep 2014 19:07:34 +0200 Subject: [PATCH] gps: erase global position struct if position invalid during 200ms --- imuboard/gps_venus.c | 37 ++++++++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) 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; } -- 2.39.5