#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 (&rx1_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 = abs_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 = abs_extend (now); if (gps_happy) pll_dispatch (gps_last_happy, abs, "GPS"); e = pll_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 (&tx1_ring) || !ring_empty (&rx1_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 (&tx1_ring) || !ring_empty (&rx1_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 = 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 int gps_bs(void) { uint32_t now = HW_CLOCK_REG; 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_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