#include "project.h" #define PPS (GPIO7) #define PPS_PORT GPIOC #define NRESET (GPIO8) #define NRESET_PORT GPIOC #define UBX_BUF_LEN 256 #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; static int ubx_nack = 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; char gps_info[60]; static int ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z, uint32_t var, uint32_t min_dur, uint32_t max_var); 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 (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; if (v) sysclk_event(); 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_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]; static int ubx_recv_nav_posllh (uint8_t *ptr, unsigned len) { uint32_t d; int32_t ilat, ilon, i; if ((!ptr) || (len != 28)) return -1; ptr += ubx_get_u32 (ptr, &d); //TOW ptr += ubx_get_i32 (ptr, &ilon); ptr += ubx_get_i32 (ptr, &ilat); ptr += ubx_get_i32 (ptr, &i); //geoid height ptr += ubx_get_i32 (ptr, &i); //msl height ptr += ubx_get_u32 (ptr, &d); //hacc ptr += ubx_get_u32 (ptr, &d); //vacc gps_lat = 1.e-7 * (double) ilat; gps_lon = 1.e-7 * (double) ilon; printf ("POS: %s %s\r\n", fd1 (gps_lat, 0, 1, 4), fd2 (gps_lon, 0, 1, 4)); snprintf (gps_pos, sizeof (gps_pos) - 1, "location: %s %s", fd1 (gps_lat, 0, 1, 4), fd2 (gps_lon, 0, 1, 4)); return 0; } static int ubx_recv_nav_status (uint8_t *ptr, unsigned len) { uint8_t gps_fix, flags, fix_stat, flags2; uint32_t d; static unsigned kick_off_survey_mode = 40; 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 = 1; kick_off_survey_mode = 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 = '?'; } display_report_fix (fix, fix2); // printf ("fix: %c%c happy %d Fix %02x flags %02x fix_stat %02x flags2 %02x \r\n",fix,fix2,gps_happy, gps_fix, flags,fix_stat,flags2); if ((gps_locked) && (gps_happy < 10000)) gps_happy++; else gps_happy = 0; if (kick_off_survey_mode > 0) { kick_off_survey_mode--; if (!kick_off_survey_mode) { printf ("GPS no time fix - kicking off survey mode\n"); ubx_cfg_tmode (1, 0, 0, 0, 0, 3600, 3600); // 1 hour and 6cm } } 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; } int gps_utc_diff; int gps_wday; static int ubx_recv_utc (uint8_t *ptr, unsigned len) { int32_t nano; uint32_t acc; uint32_t tow; int tod, utc_tod; uint16_t year; uint8_t hour, min, sec, day, month, valid; // char buf[40]; if ((!ptr) || (len != 20)) return -1; ptr += ubx_get_u32 (ptr, &tow); //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; uint64_t abs, ptp; 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); next_sec = gps_time; abs = ref_get(); ptp = ptp_get(); gps_last_happy = make_happy (abs, 180); pll_set_offset (gps_time, abs, ptp); } else printf ("!GPS %s\r\n", gps_info); #define SECONDS_IN_DAY (24*60*60) utc_tod = (((hour * 60) + min) * 60) + sec; tow /= 1000; tod = tow % SECONDS_IN_DAY; gps_wday = tow / SECONDS_IN_DAY; utc_tod += 2 * SECONDS_IN_DAY; utc_tod -= tod; while (utc_tod >= (SECONDS_IN_DAY / 2)) utc_tod -= SECONDS_IN_DAY; gps_utc_diff = utc_tod; #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) { uint32_t d4; uint8_t prn, mode, service; int8_t sys; uint8_t n; if (!chatty_gps) return 0 ; ptr += ubx_get_u32 (ptr, &d4); 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 SVS:", prn, mode, sys, service); while (n--) { ptr += ubx_get_u8 (ptr, &prn); ptr += 11; printf (" %d", prn); } printf ("\r\n"); return 0; } char gps_svinfo[4096]; static int ubx_recv_nav_svinfo (uint8_t *ptr, unsigned len) { uint32_t d4; uint16_t d2; uint8_t ch, quality, cno, svid; uint8_t n, flags, cflags; const char *st; PTB_INIT (gps_svinfo); ptr += ubx_get_u32 (ptr, &d4); ptr += ubx_get_u8 (ptr, &n); 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); ptr += ubx_get_u8 (ptr, &cflags); ptr += ubx_get_u8 (ptr, &quality); ptr += ubx_get_u8 (ptr, &cno); ptr += 7; switch (quality) { case 0: st = "idle"; break; 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 = "?"; } if (chatty_gps) printf ("GPS SVID:%3d C/N:%3d %c%c%c %s\r\n", svid, cno, (cflags & 0x01) ? 'U' : '-', (cflags & 0x02) ? 'D' : '-', (cflags & 0x04) ? 'E' : '-', st); PTB_ADD ("GPS SVID:%3d C/N:%3d %c%c%c %s\n", svid, cno, (cflags & 0x01) ? 'U' : '-', (cflags & 0x02) ? 'D' : '-', (cflags & 0x04) ? 'E' : '-', st); } return 0; } static int ubx_recv_rinex (uint8_t *payload, unsigned len) { printf ("Rinex\r\n"); return 0; } char gps_svin_info[128]; static int ubx_recv_tim_svin (uint8_t *ptr, unsigned len) { uint8_t valid, active; uint32_t dur, obs, var; int32_t x, y, z; ptr += ubx_get_u32 (ptr, &dur); ptr += ubx_get_i32 (ptr, &x); ptr += ubx_get_i32 (ptr, &y); ptr += ubx_get_i32 (ptr, &z); ptr += ubx_get_u32 (ptr, &var); ptr += ubx_get_u32 (ptr, &obs); ptr += ubx_get_u8 (ptr, &valid); ptr += ubx_get_u8 (ptr, &active); printf ("TIM-SVIN dur %u var %u obs %u valid %02x active %02x\r\n", (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active); snprintf (gps_svin_info, sizeof (gps_svin_info) - 1, "TIM-SVIN dur %u var %u obs %u valid %02x active %02x", (unsigned) dur, (unsigned) var, (unsigned) obs, valid, active); display_report_svin (valid, active); 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 0x2: ubx_recv_nav_posllh (payload, len); break; 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 0x30: ubx_recv_nav_svinfo (payload, len); break; case 0x32: ubx_recv_nav_sbas (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 0xd: switch (id) { case 0x4: ubx_recv_tim_svin (payload, len); break; default: printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len); hexdump (payload, len); } break; case 5: ubx_ack++; if (!id) ubx_nack++; 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 0x31: ubx_recv_aid_eph (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; uint64_t ptp; 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; ptp = gps_ring.events[gps_ring.rx_ptr].ptp; if (gps_happy) led3_set (v); gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK; if (!v) return; abs = ref_extend (now); if (gps_happy > 30) { //chatty_gps = 0; pll_dispatch (gps_last_happy, abs, ptp, "GPS"); } //e = ref_decompose (abs); //u = time_epoch_to_utc (e); //time_print_epoch ("GPS : ", e, gps_info); report_time ("GPS", next_sec, abs, gps_info); } 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); } 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) { uint32_t timeout = 4000; unsigned tries = 4; ubx_ack = 0; ubx_nack = 0; ubx_send (class, id, payload, len); while (!ubx_ack) { if (!timeout) { printf ("GPS timeout resending packet\r\n"); if (!tries) { printf ("GPS 3 timesout - resetting system\r\n"); usart2_drain(); scb_reset_system(); } usart1_drain(); ubx_send (class, id, payload, len); timeout = 4000; tries--; } timeout--; gps_dispatch(); delay_ms (1); } return !!ubx_nack; } 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; int ret; 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 ret = ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf)); printf ("GPS set message rate for %02x.%02x %s\r\n", class, id, ret ? "Failed" : "Succeeded"); return ret; } 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 ubx_cfg_poll (uint8_t class, uint8_t id) { return ubx_handshake (class, id, NULL, 0); } static int ubx_cfg_sbas (uint8_t mode, uint8_t usage, uint8_t channels) { uint8_t buf[8], *ptr; ptr = buf; ptr += ubx_put_u8 (ptr, mode); ptr += ubx_put_u8 (ptr, usage); ptr += ubx_put_u8 (ptr, channels); #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)); } static int ubx_cfg_tmode (uint32_t time_mode, int32_t x, int32_t y, int32_t z, uint32_t var, uint32_t min_dur, uint32_t max_var) { uint8_t buf[28], *ptr; ptr = buf; ptr += ubx_put_u32 (ptr, time_mode); ptr += ubx_put_i32 (ptr, x); ptr += ubx_put_i32 (ptr, y); ptr += ubx_put_i32 (ptr, z); ptr += ubx_put_u32 (ptr, var); ptr += ubx_put_u32 (ptr, min_dur); ptr += ubx_put_u32 (ptr, max_var); return ubx_handshake (0x06, 0x1d, buf, (unsigned) (ptr - buf)); } static int ubx_aid_ini (int32_t lat, int32_t lon, int32_t alt, uint32_t acc) { uint8_t buf[48], *ptr; ptr = buf; ptr += ubx_put_i32 (ptr, lat); ptr += ubx_put_i32 (ptr, lon); ptr += ubx_put_i32 (ptr, alt); ptr += ubx_put_u32 (ptr, acc); ptr += ubx_put_u16 (ptr, 0); //time mark flags ptr += ubx_put_u16 (ptr, 0); // week number ptr += ubx_put_u32 (ptr, 0); // tow s ptr += ubx_put_i32 (ptr, 0); // tow ns ptr += ubx_put_u32 (ptr, 0); // tow accuracy ms ptr += ubx_put_u32 (ptr, 0); // tow accuracy ns ptr += ubx_put_i32 (ptr, 0); // clock drift or freq ptr += ubx_put_u32 (ptr, 0); // clock drift or freq accuracy ptr += ubx_put_u32 (ptr, 0x21); //flags LLA no time return ubx_handshake (0x0b, 0x01, buf, (unsigned) (ptr - buf)); //return ubx_send (0x0b, 0x01, NULL, 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); /*Try ublox 5 version first */ current_ref_hz = ref_hz; ptr = buf; ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period /us ptr += ubx_put_u32 (ptr, 1000); //duration /us ptr += ubx_put_i8 (ptr, 1); //+ve sense ptr += ubx_put_u8 (ptr, 1); //lock to utc ptr += ubx_put_u8 (ptr, 0); //pulse only when locked ptr += ubx_put_u8 (ptr, 0); //res /* 50.52 ns from 10m of RG58 */ /* 15.15 ns from 3m of RG174 */ /* total 66ns*/ ptr += ubx_put_i16 (ptr, 66); //antenna cable delay /ns ptr += ubx_put_i16 (ptr, 0); //rf group delay /ns ptr += ubx_put_i32 (ptr, 0); //user delay /ns ret = ubx_handshake (0x06, 0x07, buf, (unsigned) (ptr - buf)); if (!ret) return ret; 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, 0); //ant cable delay ns ptr += ubx_put_u16 (ptr, 0); //rf group delay ns ptr += ubx_put_u32 (ptr, 0); //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, 0); //pulse width unlocked/us ptr += ubx_put_u32 (ptr, 1000 / ref_hz); //pulse width locked/us ptr += ubx_put_i32 (ptr, 0); // ? delay ptr += ubx_put_u32 (ptr, 0xf7); ret = ubx_handshake (0x06, 0x31, buf, (unsigned) (ptr - buf)); return ret; } 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"); // 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, 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 ubx_handshake (0x06, 0x00, buf, (unsigned) (ptr - buf)); printf ("configured GNSS protocol\r\n"); #if 0 ptr = buf; ptr += ubx_put_u16 (ptr, 0x14); 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); #if 0 ubx_fetch (0x06, 0x31, NULL, 0, &len); printf ("configured GNSS 6.31\r\n"); #endif ubx_set_nav_rate (1000); #if 1 ubx_set_message_rate_port1 (0x01, 0x02, 10); 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, 0x30, 10); /*SV in view*/ ubx_set_message_rate_port1 (0x01, 0x32, 10); /*SBAS*/ // 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); ubx_set_message_rate_port1 (0x0D, 0x04, 10); ubx_cfg_sbas (1, 0x7, 3); ubx_cfg_poll (0x6, 0x16); // 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); 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(); // SET (NRESET); MAP_INPUT (PPS); MAP_OUTPUT_PP (NRESET); exti_select_source (EXTI7, PPS_PORT); exti_set_trigger (EXTI7, EXTI_TRIGGER_BOTH); exti_enable_request (EXTI7); 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); } void gps_dump_eph (void) { ubx_send (0xb, 0x31, NULL, 0); } #if 1 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 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); } #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