1 files changed, 77 insertions, 12 deletions

diff --git a/app/gps.c b/app/gps.c
index 7327435..a8b8099 100644
--- a/app/gps.c
+++ b/app/gps.c
@@ -111,6 +111,39 @@ static int ubx_recv_almanac (uint8_t *ptr, unsigned len)
   return 0;
 }
 
+
+static int ubx_recv_aid_eph (uint8_t *ptr, unsigned len)
+{
+  unsigned i;
+  uint32_t sv;
+
+  ptr += ubx_get_u32 (ptr, &sv);
+  len -= 4;
+
+  if ((sv<1) ||( sv>32)) return 0;
+
+  i=sv-1;
+  
+
+  ptr += ubx_get_u32 (ptr, &ephemeris[i].how);
+  len -= 4;
+
+  printf("Received %d bytes of ephemeris for SV%d\r\n",len,(int) sv);
+
+  if ((!len) ||( !ephemeris[i].how)) return 0 ;
+
+  memcpy(&ephemeris[i].sf1,ptr,32);
+  len-=32;
+  ptr+=32;
+  memcpy(&ephemeris[i].sf2,ptr,32);
+  len-=32;
+  ptr+=32;
+  memcpy(&ephemeris[i].sf3,ptr,32);
+
+  return 0;
+}
+
+
 double gps_lat, gps_lon;
 
 char gps_pos[128];
@@ -603,7 +636,6 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
     }
 
     break;
-
   case 0xd:
     switch (id) {
     case 0x4:
@@ -637,6 +669,9 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
     case 0x30:
       ubx_recv_almanac (payload, len);
       break;
+	case 0x31:
+ 	ubx_recv_aid_eph(payload,len);
+	break;
 
     case 0x50:
       printf ("xfer ack\r\n");
@@ -815,7 +850,7 @@ static void gps_pps_dispatch (void)
 
 
   if (gps_happy > 30) {
-    chatty_gps = 0;
+    //chatty_gps = 0;
     pll_dispatch (gps_last_happy, abs, ptp, "GPS");
   }
 
@@ -830,14 +865,6 @@ static void gps_pps_dispatch (void)
 }
 
 
-void
-gps_dispatch (void)
-{
-  ubx_dispatch_search (-1, -1, NULL);
-  gps_pps_dispatch();
-
-}
-
 static void
 ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
 {
@@ -869,6 +896,28 @@ ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
 
 }
 
+
+void
+gps_dispatch (void)
+{
+static uint32_t last_eph;
+uint32_t t ;
+
+  ubx_dispatch_search (-1, -1, NULL);
+  gps_pps_dispatch();
+
+
+
+t=ticks-last_eph;
+if (t>60000)  {
+	printf("Requesting ephemeris data\r\n");
+        gps_dump_eph();
+	last_eph=ticks;
+}
+
+}
+
+
 static int
 ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
 {
@@ -1025,8 +1074,18 @@ static int  ubx_cfg_sbas (uint8_t mode, uint8_t usage, uint8_t channels)
   ptr += ubx_put_u8 (ptr, mode);
   ptr += ubx_put_u8 (ptr, usage);
   ptr += ubx_put_u8 (ptr, channels);
-  ptr += ubx_put_u8 (ptr, 0x0);
+#if 0
+  ptr += ubx_put_u8 (ptr, 0x40 /*PRN158*/);
+  ptr += ubx_put_u32 (ptr, /* 0x1 PRN 120 |  */ 0x8 /*PRN 123*/ | /* 0x40  PRN 126  | */   0x10000 /*PRN136*/);
+#else
+#if 1
+  ptr += ubx_put_u8 (ptr, 0x7f);
+  ptr += ubx_put_u32 (ptr, 0xffffffffU);
+#else
+  ptr += ubx_put_u8 (ptr, 0x40 /*PRN158*/);
   ptr += ubx_put_u32 (ptr, /* 0x1 PRN 120 |  */ 0x8 /*PRN 123*/ | /* 0x40  PRN 126  | */   0x10000 /*PRN136*/);
+#endif
+#endif
 
   return ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));
 }
@@ -1239,7 +1298,7 @@ gps_init (void)
 
   ubx_set_message_rate_port1 (0x0D, 0x04, 10);
 
-  ubx_cfg_sbas (1, 0x7, 2);
+  ubx_cfg_sbas (1, 0x7, 3);
 
   ubx_cfg_poll (0x6, 0x16);
 
@@ -1315,6 +1374,12 @@ void gps_dump_almanac (void)
 }
 
 
+void gps_dump_eph (void)
+{
+  ubx_send (0xb, 0x31, NULL, 0);
+}
+
+
 #if 1
 int gps_bs (void)
 {