| /* |
| * gps.c |
| * |
| * Copyright (C) 2019-2022 Sylvain Munaut <tnt@246tNt.com> |
| * SPDX-License-Identifier: GPL-3.0-or-later |
| */ |
| |
| #include <stddef.h> |
| #include <stdbool.h> |
| #include <stdint.h> |
| #include <string.h> |
| |
| #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"); |
| } |