From 8c7ee88332652e7e79f6c1e4baacabe2183f7e8e Mon Sep 17 00:00:00 2001 From: root Date: Tue, 2 Mar 2021 12:54:03 +0000 Subject: working, with hybrid FLL/PLL, new refclk input and support for max7219 displays, neo 5 and neo 7 and a bazillion other fixes --- app/ATTIC/gps_neo8.c | 1204 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1204 insertions(+) create mode 100644 app/ATTIC/gps_neo8.c (limited to 'app/ATTIC/gps_neo8.c') diff --git a/app/ATTIC/gps_neo8.c b/app/ATTIC/gps_neo8.c new file mode 100644 index 0000000..17118ed --- /dev/null +++ b/app/ATTIC/gps_neo8.c @@ -0,0 +1,1204 @@ +#include "project.h" + +#define PPS (GPIO9) +#define PPS_PORT GPIOC + +#define UBX_BUF_LEN 256 + +#define TIMEOUT 4000 + +static int ubx_ack = 0; +static int ubx_ack_xfer = 0; + +static int current_ref_hz = 1; + +static int gps_locked; +static int gps_happy; + +uint64_t gps_last_happy; + +static char fix, fix2; +static int32_t freq = 0; + +static const int fish[] = { 1, 2, 3, 4, 5, 6, 7 }; + +static Event_ring gps_ring; + +static char gps_info[60]; + + +void exti9_5_isr (void) +{ + uint32_t now = HW_CLOCK_REG; + int v; + + v = !!gpio_get (PPS_PORT, PPS); + + 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_EXTI9_5_IRQ); + +} + + + +static inline int +ubx_recv_byte (uint8_t *v) +{ + return !ring_read_byte (&usart1_rx_ring, v); +} + +static inline void +ubx_send_byte (uint8_t 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,fix_stat,flags2; + uint32_t d; + + if ((!ptr) || (len != 16)) + return -1; + + 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) { + case 0: + case 1: + fix = '-'; + gps_locked = 0; + break; + + case 2: + fix = '2'; + gps_locked = 0; + break; + + case 3: + fix = 'L'; + gps_locked = 1; + break; + + case 4: + fix = 'R'; + gps_locked = 0; + break; + + case 5: + fix = 'T'; + gps_locked = 0; + break; + + default: + fix = '?'; + gps_locked = 0; + } + + switch (flags & 3) { + case 0: + case 2: + fix2 = '!'; + break; + + case 1: + fix2 = '-'; + break; + + case 3: + fix2 = 'D'; + + if (gps_locked == 1) + gps_locked = 2; + + break; + + default: + fix2 = '?'; + } + + + // printf ("fix: %c%c\r\n",fix,fix2); + + + if ((gps_locked) && (gps_happy < 10000)) + gps_happy++; + else + gps_happy = 0; + + return 0; +} + + +static int +ubx_recv_clock_stats (uint8_t *ptr, unsigned len) +{ + //char buf[40]; + int32_t drift; + uint32_t d; + + if ((!ptr) || (len != 20)) + return -1; + + ptr += ubx_get_u32 (ptr, &d); //TOW + ptr += ubx_get_u32 (ptr, &d); //bias + ptr += ubx_get_i32 (ptr, &drift); //drift + ptr += ubx_get_u32 (ptr, &d); //time acc estimate + ptr += ubx_get_i32 (ptr, &freq); //freq acc estimate + + // printf ("TCXO %+8dE-12\r\n", (int) freq); +#if 0 + sprintf (buf, "TCXO %+8dE-12", (int) freq); + lcd_erase_line (18, 0); + lcd_write (buf, 0, 0); +#endif + return 0; + +} + +static int +ubx_recv_utc (uint8_t *ptr, unsigned len) +{ + int32_t nano; + uint32_t acc; + 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, &acc); //bias + ptr += ubx_get_i32 (ptr, &nano); + ptr += ubx_get_u16 (ptr, &year); + ptr += ubx_get_u8 (ptr, &month); + ptr += ubx_get_u8 (ptr, &day); + ptr += ubx_get_u8 (ptr, &hour); + ptr += ubx_get_u8 (ptr, &min); + ptr += ubx_get_u8 (ptr, &sec); + ptr += ubx_get_u8 (ptr, &valid); + +#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; + EPOCH gps_time; + uint32_t now; + uint64_t abs; + + u.jday = 0; + u.year = year; + u.month = month; + u.mday = day; + u.hour = hour; + u.minute = min; + u.second = sec; + u.nanosecond = 0; + + gps_time = time_utc_to_epoch (u); + + + now = HW_CLOCK_REG; + abs = ref_extend (now); + + gps_last_happy = make_happy (abs, 180); + + pll_set_offset (gps_time, abs); + + } + + +#if 0 + sprintf (buf, "%+6dE-12 %02d%02d%02d", (int) freq, + (int) hour, (int)min, (int) sec); + lcd_erase_line (18, 0); + lcd_write (buf, 0, 0); +#endif + + 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) +{ + + printf ("Rinex\r\n"); + 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); + + switch (class) { + case 1: + switch (id) { + case 0x3: + ubx_recv_nav_status (payload, len); + break; + + case 0x21: + ubx_recv_utc (payload, len); + break; + + case 0x22: + 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); + } + + break; + + case 2: + switch (id) { + case 0x10: + ubx_recv_rinex (payload, len); + + break; + } + + 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], + payload[1]); + + break; + + case 0x0b: + switch (id) { + case 0x30: + ubx_recv_almanac(payload,len); + break; + case 0x50: + printf ("xfer ack\r\n"); + ubx_ack_xfer++; + break; + + default: + printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len); + hexdump (payload, len); + } + + break; + + default: + printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len); + + if (class != 0x03) + hexdump (payload, len); + } + + +} + + +typedef enum { + UBX_SM_LOST = 0, + UBX_SM_S2, + UBX_SM_C, + UBX_SM_I, + UBX_SM_L1, + UBX_SM_L2, + UBX_SM_DATA, + UBX_SM_CKA, + UBX_SM_CKB +} ubx_sm_t; + + + + +static uint8_t * +ubx_dispatch_search (int s_class, int s_id, unsigned *len_ptr) +{ + static uint8_t buf[UBX_BUF_LEN]; + static ubx_sm_t sm = UBX_SM_LOST; + static uint8_t class, id; + static unsigned ptr; + static unsigned len; + static uint8_t ck_a, ck_b; + + + void ubx_ck (uint8_t v) { + ck_a += v; + ck_b = ck_b + ck_a; + } + + + uint8_t c; + + while (ubx_recv_byte (&c)) { + + switch (sm) { + case UBX_SM_LOST: + if (c == 0xb5) + sm = UBX_SM_S2; + + break; + + case UBX_SM_S2: + if (c == 0x62) + sm = UBX_SM_C; + else + sm = UBX_SM_LOST; + + break; + + case UBX_SM_C: + ck_a = ck_b = class = c; + sm = UBX_SM_I; + break; + + case UBX_SM_I: + ubx_ck (c); + id = c; + sm = UBX_SM_L1; + break; + + case UBX_SM_L1: + ubx_ck (c); + len = c; + sm = UBX_SM_L2; + break; + + case UBX_SM_L2: + ubx_ck (c); + len |= c << 8; + + ptr = 0; + + if (len) + sm = UBX_SM_DATA; + else + sm = UBX_SM_CKA; + + break; + + case UBX_SM_DATA: + ubx_ck (c); + + if (ptr < UBX_BUF_LEN) + buf[ptr] = c; + + ptr++; + + if (ptr == len) + sm = UBX_SM_CKA; + + break; + + case UBX_SM_CKA: + if (c == ck_a) + sm = UBX_SM_CKB; + else + sm = UBX_SM_LOST; + + break; + + case UBX_SM_CKB: + sm = UBX_SM_LOST; + + if (c != ck_b) + break; + + ubx_recv (class, id, buf, len); + + if ((class == s_class) && (id == s_id)) { + if (len_ptr) + *len_ptr = len; + + return buf; + } + + break; + } + + } + + return NULL; +} +static void gps_pps_dispatch (void) +{ + //char buf[80]; + uint32_t now; + uint64_t abs; + int v; + EPOCH e; + //UTC u; + + if (gps_ring.rx_ptr == gps_ring.tx_ptr) return; + + v = gps_ring.events[gps_ring.rx_ptr].value; + now = gps_ring.events[gps_ring.rx_ptr].when; + + led3_set(v); + + gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK; + + + if (!v) return; + + abs = ref_extend (now); + + if (gps_happy) + pll_dispatch (gps_last_happy, abs, "GPS"); + + e = ref_decompose (abs); + + + //u = time_epoch_to_utc (e); + time_print_epoch ("GPS : ", e, gps_info); + +} + + +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) +{ + uint8_t ck_a = 0, ck_b = 0; + uint8_t *payload = (uint8_t *) _payload; + + void ubx_send_byte_ck (uint8_t v) { + ubx_send_byte (v); + ck_a += v; + ck_b = ck_b + ck_a; + } + + + ubx_send_byte (0xb5); + ubx_send_byte (0x62); + + ubx_send_byte_ck (class); + ubx_send_byte_ck (id); + ubx_send_byte_ck (len & 0xff); + ubx_send_byte_ck (len >> 8); + + while (len--) + ubx_send_byte_ck (* (payload++)); + + ubx_send_byte (ck_a); + ubx_send_byte (ck_b); + + // 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; + + ubx_ack = 0; + ubx_send (class, id, payload, len); + + + + timeout = ticks + TIMEOUT; + + while (!ubx_ack) { + + if (ticks > timeout) { + printf ("GPS timeout resending packet\r\n"); + usart1_drain(); + ubx_send (class, id, payload, len); + timeout = ticks + TIMEOUT; + } + + + + + gps_dispatch(); + } + + return 0; +} + +static int +ubx_handshake_xfer (uint8_t class, uint8_t id, const void *payload, + unsigned len) +{ + uint32_t timeout; + + ubx_ack_xfer = 0; + // usart1_drain(); + ubx_send (class, id, payload, len); + + + + timeout = ticks + TIMEOUT; + + while (!ubx_ack_xfer) { + + if (ticks > timeout) { + printf ("GPS timeout resending packet\r\n"); + // usart1_drain(); + ubx_send (class, id, payload, len); + timeout = ticks + TIMEOUT; + } + + + + + gps_dispatch(); + } + + return 0; +} + + + + +static uint8_t * +ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len, + unsigned *len_ptr) +{ + uint8_t *ret; + + while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring)) + gps_dispatch(); + + ubx_send (class, id, payload, len); + + while (! (ret = ubx_dispatch_search (class, id, len_ptr))); + + + 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) +{ + uint8_t buf[8], *ptr; + + ptr = buf; + + ptr += ubx_put_u8 (ptr, class); + ptr += ubx_put_u8 (ptr, id); //reserved + ptr += ubx_put_u8 (ptr, 0); //nothing on port i2c + ptr += ubx_put_u8 (ptr, rate); //rate on uart + ptr += ubx_put_u8 (ptr, 0); //nothing on port 2 + ptr += ubx_put_u8 (ptr, 0); //nothing on usb + 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)); +} + + +static int +ubx_cfg_rst (uint16_t flags) +{ + uint8_t buf[8], *ptr; + + ptr = buf; + + ptr += ubx_put_u16 (ptr, flags); //Flags + ptr += ubx_put_u8 (ptr, 0x4); //Hardware reset after shutdown + ptr += ubx_put_u8 (ptr, 0); //reserved + + ubx_send (0x06, 0x04, buf, (unsigned) (ptr - buf)); + return 0; +} + + +static int +gps_set_ref (int ref_hz) +{ + uint8_t buf[80], *ptr; + int ret; + + printf ("setting gps ref to %d hz\r\n", ref_hz); + + current_ref_hz = ref_hz; + + 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, 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_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)); + + return ret; +} + + +static inline int +ubx_put_gnss_cfg (uint8_t *buf, uint8_t gnss_id,uint8_t r_chan,uint8_t m_chan, +uint16_t enable, uint16_t sig_mask) +{ + uint8_t *ptr=buf; + + ptr += ubx_put_u8(ptr,gnss_id); + ptr += ubx_put_u8(ptr,r_chan); + ptr += ubx_put_u8(ptr,m_chan); + ptr += ubx_put_u8(ptr,0); + ptr += ubx_put_u16(ptr,enable); + ptr += ubx_put_u16(ptr,sig_mask); + + return (unsigned) (ptr - buf); +} + +void gps_set_gnss(void) +{ + uint8_t buf[80], *ptr; + + ptr = buf; + ptr += ubx_put_u8 (ptr, 0x0); /* Ver 0 */ + ptr += ubx_put_u8 (ptr, 0x20); + ptr += ubx_put_u8 (ptr, 0x20); + ptr += ubx_put_u8 (ptr, 0x7); + + ptr += ubx_put_gnss_cfg(ptr,0,8,16,1,0x101); + ptr += ubx_put_gnss_cfg(ptr,1,1,3,1,0x101); + ptr += ubx_put_gnss_cfg(ptr,2,4,8,0,0x101); + ptr += ubx_put_gnss_cfg(ptr,3,8,16,0,0x101); + ptr += ubx_put_gnss_cfg(ptr,4,0,7,0,0x301); + ptr += ubx_put_gnss_cfg(ptr,5,0,3,1,0x501); + ptr += ubx_put_gnss_cfg(ptr,6,8,14,1,0x101); + + ubx_handshake (0x06, 0x3e, buf, (unsigned) (ptr - buf)); + + printf ("configured SBAS\r\n"); + + ubx_handshake (0x06, 0x3e, buf, 0); +} + + +int +gps_init (void) +{ + uint8_t buf[80], *ptr; + unsigned len; + // uint16_t u2; + + usart1_drain(); + + while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring)) + gps_dispatch(); + + + printf ("Testing GNSS...\r\n"); + ubx_handshake (0x06, 0x00, NULL, 0); + printf ("GNSS there\r\n"); + + //reset GNSS + +#if 0 + ptr=buf; + ptr += ubx_put_u32 (ptr, 0xe1f ); //uart1 + ptr += ubx_put_u32 (ptr, 0 ); //uart1 + ptr += ubx_put_u32 (ptr, 0 ); //uart1 + ubx_handshake (0x06, 0x09, buf, (unsigned) (ptr - buf)); + + printf ("reset\r\n"); +#endif + + + // Set up port + ptr = buf; + ptr += ubx_put_u8 (ptr, 1); //uart1 + ptr += ubx_put_u8 (ptr, 0); //reserved + 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, 0x1); // transmit UBX + ptr += ubx_put_u16 (ptr, 0x0); // no txtimeout + ptr += ubx_put_u16 (ptr, 0x0); // reserved + ubx_handshake (0x06, 0x00, buf, (unsigned) (ptr - buf)); + + printf ("configured GNSS protocol\r\n"); + + +#if 0 + ptr = buf; + ptr += ubx_put_u16 (ptr, 0x1b); + ptr += ubx_put_u16 (ptr, 0x00); + ubx_handshake (0x06, 0x13, buf, (unsigned) (ptr - buf)); + + printf ("configured antenna pins\r\n"); +#endif + + + +#if 0 + // Check we're in WGS84 + ptr = ubx_fetch (0x06, 0x06, NULL, 0, &len); + ptr += ubx_get_u16 (ptr, &u2); + + if (u2) + return -1; + + printf ("configured GNSS datum\r\n"); +#endif + + ptr = buf; + + ptr += ubx_put_u8 (ptr, 0); //UBX + ptr += ubx_put_u8 (ptr, 0); //reserved + ptr += ubx_put_u16 (ptr, 0); //reserved + ptr += ubx_put_u8 (ptr, 0); //nothing on port i2c + ptr += ubx_put_u8 (ptr, 0x18); //everything on uart + ptr += ubx_put_u8 (ptr, 0); //nothing on port 2 + ptr += ubx_put_u8 (ptr, 0); //nothing on usb + ptr += ubx_put_u8 (ptr, 0); //nothing on spi + ptr += ubx_put_u8 (ptr, 0); //nothing on port 5 + + ptr += ubx_put_u8 (ptr, 1); //NMEA + ptr += ubx_put_u8 (ptr, 0); //reserved + ptr += ubx_put_u16 (ptr, 0); //reserved + ptr += ubx_put_u8 (ptr, 0); //nothing on port i2c + ptr += ubx_put_u8 (ptr, 0); //nothing on uart + ptr += ubx_put_u8 (ptr, 0); //nothing on port 2 + ptr += ubx_put_u8 (ptr, 0); //nothing on usb + ptr += ubx_put_u8 (ptr, 0); //nothing on spi + ptr += ubx_put_u8 (ptr, 0); //nothing on port 5 + + ubx_handshake (0x06, 0x02, buf, (unsigned) (ptr - buf)); + + 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, 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, 0x21, 0); + ubx_set_message_rate_port1 (0x04, 0x04, 0); + + printf ("GNSS ready\r\n"); +#else + ubx_set_message_rate_port1 (0x01, 0x03, 0); + ubx_set_message_rate_port1 (0x01, 0x21, 0); + ubx_set_message_rate_port1 (0x01, 0x22, 0); + ubx_set_message_rate_port1 (0x01, 0x32, 0); + ubx_set_message_rate_port1 (0x03, 0x0a, 0); + //ubx_set_message_rate_port1 (0x03, 0x10, 0); + printf ("GNSS ready\r\n"); +#endif + + + + // printf ("GPS ready\r\n"); + // ubx_get_clock_stats(); + + MAP_INPUT (PPS); + + 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 64 + +int +gps_almanac (void) +{ + uint32_t len = almanac_alp_len; + const uint8_t *ptr = almanac_alp; + uint32_t lumpsz; + uint8_t dummy; + + printf ("Downloading GPS almanac %x %x %x\r\n", (int) ptr[0], (int) ptr[1], + (int) ptr[2]); + + + while (len) { + printf ("%d bytes to go\r\n", (int) len); + + lumpsz = len > ALMANAC_LUMP ? ALMANAC_LUMP : len; + + ubx_handshake_xfer (0x0b, 0x50, ptr, lumpsz); + + ptr += lumpsz; + len -= lumpsz; + + } + + ubx_handshake_xfer (0x0b, 0x50, &dummy, 1); + + return 0; +} + +void gps_dump_almanac(void) +{ + ubx_send (0xb, 0x30, NULL,0); +} + + +#if 0 +int gps_bs(void) +{ + uint32_t now = HW_CLOCK_REG; + uint64_t abs = ref_extend (now); + EPOCH e = ref_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 + +int gps_bs(void) +{ + uint32_t now = HW_CLOCK_REG; + uint64_t abs = ref_extend (now); + EPOCH e = ref_decompose (abs); + UTC u = time_epoch_to_utc (e); + + uint8_t buf[80], *ptr; + int ret; + + ptr = buf; + + ptr += ubx_put_u8 (ptr, 0x0); + ptr += ubx_put_u8 (ptr, 0x0); + ptr += ubx_put_u8 (ptr, 0x0); + ptr += ubx_put_u8 (ptr, 0x0); + + 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*/ + + hexdump (buf, (unsigned) (ptr-buf)); + ubx_send (0x13, 0x40, buf, (unsigned) (ptr-buf)); + + + ptr = buf; + + ptr += ubx_put_u8 (ptr, 0x10); + ptr += ubx_put_u8 (ptr, 0x0); + ptr += ubx_put_u8 (ptr, 0x0); + ptr += ubx_put_u8 (ptr, 0x80); + + ptr += ubx_put_u16 (ptr,u.year-2000); + ptr += ubx_put_u8 (ptr,u.month); + ptr += ubx_put_u8 (ptr,u.mday); + + ptr += ubx_put_u8 (ptr,u.hour); + ptr += ubx_put_u8 (ptr,u.minute); + ptr += ubx_put_u8 (ptr,u.second); + ptr += ubx_put_u8 (ptr,0); + + ptr += ubx_put_u32 (ptr,u.nanosecond); + + ptr += ubx_put_u16 (ptr, 2); /* time good to 2s */ + ptr += ubx_put_u8 (ptr,0); + ptr += ubx_put_u8 (ptr,0); + + ptr += ubx_put_u32 (ptr, 0); + + hexdump (buf, (unsigned) (ptr-buf)); + ubx_send (0x13, 0x40, buf, (unsigned) (ptr-buf)); + + + ret = 0; + + return ret; +} + + +void +gps_reset (void) +{ + printf ("Restting gps..\r\n"); + +#define REPHEMERIDIES (1UL << 0) +#define RALMANAC (1UL << 1) +#define RPOS (1UL << 4) +#define RRTC (1UL << 8) + + ubx_cfg_rst (REPHEMERIDIES | RALMANAC | RPOS | RRTC); + + delay_ms (1000); + usart1_drain(); + + + printf ("Testing GNSS...\r\n"); + ubx_handshake (0x06, 0x00, NULL, 0); + printf ("GNSS there\r\n"); + + gps_init(); + + gps_set_ref (current_ref_hz); + + gps_set_gnss(); +} + +#if 0 +static int +ubx_get_clock_stats (void) +{ + uint8_t *ptr; + + unsigned len; + + ptr = ubx_fetch (0x01, 0x22, NULL, 0, &len); + + //return ubx_recv_clock_stats(ptr,len); + return ptr ? 0 : -1; +} +#endif -- cgit v1.2.3