imuboard/gps: increase position reset timeout

[fpv.git] / imuboard / gps_venus.c

diff --git a/imuboard/gps_venus.c b/imuboard/gps_venus.c
index b4e0aa7..3c089fa 100644 (file)
--- a/imuboard/gps_venus.c
+++ b/imuboard/gps_venus.c
@@ -377,11 +377,31 @@ int8_t recv_cb(uint8_t byte)
 int recv_msg(void)
 {
        int ret;
+       uint8_t irq_flags;
        int16_t c;
+       uint16_t t1, t2, diff;
+
+       IRQ_LOCK(irq_flags);
+       t1 = global_ms;
+       IRQ_UNLOCK(irq_flags);
 
+       printf_P(PSTR("recv_msg()\n"));
        while (1) {
-               /* XXX use select for timeout */
-               c = uart_recv(GPS_UART);
+               IRQ_LOCK(irq_flags);
+               t2 = global_ms;
+               IRQ_UNLOCK(irq_flags);
+               diff = t2 - t1;
+               if (diff > 1000) {
+                       printf_P(PSTR("recv_msg timeout\n"));
+                       return -1;
+               }
+
+               c = uart_recv_nowait(GPS_UART);
+               if (c < 0)
+                       continue;
+
+               debug_printf("%2.2x\n", c);
+
                ret = recv_cb(c);
                if (ret == 0)
                        return 0;
@@ -390,13 +410,29 @@ int recv_msg(void)
 
 int wait_ack(int msg_type)
 {
+       uint8_t irq_flags;
        int ret;
+       uint16_t t1, t2, diff;
+
+       IRQ_LOCK(irq_flags);
+       t1 = global_ms;
+       IRQ_UNLOCK(irq_flags);
+
 
        while (1) {
                ret = recv_msg();
                if (ret < 0)
                        return -1;
 
+               IRQ_LOCK(irq_flags);
+               t2 = global_ms;
+               IRQ_UNLOCK(irq_flags);
+               diff = t2 - t1;
+               if (diff > 1000) {
+                       printf_P(PSTR("wait_ack timeout\n"));
+                       return -1;
+               }
+
                /* retry if it's not the expected message */
                if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
                        continue;
@@ -527,7 +563,7 @@ static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
        }
 
        diff = ms - last_valid;
-       if (is_valid == 1 && diff > 100) {
+       if (is_valid == 1 && diff > 500) {
 
                is_valid = 0;
                /* update global structure */
@@ -581,7 +617,7 @@ void gps_get_pos(struct gps_pos *pos)
        memcpy(pos, &gps_pos, sizeof(*pos));
 }
 
-int gps_log(void)
+int gps_log(uint8_t to_stdout)
 {
        uint32_t ms;
        uint8_t flags, prio;
@@ -610,13 +646,13 @@ int gps_log(void)
                (double)gps_pos.sea_altitude / 100.);
 
 
-       if (sd_log_enabled()) {
+       if (!to_stdout && sd_log_enabled()) {
                if (sd_log_write(buf, len) != len) {
                        printf_P(PSTR("error writing to file\n"));
                        return -1;
                }
        }
-       else {
+       else if (to_stdout) {
                printf_P(PSTR("%s"), buf);
        }