From b3c6320899d6b27899ab3c67c745e8d3b29af3a2 Mon Sep 17 00:00:00 2001 From: root Date: Tue, 19 Feb 2019 13:46:18 +0000 Subject: working ethernet --- app/gps.c | 839 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 839 insertions(+) create mode 100644 app/gps.c (limited to 'app/gps.c') diff --git a/app/gps.c b/app/gps.c new file mode 100644 index 0000000..9fc60de --- /dev/null +++ b/app/gps.c @@ -0,0 +1,839 @@ +#include "project.h" + +#define PPS (GPIO2) +#define PPS_PORT GPIOD + +#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; + +int gps_locked; +int gps_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; + + +void exti2_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); + + 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); + +} + + + +static inline int +ubx_recv_byte (uint8_t *v) +{ + return !ring_read_byte (&rx3_ring, v); +} + +static inline void +ubx_send_byte (uint8_t v) +{ + usart3_queue (v); +} + +static int +ubx_recv_nav_status (uint8_t *ptr, unsigned len) +{ + uint8_t gps_fix, flags; + 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 + + + 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; + 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); + +// printf ("gps %02d:%02d:%02d %09d\r\n", (int) hour, (int) min, (int) sec, (int) nano); + +#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_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; + + 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 5: + ubx_ack++; + printf (" %s for %02x.%02x\r\n", id ? "ACK" : "NAK", payload[0], + payload[1]); + + break; + + case 0x0b: + switch (id) { + 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]; + uint64_t abs; + uint32_t now; + 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; + + gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK; + + + if (!v) return; + + + + abs = abs_extend (now); + pll_dispatch(abs); + + e = pll_decompose (abs); + + u=time_epoch_to_utc(e); + + printf ("GPS %02d:%02d:%02d.%09d Fix:%c%c TXCO %+8dE-12\r\n", + u.hour, + u.minute, + u.second, + u.nanosecond, + fix,fix2,(int) freq); + + +} + + +void +gps_dispatch (void) +{ + ubx_dispatch_search (-1, -1, NULL); + gps_pps_dispatch(); +} + +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); + +} + +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"); + usart3_drain(); + ubx_send (class, id, payload, len); + timeout = ticks + TIMEOUT; + } + + + + + gps_dispatch(); + } + + return 0; +} + +int +ubx_handshake_xfer (uint8_t class, uint8_t id, const void *payload, + unsigned len) +{ + uint32_t timeout; + + ubx_ack_xfer = 0; +// usart3_drain(); + ubx_send (class, id, payload, len); + + + + timeout = ticks + TIMEOUT; + + while (!ubx_ack_xfer) { + + if (ticks > timeout) { + printf ("GPS timeout resending packet\r\n"); +// usart3_drain(); + ubx_send (class, id, payload, len); + timeout = ticks + TIMEOUT; + } + + + + + gps_dispatch(); + } + + return 0; +} + + + + +uint8_t * +ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len, + unsigned *len_ptr) +{ + uint8_t *ret; + + while (!ring_empty (&tx3_ring) || !ring_empty (&rx3_ring)) + gps_dispatch(); + + ubx_send (class, id, payload, len); + + while (! (ret = ubx_dispatch_search (class, id, len_ptr))); + + + return ret; +} + +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)); +} + + +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; +} + + +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); //reserved + 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, 500000 / ref_hz); //pulse width unlocked/us + ptr += ubx_put_u32 (ptr, 500000 / 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, 0xf3); +#endif + + + 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; + + usart3_drain(); + + while (!ring_empty (&tx3_ring) || !ring_empty (&rx3_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, 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 1 +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"); + + ubx_fetch (0x06, 0x31, NULL, 0, &len); + + printf ("configured GNSS 6.31\r\n"); + +#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 (0x03, 0x0a, 0); + //ubx_set_message_rate_port1 (0x03, 0x10, 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 + +#if 1 +ptr = buf; + ptr += ubx_put_u8 (ptr, 0x1); + ptr += ubx_put_u8 (ptr, 0x7); + ptr += ubx_put_u8 (ptr, 0x1); + ptr += ubx_put_u8 (ptr, 0xf); + ptr += ubx_put_u32 (ptr, 0xffffffff); + ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf)); + + printf ("configured SBAS\r\n"); + + ubx_handshake (0x06, 0x16, buf, 0); +#endif + + + // printf ("GPS ready\r\n"); + // ubx_get_clock_stats(); + + 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); + + return 0; +} + + +#define ALMANAC_LUMP 128 + +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_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); +usart3_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); + +} + +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; +} -- cgit v1.2.3