diff options
Diffstat (limited to 'app/gps.c')
| -rw-r--r-- | app/gps.c | 316 |
1 files changed, 277 insertions, 39 deletions
@@ -1,12 +1,15 @@ #include "project.h" -#define PPS (GPIO2) -#define PPS_PORT GPIOD +#define PPS (GPIO9) +#define PPS_PORT GPIOC #define UBX_BUF_LEN 256 #define TIMEOUT 4000 + +static EPOCH next_sec; + static int ubx_ack = 0; static int ubx_ack_xfer = 0; @@ -24,22 +27,24 @@ static const int fish[] = { 1, 2, 3, 4, 5, 6, 7 }; static Event_ring gps_ring; +static char gps_info[60]; -void exti2_isr (void) + +void exti9_5_isr (void) { uint32_t now = SCS_DWT_CYCCNT; int v; v = !!gpio_get (PPS_PORT, PPS); - nvic_disable_irq (NVIC_EXTI2_IRQ); - exti_reset_request (EXTI2); + nvic_disable_irq (NVIC_EXTI9_5_IRQ); + exti_reset_request (EXTI9); gps_ring.events[gps_ring.tx_ptr].when = now; gps_ring.events[gps_ring.tx_ptr].value = v; gps_ring.tx_ptr = (gps_ring.tx_ptr + 1) & ERING_MASK; - nvic_enable_irq (NVIC_EXTI2_IRQ); + nvic_enable_irq (NVIC_EXTI9_5_IRQ); } @@ -48,19 +53,51 @@ void exti2_isr (void) static inline int ubx_recv_byte (uint8_t *v) { - return !ring_read_byte (&rx3_ring, v); + return !ring_read_byte (&rx1_ring, v); } static inline void ubx_send_byte (uint8_t v) { - usart3_queue (v); + usart1_queue (v); +} + +#define ALMANAC_SIZE + +static int ubx_recv_almanac (uint8_t *ptr, unsigned len) +{ + uint32_t sv, week; + uint32_t almanac[8]; + + ptr += ubx_get_u32 (ptr, &sv); + len -= 4; + ptr += ubx_get_u32 (ptr, &week); + len -= 4; + + if (len > sizeof (almanac)) + len = sizeof (almanac); + + bzero (almanac, sizeof (almanac)); + memcpy (almanac, ptr, len); + + printf (" Almanac: %2d %d %06x %06x %06x %06x %06x %06x %06x %06x\r\n", + (int) sv, (int) week, + (unsigned) almanac[0], + (unsigned) almanac[1], + (unsigned) almanac[2], + (unsigned) almanac[3], + (unsigned) almanac[4], + (unsigned) almanac[5], + (unsigned) almanac[6], + (unsigned) almanac[7]); + + return 0; } static int ubx_recv_nav_status (uint8_t *ptr, unsigned len) { - uint8_t gps_fix, flags; + uint8_t gps_fix, flags, fix_stat, flags2; uint32_t d; if ((!ptr) || (len != 16)) @@ -69,6 +106,8 @@ ubx_recv_nav_status (uint8_t *ptr, unsigned len) ptr += ubx_get_u32 (ptr, &d); //TOW ptr += ubx_get_u8 (ptr, &gps_fix); //fix type ptr += ubx_get_u8 (ptr, &flags); //fix type + ptr += ubx_get_u8 (ptr, &fix_stat); + ptr += ubx_get_u8 (ptr, &flags2); switch (gps_fix) { @@ -170,7 +209,7 @@ ubx_recv_utc (uint8_t *ptr, unsigned len) int32_t nano; uint32_t acc; uint16_t year; - uint8_t hour, min, sec, day, month; + uint8_t hour, min, sec, day, month, valid; uint32_t d; // char buf[40]; @@ -186,15 +225,31 @@ ubx_recv_utc (uint8_t *ptr, unsigned len) ptr += ubx_get_u8 (ptr, &hour); ptr += ubx_get_u8 (ptr, &min); ptr += ubx_get_u8 (ptr, &sec); + ptr += ubx_get_u8 (ptr, &valid); - printf ("GPS META-DATA %04d-%02d-%02d %02d:%02d:%02d Fix:%c%c TXCO %+8dE-12\r\n", - year, - month, - day, - hour, - min, - sec, +#if 0 + printf ("GPS META-DATA %04d-%02d-%02d %02d:%02d:%02d V:%02x Fix:%c%c TXCO %+8dE-12\r\n", + (int) year, + (int) month, + (int) day, + (int) hour, + (int) min, + (int) sec, + (unsigde) valid, fix, fix2, (int) freq); +#endif + + sprintf (gps_info, " %04d-%02d-%02d %02d:%02d:%02d V:%02x Fix:%c%c TXCO %+8dE-12", + + (int)year, + (int)month, + (int)day, + (int)hour, + (int)min, + (int)sec, + (unsigned) valid, fix, fix2, (int) freq); + + if (gps_happy > 3) { UTC u; @@ -213,6 +268,7 @@ ubx_recv_utc (uint8_t *ptr, unsigned len) gps_time = time_utc_to_epoch (u); + next_sec = gps_time; now = SCS_DWT_CYCCNT; abs = abs_extend (now); @@ -234,6 +290,60 @@ ubx_recv_utc (uint8_t *ptr, unsigned len) return 0; } + + +static int ubx_recv_nav_sbas (uint8_t *ptr, unsigned len) +{ + uint8_t prn, mode, service; + int8_t sys; + uint8_t n; + + ptr += 4; + + ptr += ubx_get_u8 (ptr, &prn); + ptr += ubx_get_u8 (ptr, &mode); + ptr += ubx_get_i8 (ptr, &sys); + ptr += ubx_get_u8 (ptr, &service); + + 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); + + while (n--) { + ptr += ubx_get_u8 (ptr, &prn); + ptr += 11; + printf ("GPS SBAS SV:%d\r\n", prn); + } + + return 0; +} + +static int ubx_recv_nav_sat (uint8_t *ptr, unsigned len) +{ + uint8_t gnssid, prn, cno, ver; + uint8_t n; + + ptr += 4; + + ptr += ubx_get_u8 (ptr, &ver); + ptr += ubx_get_u8 (ptr, &n); + ptr += 2; + + while (n--) { + ptr += ubx_get_u8 (ptr, &gnssid); + ptr += ubx_get_u8 (ptr, &prn); + ptr += ubx_get_u8 (ptr, &cno); + ptr += 9; + + printf ("GPS GNSS:%2d PRN:%3d CNO:%3d\r\n", gnssid, prn, cno); + } + + + return 0; +} + + static int ubx_recv_rinex (uint8_t *payload, unsigned len) { @@ -263,6 +373,14 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len) ubx_recv_clock_stats (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); hexdump (payload, len); @@ -280,6 +398,20 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len) break; + case 4: + switch (id) { + case 0x4: + break; + + default: + printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len); + hexdump (payload, len); + } + + break; + + + case 5: ubx_ack++; printf (" %s for %02x.%02x\r\n", id ? "ACK" : "NAK", payload[0], @@ -289,6 +421,10 @@ ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len) case 0x0b: switch (id) { + case 0x30: + ubx_recv_almanac (payload, len); + break; + case 0x50: printf ("xfer ack\r\n"); ubx_ack_xfer++; @@ -450,6 +586,9 @@ static void gps_pps_dispatch (void) v = gps_ring.events[gps_ring.rx_ptr].value; now = gps_ring.events[gps_ring.rx_ptr].when; + if (gps_happy) + led3_set (v); + gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK; @@ -459,14 +598,16 @@ static void gps_pps_dispatch (void) - if (gps_happy) + if (gps_happy > 30) pll_dispatch (gps_last_happy, abs, "GPS"); e = pll_decompose (abs); //u = time_epoch_to_utc (e); - time_print_epoch ("GPS : ", e); + //time_print_epoch ("GPS : ", e, gps_info); + + report_time ("GPS", next_sec, abs, gps_info); } @@ -476,6 +617,7 @@ gps_dispatch (void) { ubx_dispatch_search (-1, -1, NULL); gps_pps_dispatch(); + } static void @@ -525,7 +667,7 @@ ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len) if (ticks > timeout) { printf ("GPS timeout resending packet\r\n"); - usart3_drain(); + usart1_drain(); ubx_send (class, id, payload, len); timeout = ticks + TIMEOUT; } @@ -546,7 +688,7 @@ ubx_handshake_xfer (uint8_t class, uint8_t id, const void *payload, uint32_t timeout; ubx_ack_xfer = 0; - // usart3_drain(); + // usart1_drain(); ubx_send (class, id, payload, len); @@ -557,7 +699,7 @@ ubx_handshake_xfer (uint8_t class, uint8_t id, const void *payload, if (ticks > timeout) { printf ("GPS timeout resending packet\r\n"); - // usart3_drain(); + // usart1_drain(); ubx_send (class, id, payload, len); timeout = ticks + TIMEOUT; } @@ -580,7 +722,7 @@ ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len, { uint8_t *ret; - while (!ring_empty (&tx3_ring) || !ring_empty (&rx3_ring)) + while (!ring_empty (&tx1_ring) || !ring_empty (&rx1_ring)) gps_dispatch(); ubx_send (class, id, payload, len); @@ -591,6 +733,19 @@ ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len, return ret; } +static int ubx_set_nav_rate (uint16_t ms) +{ + uint8_t buf[6], *ptr; + + ptr = buf; + + ptr += ubx_put_u16 (ptr, ms); + ptr += ubx_put_u16 (ptr, 1); /*1:1*/ + ptr += ubx_put_u16 (ptr, 0); /*UTC*/ + + return ubx_handshake (0x06, 0x08, buf, (unsigned) (ptr - buf)); +} + static int ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate) { @@ -640,7 +795,7 @@ gps_set_ref (int ref_hz) ptr = buf; ptr += ubx_put_u8 (ptr, 0); //timepluse 1 - ptr += ubx_put_u8 (ptr, 0); //reserved + 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 @@ -650,8 +805,8 @@ gps_set_ref (int ref_hz) 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, 500000 / ref_hz); //pulse width unlocked/us - ptr += ubx_put_u32 (ptr, 500000 / ref_hz); //pulse width locked/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_i32 (ptr, 0); // ? delay @@ -660,7 +815,7 @@ gps_set_ref (int ref_hz) ptr += ubx_put_u32 (ptr, 0xf7); #else /*Same numbers for locked/unlocked*/ - ptr += ubx_put_u32 (ptr, 0xf3); + ptr += ubx_put_u32 (ptr, 0xf7); #endif @@ -677,15 +832,16 @@ gps_init (void) unsigned len; // uint16_t u2; - usart3_drain(); + usart1_drain(); - while (!ring_empty (&tx3_ring) || !ring_empty (&rx3_ring)) + while (!ring_empty (&tx1_ring) || !ring_empty (&rx1_ring)) gps_dispatch(); printf ("Testing GNSS...\r\n"); ubx_handshake (0x06, 0x00, NULL, 0); printf ("GNSS there\r\n"); + // Set up port ptr = buf; ptr += ubx_put_u8 (ptr, 1); //uart1 @@ -693,7 +849,7 @@ gps_init (void) ptr += ubx_put_u16 (ptr, 0x0); //flow control off ptr += ubx_put_u32 (ptr, 0x8c0); //no parity, 8 bits ptr += ubx_put_u32 (ptr, 9600); // baudrate - ptr += ubx_put_u16 (ptr, 0x7); // receive RTCM, NMEA, UBX + ptr += ubx_put_u16 (ptr, 0x3); // receive NMEA, UBX ptr += ubx_put_u16 (ptr, 0x1); // transmit UBX ptr += ubx_put_u16 (ptr, 0x0); // no txtimeout ptr += ubx_put_u16 (ptr, 0x0); // reserved @@ -701,7 +857,7 @@ gps_init (void) printf ("configured GNSS protocol\r\n"); -#if 1 +#if 0 ptr = buf; ptr += ubx_put_u16 (ptr, 0x14); ptr += ubx_put_u16 (ptr, 0x00); @@ -749,17 +905,30 @@ gps_init (void) printf ("configured GNSS data\r\n"); + gps_set_ref (1); + ubx_fetch (0x06, 0x31, NULL, 0, &len); printf ("configured GNSS 6.31\r\n"); + + ubx_set_nav_rate (1000); + #if 1 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 (0x03, 0x0a, 0); - //ubx_set_message_rate_port1 (0x03, 0x10, 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); + printf ("GNSS ready\r\n"); #else ubx_set_message_rate_port1 (0x01, 0x03, 0); @@ -771,7 +940,7 @@ gps_init (void) printf ("GNSS ready\r\n"); #endif -#if 1 +#if 0 ptr = buf; ptr += ubx_put_u8 (ptr, 0x1); ptr += ubx_put_u8 (ptr, 0x7); @@ -791,16 +960,16 @@ gps_init (void) MAP_INPUT (PPS); - exti_select_source (EXTI2, PPS_PORT); - exti_set_trigger (EXTI2, EXTI_TRIGGER_BOTH); - exti_enable_request (EXTI2); - nvic_enable_irq (NVIC_EXTI2_IRQ); + exti_select_source (EXTI9, PPS_PORT); + exti_set_trigger (EXTI9, EXTI_TRIGGER_BOTH); + exti_enable_request (EXTI9); + nvic_enable_irq (NVIC_EXTI9_5_IRQ); return 0; } -#define ALMANAC_LUMP 128 +#define ALMANAC_LUMP 64 int gps_almanac (void) @@ -831,6 +1000,75 @@ gps_almanac (void) return 0; } +void gps_dump_almanac (void) +{ + ubx_send (0xb, 0x30, NULL, 0); +} + + +#if 1 +int gps_bs (void) +{ + uint32_t now = SCS_DWT_CYCCNT; + uint64_t abs = abs_extend (now); + EPOCH e = pll_decompose (abs); + UTC u = time_epoch_to_utc (e); + + uint8_t buf[80], *ptr; + int ret; + + ptr = buf; + + ptr += ubx_put_u32 (ptr, 391706007); /*19 sleaford st*/ + ptr += ubx_put_u32 (ptr, 955140); + ptr += ubx_put_u32 (ptr, 501672842); + ptr += ubx_put_u32 (ptr, 100000); /*1km std dev*/ + + ptr += ubx_put_u16 (ptr, 0); /*no time mark*/ + + ptr += ubx_put_u16 (ptr, (u.year - 2000) * 100 + u.month); + ptr += ubx_put_u32 (ptr, (u.mday * 1000000) + (u.hour * 10000) + (u.minute * 100) + u.second); + ptr += ubx_put_u32 (ptr, u.nanosecond); + + ptr += ubx_put_u32 (ptr, 2000); /* time good to 2s */ + ptr += ubx_put_u32 (ptr, 0); + + ptr += ubx_put_u32 (ptr, 0); + ptr += ubx_put_u32 (ptr, 0); + + ptr += ubx_put_u32 (ptr, 0x403); + + printf ("Bootstrapping GPS\r\n"); + + hexdump (buf, (unsigned) (ptr - buf)); + + ubx_send (0x0b, 0x01, buf, (unsigned) (ptr - buf)); + + + ret = 0; + + + /* + 00000: 97 f5 58 17 + 04 93 0e 00 + 8a eb e6 1d + a0 86 01 00 + 00010: 00 00 + 6f 07 19/03 + 03 cb 06 01 /17 22:24:03 + 65 ca af 02 45075045ns + + d0 07 00 00 2000ms + 00020: 00 00 00 00 0ns + 00 00 00 00 + 00 00 00 00 + 03 04 00 00 + */ + + return ret; +} + +#endif void @@ -846,7 +1084,7 @@ gps_reset (void) ubx_cfg_rst (REPHEMERIDIES | RALMANAC | RPOS | RRTC); delay_ms (1000); - usart3_drain(); + usart1_drain(); printf ("Testing GNSS...\r\n"); |