summaryrefslogtreecommitdiffstats
path: root/app/gps_neo8.c
diff options
context:
space:
mode:
Diffstat (limited to 'app/gps_neo8.c')
-rw-r--r--app/gps_neo8.c1204
1 files changed, 1204 insertions, 0 deletions
diff --git a/app/gps_neo8.c b/app/gps_neo8.c
new file mode 100644
index 0000000..ea5d984
--- /dev/null
+++ b/app/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 = SCS_DWT_CYCCNT;
+ 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 = SCS_DWT_CYCCNT;
+ 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 = SCS_DWT_CYCCNT;
+ 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 = SCS_DWT_CYCCNT;
+ 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
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-10 19:31:09 +0000