diff options
Diffstat (limited to 'app/gps.c')
| -rw-r--r-- | app/gps.c | 60 |
1 files changed, 45 insertions, 15 deletions
@@ -11,6 +11,14 @@ #define TIMEOUT 4000 +unsigned gps_sats_searching; +unsigned gps_sats_inop; +unsigned gps_sats_locked; +unsigned gps_sats_with_e; +int gps_initting = 1; + +static int chatty_gps = 1; + static EPOCH next_sec; static int ubx_ack = 0; @@ -47,6 +55,7 @@ void exti9_5_isr (void) exti_reset_request (EXTI7); gps_ring.events[gps_ring.tx_ptr].when = now; + gps_ring.events[gps_ring.tx_ptr].ptp = ptp_get(); gps_ring.events[gps_ring.tx_ptr].value = v; gps_ring.tx_ptr = (gps_ring.tx_ptr + 1) & ERING_MASK; @@ -61,7 +70,7 @@ void exti9_5_isr (void) static inline int ubx_recv_byte (uint8_t *v) { - return !ring_read_byte (&rx1_ring, v); + return !ring_read_byte (&usart1_rx_ring, v); } static inline void @@ -311,8 +320,7 @@ ubx_recv_utc (uint8_t *ptr, unsigned len) if (gps_happy > 3) { UTC u; EPOCH gps_time; - uint32_t now; - uint64_t abs; + uint64_t abs, ptp; u.jday = 0; u.year = year; @@ -330,12 +338,12 @@ ubx_recv_utc (uint8_t *ptr, unsigned len) next_sec = gps_time; - now = HW_CLOCK_REG; - abs = abs_extend (now); + abs = ref_get(); + ptp = ptp_get(); gps_last_happy = make_happy (abs, 180); - pll_set_offset (gps_time, abs); + pll_set_offset (gps_time, abs, ptp); } else printf ("!GPS %s\r\n", gps_info); @@ -373,7 +381,7 @@ static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len) int8_t sys; uint8_t n; - if (!chatty_gps) return0 ; + if (!chatty_gps) return 0 ; ptr += ubx_get_u32 (ptr, &d4); ptr += ubx_get_u8 (ptr, &prn); @@ -411,6 +419,8 @@ static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len) ptr += ubx_get_u8 (ptr, &flags); ptr += ubx_get_u16 (ptr, &d2); + gps_sats_searching = gps_sats_inop = gps_sats_locked = gps_sats_with_e = 0; + while (n--) { ptr += ubx_get_u8 (ptr, &ch); ptr += ubx_get_u8 (ptr, &svid); @@ -419,6 +429,7 @@ static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len) ptr += ubx_get_u8 (ptr, &cno); ptr += 7; + switch (quality) { case 0: st = "idle"; @@ -426,30 +437,44 @@ static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len) case 1: st = "search"; + gps_sats_searching++; break; case 2: st = "acq"; + gps_sats_searching++; break; case 3: st = "inop"; + gps_sats_inop++; break; case 4: st = "lock"; + gps_sats_locked++; + + if (cflags & 0x04) + gps_sats_with_e++; + break; case 5: case 6: case 7: st = "good"; + gps_sats_locked++; + + if (cflags & 0x04) + gps_sats_with_e++; + break; default: st = "?"; } + printf ("GPS SVID:%3d C/N:%3d %c%c%c %s\r\n", svid, cno, (cflags & 0x01) ? 'U' : '-', @@ -743,6 +768,7 @@ static void gps_pps_dispatch (void) //char buf[80]; uint32_t now; uint64_t abs; + uint64_t ptp; int v; //EPOCH e; //UTC u; @@ -751,6 +777,7 @@ static void gps_pps_dispatch (void) v = gps_ring.events[gps_ring.rx_ptr].value; now = gps_ring.events[gps_ring.rx_ptr].when; + ptp = gps_ring.events[gps_ring.rx_ptr].ptp; if (gps_happy) led3_set (v); @@ -761,14 +788,16 @@ static void gps_pps_dispatch (void) if (!v) return; - abs = abs_extend (now); + abs = ref_extend (now); - if (gps_happy > 30) - pll_dispatch (gps_last_happy, abs, "GPS"); + if (gps_happy > 30) { + chatty_gps = 0; + pll_dispatch (gps_last_happy, abs, ptp, "GPS"); + } - //e = pll_decompose (abs); + //e = ref_decompose (abs); //u = time_epoch_to_utc (e); @@ -896,7 +925,7 @@ ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len, { uint8_t *ret; - while (!ring_empty (&tx1_ring) || !ring_empty (&rx1_ring)) + while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring)) gps_dispatch(); ubx_send (class, id, payload, len); @@ -1090,7 +1119,7 @@ gps_init (void) usart1_drain(); - while (!ring_empty (&tx1_ring) || !ring_empty (&rx1_ring)) + while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring)) gps_dispatch(); @@ -1195,6 +1224,7 @@ gps_init (void) // ubx_aid_ini (522202400, 1279080, 2900, 20000); printf ("GNSS ready\r\n"); + gps_initting = 0; #else ubx_set_message_rate_port1 (0x01, 0x03, 0); ubx_set_message_rate_port1 (0x01, 0x21, 0); @@ -1267,8 +1297,8 @@ void gps_dump_almanac (void) int gps_bs (void) { uint32_t now = HW_CLOCK_REG; - uint64_t abs = abs_extend (now); - EPOCH e = pll_decompose (abs); + uint64_t abs = ref_extend (now); + EPOCH e = ref_decompose (abs); UTC u = time_epoch_to_utc (e); uint8_t buf[80], *ptr; |