summaryrefslogtreecommitdiffstats
path: root/app/gps.c
diff options
context:
space:
mode:
Diffstat (limited to 'app/gps.c')
-rw-r--r--app/gps.c839
1 files changed, 839 insertions, 0 deletions
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;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-02 13:56:32 +0000