From: Olivier Matz <zer0@droids-corp.org>
Date: Tue, 9 Sep 2014 17:32:59 +0000 (+0200)
Subject: gps: add rx timeout at init
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=38ab4aa1d171a574beb7c286345260dc6ece06dc;p=fpv.git

gps: add rx timeout at init
---

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