gps: add rx timeout at init

diff --git a/imuboard/gps_venus.c b/imuboard/gps_venus.c
index b4e0aa7d15663d33b65d38bb610fee289f27ca1a..2aa8f8386e8a46d6eb4aed9fc2a263664d90d592 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;