1 files changed, 30 insertions, 14 deletions

diff --git a/app/gps.c b/app/gps.c
index 101413f..206f0c0 100644
--- a/app/gps.c
+++ b/app/gps.c
@@ -138,7 +138,7 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
   uint8_t gps_fix, flags, fix_stat, flags2;
   uint32_t d;
 
-  unsigned kick_off_survey_mode=300;
+  unsigned kick_off_survey_mode = 300;
 
   if ((!ptr) || (len != 16))
     return -1;
@@ -175,7 +175,7 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
   case 5:
     fix = 'T';
     gps_locked = 1;
-    kick_off_survey_mode=0;
+    kick_off_survey_mode = 0;
     break;
 
   default:
@@ -205,6 +205,8 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
     fix2 = '?';
   }
 
+  max7219_report_fix (fix, fix2);
+
 
   //  printf ("fix: %c%c happy %d Fix %02x flags %02x fix_stat %02x flags2 %02x \r\n",fix,fix2,gps_happy, gps_fix, flags,fix_stat,flags2);
 
@@ -215,13 +217,13 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
     gps_happy = 0;
 
 
-  if (kick_off_survey_mode>0) {
-	  kick_off_survey_mode--;
+  if (kick_off_survey_mode > 0) {
+    kick_off_survey_mode--;
 
-	  if (!kick_off_survey_mode)  {
-		printf("GPS no time fix - kicking off survey mode\n");
-  		ubx_cfg_tmode (1, 0, 0, 0, 0, 3600, 3600); // 1 hour and 6cm
-	  }
+    if (!kick_off_survey_mode)  {
+      printf ("GPS no time fix - kicking off survey mode\n");
+      ubx_cfg_tmode (1, 0, 0, 0, 0, 3600, 3600); // 1 hour and 6cm
+    }
   }
 
   return 0;
@@ -371,6 +373,7 @@ static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len)
   int8_t sys;
   uint8_t  n;
 
+  if (!chatty_gps) return0 ;
 
   ptr += ubx_get_u32 (ptr, &d4);
   ptr += ubx_get_u8 (ptr, &prn);
@@ -381,7 +384,7 @@ static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len)
   ptr += ubx_get_u8 (ptr, &n);
   ptr += 3;
 
-//  printf ("GPS SBAS PRN:%d M:%d S:%d SVC:%02x SVS:", prn, mode, sys, service);
+  printf ("GPS SBAS PRN:%d M:%d S:%d SVC:%02x SVS:", prn, mode, sys, service);
 
   while (n--) {
     ptr += ubx_get_u8 (ptr, &prn);
@@ -401,7 +404,7 @@ static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len)
   uint8_t  n, flags, cflags;
   const char *st;
 
-  return 0;
+  if (!chatty_gps) return 0;
 
   ptr += ubx_get_u32 (ptr, &d4);
   ptr += ubx_get_u8 (ptr, &n);
@@ -485,6 +488,8 @@ static int ubx_recv_tim_svin (uint8_t *ptr, unsigned len)
   printf ("TIM-SVIN dur %u var %u obs %u valid %02x active %02x\r\n",
           (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active);
 
+  max7219_report_svin (valid, active);
+
   return 0;
 }
 
@@ -750,6 +755,7 @@ static void gps_pps_dispatch (void)
   if (gps_happy)
     led3_set (v);
 
+
   gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK;
 
 
@@ -815,7 +821,8 @@ ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
 static int
 ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
 {
-  uint32_t timeout=4000;
+  uint32_t timeout = 4000;
+  unsigned tries = 4;
 
   ubx_ack = 0;
   ubx_nack = 0;
@@ -825,15 +832,24 @@ ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
 
     if (!timeout) {
       printf ("GPS timeout resending packet\r\n");
+
+      if (!tries) {
+        printf ("GPS 3 timesout - resetting system\r\n");
+        usart2_drain();
+        scb_reset_system();
+      }
+
       usart1_drain();
       ubx_send (class, id, payload, len);
-      timeout = 40000;
+      timeout = 4000;
+      tries--;
     }
+
     timeout--;
 
 
     gps_dispatch();
-    delay_ms(1);
+    delay_ms (1);
   }
 
   return !!ubx_nack;
@@ -1176,7 +1192,7 @@ gps_init (void)
 
   ubx_cfg_poll (0x6, 0x16);
 
-//  ubx_aid_ini (522202400, 1279080, 2900, 20000);
+  //  ubx_aid_ini (522202400, 1279080, 2900, 20000);
 
   printf ("GNSS ready\r\n");
 #else