Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 1 | /* |
| 2 | * gps.c |
| 3 | * |
| 4 | * Copyright (C) 2019-2022 Sylvain Munaut <tnt@246tNt.com> |
| 5 | * SPDX-License-Identifier: GPL-3.0-or-later |
| 6 | */ |
| 7 | |
| 8 | #include <stddef.h> |
| 9 | #include <stdbool.h> |
| 10 | #include <stdint.h> |
| 11 | #include <string.h> |
| 12 | |
| 13 | #include "console.h" |
| 14 | #include "gps.h" |
| 15 | #include "misc.h" |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 16 | #include "rs422.h" |
Sylvain Munaut | 70c10f0 | 2022-01-12 11:58:34 +0100 | [diff] [blame] | 17 | #include "usb_gps.h" |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 18 | #include "utils.h" |
| 19 | |
| 20 | #include "config.h" |
| 21 | |
| 22 | |
| 23 | struct gps_uart { |
| 24 | uint32_t data; |
| 25 | uint32_t csr; |
| 26 | } __attribute__((packed,aligned(4))); |
| 27 | |
| 28 | #define GPS_UART_CSR_DIV(baud) ((SYS_CLK_FREQ+(baud)/2)/(baud)-2) |
| 29 | #define GPS_UART_CSR_TX_EMPTY (1 << 29) |
| 30 | #define GPS_UART_DATA_EMPTY (1 << 31) |
| 31 | |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 32 | static volatile struct gps_uart * const gps_uart_regs = (void*)(AUX_UART_BASE); |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 33 | |
| 34 | |
| 35 | static struct { |
| 36 | /* NMEA rx state */ |
| 37 | struct { |
| 38 | enum { |
| 39 | GNS_WAIT = 0, |
| 40 | GNS_READ = 1, |
| 41 | GNS_CK_HI = 2, |
| 42 | GNS_CK_LO = 3, |
| 43 | GNS_END_CR = 4, |
| 44 | GNS_END_LF = 5, |
| 45 | } state; |
| 46 | |
| 47 | int len; |
| 48 | uint8_t cksum; |
| 49 | char buf[95]; /* Max length is actually 82 + 1 (\0) */ |
| 50 | } nmea; |
| 51 | |
| 52 | /* Current lock state */ |
| 53 | struct { |
| 54 | bool valid; |
| 55 | } fix; |
| 56 | |
| 57 | /* Reported antenna state */ |
| 58 | enum gps_antenna_state antenna; |
| 59 | } g_gps; |
| 60 | |
| 61 | |
| 62 | static void |
| 63 | _gps_empty(bool wait_eol) |
| 64 | { |
| 65 | bool eol = false; |
| 66 | |
| 67 | uint32_t c; |
| 68 | while (1) { |
| 69 | c = gps_uart_regs->data; |
| 70 | if (c & GPS_UART_DATA_EMPTY) { |
| 71 | if (!wait_eol || eol) |
| 72 | break; |
| 73 | } else { |
| 74 | eol = (c == '\n'); |
| 75 | } |
| 76 | } |
| 77 | } |
| 78 | |
| 79 | static void |
| 80 | _gps_send(const char *s) |
| 81 | { |
| 82 | char cksum = 0; |
| 83 | |
| 84 | /* Start sentence */ |
| 85 | gps_uart_regs->data = '$'; |
| 86 | |
| 87 | /* Send payload */ |
| 88 | while (*s) |
| 89 | cksum ^= (gps_uart_regs->data = *s++); |
| 90 | |
| 91 | /* Send checksum */ |
| 92 | gps_uart_regs->data = '*'; |
| 93 | |
| 94 | s = hexstr(&cksum, 1, false); |
| 95 | gps_uart_regs->data = *s++; |
| 96 | gps_uart_regs->data = *s++; |
| 97 | |
| 98 | gps_uart_regs->data = '\r'; |
| 99 | gps_uart_regs->data = '\n'; |
| 100 | } |
| 101 | |
| 102 | static const char * |
| 103 | _gps_query(void) |
| 104 | { |
| 105 | uint32_t c; |
| 106 | while (1) { |
| 107 | /* Get next char */ |
| 108 | c = gps_uart_regs->data; |
| 109 | if (c & GPS_UART_DATA_EMPTY) |
| 110 | break; |
| 111 | |
| 112 | /* Always store it */ |
| 113 | if (g_gps.nmea.len == sizeof(g_gps.nmea.buf)) |
| 114 | g_gps.nmea.state = GNS_WAIT; |
| 115 | else |
| 116 | g_gps.nmea.buf[g_gps.nmea.len++] = c; |
| 117 | |
| 118 | /* State */ |
| 119 | if (c == '$') { |
| 120 | /* '$' always triggers reset */ |
| 121 | g_gps.nmea.state = GNS_READ; |
| 122 | g_gps.nmea.cksum = 0; |
| 123 | g_gps.nmea.len = 1; |
| 124 | g_gps.nmea.buf[0] = '$'; |
| 125 | } else { |
| 126 | switch (g_gps.nmea.state) { |
| 127 | case GNS_READ: |
| 128 | if (c == '*') |
| 129 | g_gps.nmea.state = GNS_CK_HI; |
| 130 | else |
| 131 | g_gps.nmea.cksum ^= c; |
| 132 | break; |
| 133 | case GNS_CK_HI: |
| 134 | g_gps.nmea.cksum ^= hexval(c) << 4; |
| 135 | g_gps.nmea.state = GNS_CK_LO; |
| 136 | break; |
| 137 | case GNS_CK_LO: |
| 138 | g_gps.nmea.cksum ^= hexval(c); |
| 139 | g_gps.nmea.state = GNS_END_CR; |
| 140 | break; |
| 141 | case GNS_END_CR: |
| 142 | g_gps.nmea.state = (c == '\r') ? GNS_END_LF : GNS_WAIT; |
| 143 | break; |
| 144 | case GNS_END_LF: |
| 145 | g_gps.nmea.state = GNS_WAIT; |
| 146 | g_gps.nmea.buf[g_gps.nmea.len] = 0x00; |
| 147 | if (c == '\n') |
| 148 | return g_gps.nmea.buf; |
| 149 | break; |
| 150 | default: |
| 151 | g_gps.nmea.state = GNS_WAIT; |
| 152 | g_gps.nmea.len = 0; |
| 153 | break; |
| 154 | } |
| 155 | } |
| 156 | } |
| 157 | return NULL; |
| 158 | } |
| 159 | |
| 160 | void |
| 161 | _gps_parse_nmea(const char *nmea) |
| 162 | { |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 163 | /* Very basic parsing, we just look at $PERC,GPsts message for |
Harald Welte | e3ff908 | 2022-09-05 20:26:48 +0200 | [diff] [blame] | 164 | * state 1 (survey mode) and 2 (position-hold) */ |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 165 | if (!strncmp(nmea, "$PERC,GPsts,", 12)) |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 166 | { |
Harald Welte | e3ff908 | 2022-09-05 20:26:48 +0200 | [diff] [blame] | 167 | g_gps.fix.valid = (nmea[12] == '1') || (nmea[12] == '2'); |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 168 | } |
| 169 | } |
| 170 | |
| 171 | |
| 172 | bool |
| 173 | gps_has_valid_fix(void) |
| 174 | { |
| 175 | return g_gps.fix.valid; |
| 176 | } |
| 177 | |
| 178 | enum gps_antenna_state |
| 179 | gps_antenna_status(void) |
| 180 | { |
| 181 | return g_gps.antenna; |
| 182 | } |
| 183 | |
| 184 | void |
| 185 | gps_poll(void) |
| 186 | { |
| 187 | const char *nmea; |
| 188 | |
| 189 | /* Check if we have anything from the module */ |
| 190 | nmea = _gps_query(); |
| 191 | if (!nmea) |
| 192 | return; |
| 193 | |
| 194 | /* If we do, process it locally to update our state */ |
| 195 | _gps_parse_nmea(nmea); |
Sylvain Munaut | 70c10f0 | 2022-01-12 11:58:34 +0100 | [diff] [blame] | 196 | |
| 197 | /* And queue it for USB */ |
| 198 | usb_gps_puts(nmea); |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 199 | } |
| 200 | |
| 201 | void |
| 202 | gps_init(void) |
| 203 | { |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 204 | uint32_t start_time = time_now_read(); |
| 205 | bool init_ok; |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 206 | |
| 207 | /* State init */ |
| 208 | memset(&g_gps, 0x00, sizeof(g_gps)); |
| 209 | |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 210 | /* Hold onboard GPS in reset */ |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 211 | gpio_out(3, false); |
| 212 | gpio_dir(3, true); |
| 213 | |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 214 | /* Init RS422 board */ |
| 215 | rs422_init(); |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 216 | |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 217 | /* Configure uart and empty buffer */ |
| 218 | gps_uart_regs->csr = GPS_UART_CSR_DIV(9600); |
| 219 | _gps_empty(false); |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 220 | |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 221 | /* Wait for first line of output as sign it's ready, timeout after 10 s */ |
| 222 | while (!time_elapsed(start_time, 10 * SYS_CLK_FREQ)) |
| 223 | if ((init_ok = (_gps_query() != NULL))) |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 224 | break; |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 225 | |
Sylvain Munaut | dae5a01 | 2022-09-01 18:16:19 +0200 | [diff] [blame] | 226 | if (init_ok) { |
| 227 | printf("[+] GPS ok\n"); |
| 228 | } else { |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 229 | printf("[!] GPS init failed\n"); |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 230 | } |
Sylvain Munaut | ef5fe38 | 2022-01-12 11:55:44 +0100 | [diff] [blame] | 231 | } |