diff --git a/firmware/ice40-riscv/icE1usb/Makefile b/firmware/ice40-riscv/icE1usb/Makefile index 36fd921..6725f6a 100644 --- a/firmware/ice40-riscv/icE1usb/Makefile +++ b/firmware/ice40-riscv/icE1usb/Makefile @@ -48,6 +48,7 @@ SOURCES_common += $(SOURCES_no2usb) HEADERS_app=\ config.h \ e1.h \ + gps.h \ ice1usb_proto.h \ misc.h \ usb_desc_ids.h \ @@ -59,6 +60,7 @@ HEADERS_app=\ SOURCES_app=\ e1.c \ fw_app.c \ + gps.c \ misc.c \ usb_desc_app.c \ usb_dev.c \ diff --git a/firmware/ice40-riscv/icE1usb/fw_app.c b/firmware/ice40-riscv/icE1usb/fw_app.c index 880d2d6..c5c2b7c 100644 --- a/firmware/ice40-riscv/icE1usb/fw_app.c +++ b/firmware/ice40-riscv/icE1usb/fw_app.c @@ -14,6 +14,7 @@ #include "console.h" #include "e1.h" +#include "gps.h" #include "led.h" #include "misc.h" #include "mini-printf.h" @@ -96,6 +97,9 @@ void main() pdm_set(PDM_CLK_HI, true, 2048, false); pdm_set(PDM_CLK_LO, false, 0, false); + /* GPS init */ + gps_init(); + /* Enable USB directly */ usb_init(&app_stack_desc); usb_dev_init(); @@ -161,5 +165,8 @@ void main() /* E1 poll */ usb_e1_poll(); + + /* GPS poll */ + gps_poll(); } } diff --git a/firmware/ice40-riscv/icE1usb/gps.c b/firmware/ice40-riscv/icE1usb/gps.c new file mode 100644 index 0000000..2fd6ab8 --- /dev/null +++ b/firmware/ice40-riscv/icE1usb/gps.c @@ -0,0 +1,286 @@ +/* + * gps.c + * + * Copyright (C) 2019-2022 Sylvain Munaut + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#include +#include +#include +#include + +#include "console.h" +#include "gps.h" +#include "misc.h" +#include "utils.h" + +#include "config.h" + + +struct gps_uart { + uint32_t data; + uint32_t csr; +} __attribute__((packed,aligned(4))); + +#define GPS_UART_CSR_DIV(baud) ((SYS_CLK_FREQ+(baud)/2)/(baud)-2) +#define GPS_UART_CSR_TX_EMPTY (1 << 29) +#define GPS_UART_DATA_EMPTY (1 << 31) + +static volatile struct gps_uart * const gps_uart_regs = (void*)(GPS_UART_BASE); + + +static struct { + /* NMEA rx state */ + struct { + enum { + GNS_WAIT = 0, + GNS_READ = 1, + GNS_CK_HI = 2, + GNS_CK_LO = 3, + GNS_END_CR = 4, + GNS_END_LF = 5, + } state; + + int len; + uint8_t cksum; + char buf[95]; /* Max length is actually 82 + 1 (\0) */ + } nmea; + + /* Current lock state */ + struct { + bool valid; + } fix; + + /* Reported antenna state */ + enum gps_antenna_state antenna; +} g_gps; + + +static void +_gps_empty(bool wait_eol) +{ + bool eol = false; + + uint32_t c; + while (1) { + c = gps_uart_regs->data; + if (c & GPS_UART_DATA_EMPTY) { + if (!wait_eol || eol) + break; + } else { + eol = (c == '\n'); + } + } +} + +static void +_gps_send(const char *s) +{ + char cksum = 0; + + /* Start sentence */ + gps_uart_regs->data = '$'; + + /* Send payload */ + while (*s) + cksum ^= (gps_uart_regs->data = *s++); + + /* Send checksum */ + gps_uart_regs->data = '*'; + + s = hexstr(&cksum, 1, false); + gps_uart_regs->data = *s++; + gps_uart_regs->data = *s++; + + gps_uart_regs->data = '\r'; + gps_uart_regs->data = '\n'; +} + +static const char * +_gps_query(void) +{ + uint32_t c; + while (1) { + /* Get next char */ + c = gps_uart_regs->data; + if (c & GPS_UART_DATA_EMPTY) + break; + + /* Always store it */ + if (g_gps.nmea.len == sizeof(g_gps.nmea.buf)) + g_gps.nmea.state = GNS_WAIT; + else + g_gps.nmea.buf[g_gps.nmea.len++] = c; + + /* State */ + if (c == '$') { + /* '$' always triggers reset */ + g_gps.nmea.state = GNS_READ; + g_gps.nmea.cksum = 0; + g_gps.nmea.len = 1; + g_gps.nmea.buf[0] = '$'; + } else { + switch (g_gps.nmea.state) { + case GNS_READ: + if (c == '*') + g_gps.nmea.state = GNS_CK_HI; + else + g_gps.nmea.cksum ^= c; + break; + case GNS_CK_HI: + g_gps.nmea.cksum ^= hexval(c) << 4; + g_gps.nmea.state = GNS_CK_LO; + break; + case GNS_CK_LO: + g_gps.nmea.cksum ^= hexval(c); + g_gps.nmea.state = GNS_END_CR; + break; + case GNS_END_CR: + g_gps.nmea.state = (c == '\r') ? GNS_END_LF : GNS_WAIT; + break; + case GNS_END_LF: + g_gps.nmea.state = GNS_WAIT; + g_gps.nmea.buf[g_gps.nmea.len] = 0x00; + if (c == '\n') + return g_gps.nmea.buf; + break; + default: + g_gps.nmea.state = GNS_WAIT; + g_gps.nmea.len = 0; + break; + } + } + } + return NULL; +} + +void +_gps_parse_nmea(const char *nmea) +{ + /* Very basic parsing, we just look at GSA messages and consider + * that if we have a 3D fix with PDOP < 5, the timing data should + * be usable + */ + if (!strncmp(nmea, "$GPGSA", 6)) + { + /* Check for Autonomous 3D fix (fixed field positions) */ + if ((nmea[7] != 'A') || (nmea[9] != '3')) { + g_gps.fix.valid = false; + return; + } + + /* Find PDOP */ + const char *p = nmea; + for (int i=0; (i < 15) && (*p != '*'); i += (*(p++)==',') ); + + /* Is it low enough ? */ + g_gps.fix.valid = (p[1] < '5'); + } + + /* Parse TXT ANTENNA Status */ + if (!strncmp(nmea, "$GPTXT", 6) && nmea[13] == '0' && nmea[14] == '1') + { + if (!strncmp(&nmea[16], "ANTENNA OK", 10)) + g_gps.antenna = ANT_OK; + else if (!strncmp(&nmea[16], "ANTENNA OPEN", 12)) + g_gps.antenna = ANT_OPEN; + else if (!strncmp(&nmea[16], "ANTENNA SHORT", 13)) + g_gps.antenna = ANT_SHORT; + else + g_gps.antenna = ANT_UNKNOWN; + } +} + + +bool +gps_has_valid_fix(void) +{ + return g_gps.fix.valid; +} + +enum gps_antenna_state +gps_antenna_status(void) +{ + return g_gps.antenna; +} + +void +gps_poll(void) +{ + const char *nmea; + + /* Check if we have anything from the module */ + nmea = _gps_query(); + if (!nmea) + return; + + /* If we do, process it locally to update our state */ + _gps_parse_nmea(nmea); +} + +void +gps_init(void) +{ + int i; + + /* State init */ + memset(&g_gps, 0x00, sizeof(g_gps)); + + /* Configure reset gpio */ + gpio_out(3, false); + gpio_dir(3, true); + + /* Attempt reset sequence at 9600 baud and then 115200 baud */ + for (i=0; i<2; i++) + { + uint32_t start_time = time_now_read(); + bool init_ok; + + /* Assert reset */ + gpio_out(3, false); + + /* Configure uart and empty buffer */ + gps_uart_regs->csr = i ? GPS_UART_CSR_DIV(115200) : GPS_UART_CSR_DIV(9600); + _gps_empty(false); + + /* Wait 100 ms */ + delay(100); + + /* Release reset line */ + gpio_out(3, true); + + /* Wait for first line of output as sign it's ready, timeout after 1s */ + while (!time_elapsed(start_time, SYS_CLK_FREQ)) + if ((init_ok = (_gps_query() != NULL))) + break; + + if (init_ok) { + printf("[+] GPS ok at %d baud\n", i ? 115200 : 9600); + break; + } + } + + /* Failed ? */ + if (i == 2) { + printf("[!] GPS init failed\n"); + return; + } + + /* If success was at 9600 baud, need to speed up */ + if (i == 0) { + /* Configure GPS to use serial at 115200 baud */ + _gps_send("PCAS01,5"); + + /* Add dummy byte which will be mangled during baudrate switch ... */ + gps_uart_regs->data = 0x00; + while (!(gps_uart_regs->csr & GPS_UART_CSR_TX_EMPTY)); + + /* Set uart to 115200 and empty uart buffer, line aligned */ + gps_uart_regs->csr = GPS_UART_CSR_DIV(115200) ; + _gps_empty(true); + } + + /* Configure GPS to be GPS-only (no GLONASS/BEIDOU) */ + _gps_send("PCAS04,1"); +} diff --git a/firmware/ice40-riscv/icE1usb/gps.h b/firmware/ice40-riscv/icE1usb/gps.h new file mode 100644 index 0000000..57ef271 --- /dev/null +++ b/firmware/ice40-riscv/icE1usb/gps.h @@ -0,0 +1,23 @@ +/* + * gps.h + * + * Copyright (C) 2019-2022 Sylvain Munaut + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#pragma once + +#include + +enum gps_antenna_state { + ANT_UNKNOWN = 0, + ANT_OK, + ANT_OPEN, + ANT_SHORT +}; + +bool gps_has_valid_fix(void); +enum gps_antenna_state gps_antenna_status(void); + +void gps_poll(void); +void gps_init(void);