tim

1 files changed, 300 insertions, 68 deletions

diff --git a/app/gps.c b/app/gps.c
index c3ac2d1..101413f 100644
--- a/app/gps.c
+++ b/app/gps.c
@@ -14,6 +14,7 @@
 static EPOCH next_sec;
 
 static int ubx_ack = 0;
+static int ubx_nack = 0;
 static int ubx_ack_xfer = 0;
 
 static int current_ref_hz = 1;
@@ -32,6 +33,9 @@ static Event_ring gps_ring;
 
 static char gps_info[60];
 
+static int  ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z,
+                           uint32_t var, uint32_t min_dur, uint32_t max_var);
+
 void exti9_5_isr (void)
 {
   uint32_t now = HW_CLOCK_REG;
@@ -98,12 +102,44 @@ static int ubx_recv_almanac (uint8_t *ptr, unsigned len)
   return 0;
 }
 
+double gps_lat, gps_lon;
+
+static int
+ubx_recv_nav_posllh (uint8_t *ptr, unsigned len)
+{
+  uint32_t d;
+  int32_t ilat, ilon, i;
+
+  if ((!ptr) || (len != 28))
+    return -1;
+
+  ptr += ubx_get_u32 (ptr, &d); //TOW
+  ptr += ubx_get_i32 (ptr, &ilon);
+  ptr += ubx_get_i32 (ptr, &ilat);
+  ptr += ubx_get_i32 (ptr, &i); //geoid height
+  ptr += ubx_get_i32 (ptr, &i); //msl height
+  ptr += ubx_get_u32 (ptr, &d); //hacc
+  ptr += ubx_get_u32 (ptr, &d); //vacc
+
+
+  gps_lat = 1.e-7 * (double) ilat;
+  gps_lon = 1.e-7 * (double) ilon;
+
+
+  printf ("POS: %.4f %.4f\r\n", gps_lat, gps_lon);
+
+  return 0;
+}
+
+
 static int
 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;
+
   if ((!ptr) || (len != 16))
     return -1;
 
@@ -138,7 +174,8 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
 
   case 5:
     fix = 'T';
-    gps_locked = 0;
+    gps_locked = 1;
+    kick_off_survey_mode=0;
     break;
 
   default:
@@ -169,7 +206,7 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
   }
 
 
-  //  printf ("fix: %c%c\r\n",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);
 
 
   if ((gps_locked) && (gps_happy < 10000))
@@ -177,6 +214,16 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len)
   else
     gps_happy = 0;
 
+
+  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
+	  }
+  }
+
   return 0;
 }
 
@@ -207,20 +254,24 @@ ubx_recv_clock_stats (uint8_t *ptr, unsigned len)
 
 }
 
+int gps_utc_diff;
+int gps_wday;
+
 static int
 ubx_recv_utc (uint8_t *ptr, unsigned len)
 {
   int32_t nano;
   uint32_t acc;
+  uint32_t tow;
+  int tod, utc_tod;
   uint16_t year;
   uint8_t hour, min, sec, day, month, valid;
-  uint32_t d;
   //  char buf[40];
 
   if ((!ptr) || (len != 20))
     return -1;
 
-  ptr += ubx_get_u32 (ptr, &d); //TOW
+  ptr += ubx_get_u32 (ptr, &tow); //TOW
   ptr += ubx_get_u32 (ptr, &acc); //bias
   ptr += ubx_get_i32 (ptr, &nano);
   ptr += ubx_get_u16 (ptr, &year);
@@ -270,8 +321,11 @@ ubx_recv_utc (uint8_t *ptr, unsigned len)
     u.second = sec;
     u.nanosecond = 0;
 
+
+
     gps_time = time_utc_to_epoch (u);
 
+
     next_sec = gps_time;
 
     now = HW_CLOCK_REG;
@@ -281,8 +335,22 @@ ubx_recv_utc (uint8_t *ptr, unsigned len)
 
     pll_set_offset (gps_time, abs);
 
-  }
+  } else
+    printf ("!GPS %s\r\n", gps_info);
+
+#define SECONDS_IN_DAY  (24*60*60)
+
+  utc_tod = (((hour * 60) + min) * 60) + sec;
+  tow /= 1000;
+  tod = tow % SECONDS_IN_DAY;
+  gps_wday = tow / SECONDS_IN_DAY;
 
+  utc_tod += 2 * SECONDS_IN_DAY;
+  utc_tod -= tod;
+
+  while (utc_tod >= (SECONDS_IN_DAY / 2)) utc_tod -= SECONDS_IN_DAY;
+
+  gps_utc_diff = utc_tod;
 
 #if 0
   sprintf (buf, "%+6dE-12 %02d%02d%02d", (int) freq,
@@ -298,12 +366,13 @@ ubx_recv_utc (uint8_t *ptr, unsigned len)
 
 static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len)
 {
+  uint32_t d4;
   uint8_t prn, mode, service;
   int8_t sys;
   uint8_t  n;
 
-  ptr += 4;
 
+  ptr += ubx_get_u32 (ptr, &d4);
   ptr += ubx_get_u8 (ptr, &prn);
   ptr += ubx_get_u8 (ptr, &mode);
   ptr += ubx_get_i8 (ptr, &sys);
@@ -312,35 +381,78 @@ 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\r\n", 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);
     ptr += 11;
-    printf ("GPS SBAS     SV:%d\r\n", prn);
+    printf (" %d", prn);
   }
 
+  printf ("\r\n");
   return 0;
 }
 
-static int ubx_recv_nav_sat (uint8_t *ptr, unsigned len)
+static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len)
 {
-  uint8_t gnssid, prn, cno, ver;
-  uint8_t  n;
+  uint32_t d4;
+  uint16_t d2;
+  uint8_t ch, quality, cno, svid;
+  uint8_t  n, flags, cflags;
+  const char *st;
 
-  ptr += 4;
+  return 0;
 
-  ptr += ubx_get_u8 (ptr, &ver);
+  ptr += ubx_get_u32 (ptr, &d4);
   ptr += ubx_get_u8 (ptr, &n);
-  ptr += 2;
+  ptr += ubx_get_u8 (ptr, &flags);
+  ptr += ubx_get_u16 (ptr, &d2);
 
   while (n--) {
-    ptr += ubx_get_u8 (ptr, &gnssid);
-    ptr += ubx_get_u8 (ptr, &prn);
+    ptr += ubx_get_u8 (ptr, &ch);
+    ptr += ubx_get_u8 (ptr, &svid);
+    ptr += ubx_get_u8 (ptr, &cflags);
+    ptr += ubx_get_u8 (ptr, &quality);
     ptr += ubx_get_u8 (ptr, &cno);
-    ptr += 9;
+    ptr += 7;
+
+    switch (quality) {
+    case 0:
+      st = "idle";
+      break;
+
+    case 1:
+      st = "search";
+      break;
 
-    printf ("GPS    GNSS:%2d PRN:%3d CNO:%3d\r\n", gnssid, prn, cno);
+    case 2:
+      st = "acq";
+      break;
+
+    case 3:
+      st = "inop";
+      break;
+
+    case 4:
+      st = "lock";
+      break;
+
+    case 5:
+    case 6:
+    case 7:
+      st = "good";
+      break;
+
+    default:
+      st = "?";
+    }
+
+    printf ("GPS    SVID:%3d C/N:%3d %c%c%c %s\r\n",
+            svid, cno,
+            (cflags & 0x01) ? 'U' : '-',
+            (cflags & 0x02) ? 'D' : '-',
+            (cflags & 0x04) ? 'E' : '-',
+            st);
   }
 
 
@@ -355,16 +467,41 @@ static int ubx_recv_rinex (uint8_t *payload, unsigned len)
   return 0;
 }
 
+static int ubx_recv_tim_svin (uint8_t *ptr, unsigned len)
+{
+  uint8_t valid, active;
+  uint32_t dur, obs, var;
+  int32_t x, y, z;
+
+  ptr += ubx_get_u32 (ptr, &dur);
+  ptr += ubx_get_i32 (ptr, &x);
+  ptr += ubx_get_i32 (ptr, &y);
+  ptr += ubx_get_i32 (ptr, &z);
+  ptr += ubx_get_u32 (ptr, &var);
+  ptr += ubx_get_u32 (ptr, &obs);
+  ptr += ubx_get_u8 (ptr, &valid);
+  ptr += ubx_get_u8 (ptr, &active);
+
+  printf ("TIM-SVIN dur %u var %u obs %u valid %02x active %02x\r\n",
+          (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active);
+
+  return 0;
+}
+
 
 
 static void
 ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
 {
-  //  printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
+  //printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
 
   switch (class) {
   case 1:
     switch (id) {
+    case 0x2:
+      ubx_recv_nav_posllh (payload, len);
+      break;
+
     case 0x3:
       ubx_recv_nav_status (payload, len);
       break;
@@ -377,13 +514,14 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
       ubx_recv_clock_stats (payload, len);
       break;
 
+    case 0x30:
+      ubx_recv_nav_svinfo (payload, len);
+      break;
+
     case 0x32:
       ubx_recv_nav_sbas (payload, len);
       break;
 
-    case 0x35:
-      ubx_recv_nav_sat (payload, len);
-      break;
 
     default:
       printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
@@ -414,10 +552,29 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
 
     break;
 
+  case 0xd:
+    switch (id) {
+    case 0x4:
+      ubx_recv_tim_svin (payload, len);
+
+      break;
+
+    default:
+      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
+      hexdump (payload, len);
+    }
+
+    break;
+
+
+
 
 
   case 5:
     ubx_ack++;
+
+    if (!id) ubx_nack++;
+
     printf (" %s for %02x.%02x\r\n", id ? "ACK" : "NAK", payload[0],
             payload[1]);
 
@@ -651,38 +808,35 @@ ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
   ubx_send_byte (ck_a);
   ubx_send_byte (ck_b);
 
-  //  printf ("TX> %02x.%02x\r\n", class, id);
+  printf ("TX> %02x.%02x\r\n", class, id);
 
 }
 
 static int
 ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
 {
-  uint32_t timeout;
+  uint32_t timeout=4000;
 
   ubx_ack = 0;
+  ubx_nack = 0;
   ubx_send (class, id, payload, len);
 
-
-
-  timeout = ticks + TIMEOUT;
-
   while (!ubx_ack) {
 
-    if (ticks > timeout) {
+    if (!timeout) {
       printf ("GPS timeout resending packet\r\n");
       usart1_drain();
       ubx_send (class, id, payload, len);
-      timeout = ticks + TIMEOUT;
+      timeout = 40000;
     }
-
-
+    timeout--;
 
 
     gps_dispatch();
+    delay_ms(1);
   }
 
-  return 0;
+  return !!ubx_nack;
 }
 
 static int
@@ -754,6 +908,7 @@ static int
 ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate)
 {
   uint8_t buf[8], *ptr;
+  int ret;
 
   ptr = buf;
 
@@ -766,7 +921,10 @@ ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate)
   ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
   ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5
 
-  return ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf));
+  ret = ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf));
+
+  printf ("GPS set message rate for %02x.%02x %s\r\n", class, id, ret ? "Failed" : "Succeeded");
+  return ret;
 }
 
 
@@ -785,6 +943,70 @@ ubx_cfg_rst (uint16_t flags)
   return 0;
 }
 
+static int
+ubx_cfg_poll (uint8_t class, uint8_t id)
+{
+  return ubx_handshake (class, id, NULL, 0);
+}
+
+static int  ubx_cfg_sbas (uint8_t mode, uint8_t usage, uint8_t channels)
+{
+  uint8_t buf[8], *ptr;
+
+  ptr = buf;
+
+  ptr += ubx_put_u8 (ptr, mode);
+  ptr += ubx_put_u8 (ptr, usage);
+  ptr += ubx_put_u8 (ptr, channels);
+  ptr += ubx_put_u8 (ptr, 0x0);
+  ptr += ubx_put_u32 (ptr, /* 0x1 PRN 120 |  */ 0x8 /*PRN 123*/ | /* 0x40  PRN 126  | */   0x10000 /*PRN136*/);
+
+  return ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));
+}
+
+
+static int  ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z,
+                           uint32_t var, uint32_t min_dur, uint32_t max_var)
+{
+  uint8_t buf[28], *ptr;
+
+  ptr = buf;
+
+  ptr += ubx_put_u32 (ptr, time_mode);
+  ptr += ubx_put_i32 (ptr, x);
+  ptr += ubx_put_i32 (ptr, y);
+  ptr += ubx_put_i32 (ptr, z);
+  ptr += ubx_put_u32 (ptr, var);
+  ptr += ubx_put_u32 (ptr, min_dur);
+  ptr += ubx_put_u32 (ptr, max_var);
+
+  return ubx_handshake (0x06, 0x1d, buf, (unsigned) (ptr - buf));
+}
+
+static int  ubx_aid_ini (int32_t lat, int32_t lon, int32_t alt, uint32_t acc)
+{
+  uint8_t buf[48], *ptr;
+
+  ptr = buf;
+
+  ptr += ubx_put_i32 (ptr, lat);
+  ptr += ubx_put_i32 (ptr, lon);
+  ptr += ubx_put_i32 (ptr, alt);
+  ptr += ubx_put_u32 (ptr, acc);
+  ptr += ubx_put_u16 (ptr, 0); //time mark flags
+  ptr += ubx_put_u16 (ptr, 0); // week number
+  ptr += ubx_put_u32 (ptr, 0); // tow s
+  ptr += ubx_put_i32 (ptr, 0); // tow ns
+  ptr += ubx_put_u32 (ptr, 0); // tow accuracy ms
+  ptr += ubx_put_u32 (ptr, 0); // tow accuracy ns
+  ptr += ubx_put_i32 (ptr, 0); // clock drift or freq
+  ptr += ubx_put_u32 (ptr, 0); // clock drift or freq accuracy
+  ptr += ubx_put_u32 (ptr, 0x21); //flags LLA no time
+
+  return ubx_handshake (0x0b, 0x01, buf, (unsigned) (ptr - buf));
+  //return ubx_send (0x0b, 0x01, NULL, 0);
+
+}
 
 static int
 gps_set_ref (int ref_hz)
@@ -794,34 +1016,48 @@ gps_set_ref (int ref_hz)
 
   printf ("setting gps ref to %d hz\r\n", ref_hz);
 
+  /*Try ublox 5 version first */
+
   current_ref_hz = ref_hz;
 
   ptr = buf;
 
+  ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period /us
+  ptr += ubx_put_u32 (ptr, 1000); //duration /us
+  ptr += ubx_put_i8 (ptr, 1); //+ve sense
+  ptr += ubx_put_u8 (ptr, 1); //lock to utc
+  ptr += ubx_put_u8 (ptr, 0); //pulse only when locked
+  ptr += ubx_put_u8 (ptr, 0); //res
+
+  /* 50.52 ns from 10m of RG58 */
+  /* 15.15 ns from 3m of RG174 */
+  /* total 66ns*/
+
+  ptr += ubx_put_i16 (ptr, 66); //antenna cable delay /ns
+  ptr += ubx_put_i16 (ptr, 0); //rf group delay /ns
+  ptr += ubx_put_i32 (ptr, 0); //user delay /ns
+
+  ret = ubx_handshake (0x06, 0x07, buf, (unsigned) (ptr - buf));
+
+  if (!ret) return ret;
+
+  ptr = buf;
+
   ptr += ubx_put_u8 (ptr, 0);   //timepluse 1
   ptr += ubx_put_u8 (ptr, 0);   //version 0
   ptr += ubx_put_u16 (ptr, 0);  //reserved
 
-  //  ptr += ubx_put_u16 (ptr, 32); //ant cable delay ns
   ptr += ubx_put_u16 (ptr, 0); //ant cable delay ns
   ptr += ubx_put_u16 (ptr, 0);  //rf group delay ns
-  ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period unlocked/us
+  ptr += ubx_put_u32 (ptr, 0); //period unlocked/us
   ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period locked/us
 
   //  ptr += ubx_put_u32 (ptr, 0);  //pulse width unlocked/us
-  ptr += ubx_put_u32 (ptr, 100000 / ref_hz); //pulse width unlocked/us
-  ptr += ubx_put_u32 (ptr, 100000 / ref_hz); //pulse width locked/us
+  ptr += ubx_put_u32 (ptr, 0); //pulse width unlocked/us
+  ptr += ubx_put_u32 (ptr, 1000 / ref_hz); //pulse width locked/us
 
   ptr += ubx_put_i32 (ptr, 0);  // ? delay
-
-#if 0
-  /*Separate numbers for locked/unlocked*/
   ptr += ubx_put_u32 (ptr, 0xf7);
-#else
-  /*Same numbers for locked/unlocked*/
-  ptr += ubx_put_u32 (ptr, 0xf7);
-#endif
-
 
   ret = ubx_handshake (0x06, 0x31, buf, (unsigned) (ptr - buf));
 
@@ -833,7 +1069,7 @@ int
 gps_init (void)
 {
   uint8_t buf[80], *ptr;
-  unsigned len;
+  //unsigned len;
   //  uint16_t u2;
 
   usart1_drain();
@@ -911,27 +1147,36 @@ gps_init (void)
 
   gps_set_ref (1);
 
+#if 0
   ubx_fetch (0x06, 0x31, NULL, 0, &len);
-
   printf ("configured GNSS 6.31\r\n");
+#endif
 
 
   ubx_set_nav_rate (1000);
 
 #if 1
+  ubx_set_message_rate_port1 (0x01, 0x02, 10);
   ubx_set_message_rate_port1 (0x01, 0x03, 1);
   ubx_set_message_rate_port1 (0x01, 0x06, 0);
   ubx_set_message_rate_port1 (0x01, 0x21, 1);
   ubx_set_message_rate_port1 (0x01, 0x22, 1);
-  ubx_set_message_rate_port1 (0x01, 0x32, 0);
 
-  ubx_set_message_rate_port1 (0x01, 0x32, 0); /*SBAS*/
-  ubx_set_message_rate_port1 (0x01, 0x35, 0); /*SV in view*/
+  ubx_set_message_rate_port1 (0x01, 0x30, 10); /*SV in view*/
+  ubx_set_message_rate_port1 (0x01, 0x32, 10); /*SBAS*/
 
-  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
-  ubx_set_message_rate_port1 (0x03, 0x10, 0);
-  ubx_set_message_rate_port1 (0x03, 0x21, 0);
-  ubx_set_message_rate_port1 (0x04, 0x04, 0);
+  //  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
+  //  ubx_set_message_rate_port1 (0x03, 0x10, 0);
+  // ubx_set_message_rate_port1 (0x03, 0x21, 0); //?
+  //  ubx_set_message_rate_port1 (0x04, 0x04, 0);
+
+  ubx_set_message_rate_port1 (0x0D, 0x04, 10);
+
+  ubx_cfg_sbas (1, 0x7, 2);
+
+  ubx_cfg_poll (0x6, 0x16);
+
+//  ubx_aid_ini (522202400, 1279080, 2900, 20000);
 
   printf ("GNSS ready\r\n");
 #else
@@ -944,19 +1189,6 @@ gps_init (void)
   printf ("GNSS ready\r\n");
 #endif
 
-#if 0
-  ptr = buf;
-  ptr += ubx_put_u8 (ptr, 0x1);
-  ptr += ubx_put_u8 (ptr, 0x7);
-  ptr += ubx_put_u8 (ptr, 0x1);
-  ptr += ubx_put_u8 (ptr, 0xf);
-  ptr += ubx_put_u32 (ptr, 0xffffffff);
-  ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));
-
-  printf ("configured SBAS\r\n");
-
-  ubx_handshake (0x06, 0x16, buf, 0);
-#endif
 
 
   //  printf ("GPS ready\r\n");