nmea.c 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363
  1. #include <avr/wdt.h>
  2. #include <stddef.h>
  3. #include <string.h>
  4. #include <stdlib.h>
  5. #include <ctype.h>
  6. #include <avr/sleep.h>
  7. #include "nmea.h"
  8. #include "main.h"
  9. #include "uart0.h"
  10. #include "uart1.h"
  11. #include "xprintf.h"
  12. /*----------------------------------------------------*/
  13. /* Get a line received from GPS module */
  14. /*----------------------------------------------------*/
  15. uint16_t get_line ( /* 0:line incomplete or timed out, >0: Number of bytes received. */
  16. char *buff,
  17. uint16_t sz_buf
  18. )
  19. {
  20. char c;
  21. static uint16_t i = 0;
  22. uint16_t ret_len;
  23. set_timer(recv_timeout, 1000);
  24. for (;;) {
  25. wdt_reset();
  26. if (FLAGS & (F_LVD | F_POWEROFF))
  27. return 0; /* A brownout is detected */
  28. if (timer_expired(recv_timeout))
  29. return 0; /* timeout; continue the main loop */
  30. if (System.keypress) /* process user keypress */
  31. return 0;
  32. if (!uart0_test()) {
  33. sleep();
  34. continue;
  35. }
  36. c = (char)uart0_get();
  37. if (i == 0 && c != '$' )
  38. continue; /* Find start of line */
  39. if (c == '\n' || c == '\r') {
  40. buff[i++] = '\r';
  41. buff[i++] = '\n';
  42. buff[i++] = '\0'; /* add null termination for string */
  43. break; /* EOL */
  44. }
  45. buff[i++] = c;
  46. uart1_put(c);
  47. if (i >= sz_buf - 3) /* keep 3 bytes for terminating character */
  48. i = 0; /* Buffer overflow (abort this line) */
  49. }
  50. ret_len = i;
  51. i = 0;
  52. if (ret_len > 0) {
  53. uart1_put('\r');
  54. uart1_put('\n');
  55. }
  56. return ret_len;
  57. }
  58. /* Compare sentence header string */
  59. BYTE gp_comp (const char *str1, __flash const char *str2)
  60. {
  61. char c;
  62. do {
  63. c = pgm_read_byte(str2++);
  64. } while (c && c == *str1++);
  65. return c;
  66. }
  67. #define FIELD_BUF_LEN 32
  68. /* Get a column item */
  69. static
  70. const char* gp_col ( /* Returns pointer to the item (returns a NULL when not found) */
  71. const char* buf, /* Pointer to the sentence */
  72. BYTE col /* Column number (0 is the 1st item) */
  73. ) {
  74. BYTE c;
  75. static char field_buf[FIELD_BUF_LEN];
  76. unsigned char length = 0;
  77. while (col) {
  78. do {
  79. c = *buf++;
  80. if (c <= ' ') return NULL;
  81. } while (c != ',');
  82. col--;
  83. }
  84. while (*buf && *buf != ',' && length < FIELD_BUF_LEN-1) {
  85. field_buf[length++] = *buf++;
  86. }
  87. field_buf[length] = '\0';
  88. return field_buf;
  89. }
  90. unsigned int gp_val(const char *db, unsigned char count) {
  91. unsigned int out = 0;
  92. while (count--) {
  93. unsigned char n;
  94. n = *db - '0';
  95. if (n >= 10)
  96. return 0;
  97. out *= 10;
  98. out += n;
  99. db++;
  100. }
  101. return out;
  102. }
  103. static time_t gp_rmc_parse(const char *str) {
  104. const char *p;
  105. struct tm tmc;
  106. p = gp_col(str, 1); /* Get h:m:s */
  107. if (!p)
  108. return 0;
  109. tmc.tm_hour = gp_val(p, 2);
  110. tmc.tm_min = gp_val(p+2, 2);
  111. tmc.tm_sec = gp_val(p+4, 2);
  112. p = gp_col(str, 9); /* Get y:m:d */
  113. if (!p)
  114. return 0;
  115. tmc.tm_mday = gp_val(p, 2);
  116. tmc.tm_mon = gp_val(p+2, 2) - 1;
  117. tmc.tm_year = gp_val(p+4, 2) + 100;
  118. utc = mktime(&tmc); /* Check time validity */
  119. if (utc == -1)
  120. return 0;
  121. p = gp_col(str, 2); /* Get status */
  122. if (!p || *p != 'A') {
  123. System.location_valid = LOC_INVALID;
  124. FLAGS &= ~F_GPSOK;
  125. return 0; /* Return 0 even is time is valid (comes from module's internal RTC) */
  126. }
  127. FLAGS |= F_GPSOK;
  128. return utc;
  129. }
  130. static void gp_gga_parse(const char *str) {
  131. const char *p;
  132. double tmp;
  133. p = gp_col(str, 7); /* satellites used */
  134. System.satellites_used = atoi(p);
  135. if (System.satellites_used >= System.conf.min_sats) {
  136. System.sat_count_low = 0;
  137. } else {
  138. System.sat_count_low = 1;
  139. }
  140. /* check validity */
  141. p = gp_col(str, 6);
  142. if (*p == '0') {
  143. System.location_valid = LOC_INVALID;
  144. return;
  145. }
  146. if (!System.sat_count_low)
  147. System.location_valid = LOC_VALID_NEW; /* don't accept the coordinates otherwise, even if reported valid */
  148. /* parse location */
  149. p = gp_col(str, 2); /* latitude */
  150. location.lat = gp_val(p, 2); /* degrees */
  151. p += 2;
  152. xatof(&p, &tmp); /* minutes */
  153. tmp /= 60; /* convert minutes to degrees */
  154. location.lat += tmp;
  155. p = gp_col(str, 3); /* N/S */
  156. if (*p != 'N')
  157. location.lat = -location.lat;
  158. p = gp_col(str, 4); /* longitude */
  159. location.lon = gp_val(p, 3); /* degrees */
  160. p += 3;
  161. xatof(&p, &tmp); /* minutes */
  162. tmp /= 60; /* convert minutes to degrees */
  163. location.lon += tmp;
  164. p = gp_col(str, 5); /* E/W */
  165. if (*p != 'E')
  166. location.lon = -location.lon;
  167. p = gp_col(str, 6); /* fix type */
  168. if (*p == '2')
  169. System.sbas = 1;
  170. else
  171. System.sbas = 0;
  172. p = gp_col(str, 9); /* MSL altitude */
  173. xatof(&p, &tmp);
  174. location.alt = tmp;
  175. location.time = utc; /* parsed from RMC */
  176. }
  177. static void gp_vtg_parse(const char *str) {
  178. const char *p;
  179. double speed;
  180. p = gp_col(str, 9);
  181. if (*p == 'N') /* Not valid */
  182. return;
  183. p = gp_col(str, 7); /* speed in km/h */
  184. xatof(&p, &speed);
  185. System.speed = speed+0.5;
  186. }
  187. /*$PMTK355*31<CR><LF>
  188. Return $PMTK001,355,3,1,0,0*2E “$PMTK001,355,3,GLON_Enable,BEIDOU_Enable,GALILEO_Enable”
  189. The GLONASS search mode is enabled. */
  190. static void pmtk001_parse(const char *str) {
  191. const char *p;
  192. /* check validity */
  193. p = gp_col(str, 1);
  194. if (strcmp_P(p, PSTR("355"))) /* not the PMTK355 reply */
  195. return;
  196. p = gp_col(str, 2);
  197. if (*p == '0' || *p == '1') { /* invalid / unsupported */
  198. System.gps_only = 1;
  199. xputs_P(PSTR("GPS only\r\n"));
  200. } else {
  201. System.gps_only = 0;
  202. xputs_P(PSTR("Multi GNSS\r\n"));
  203. }
  204. gps_initialize();
  205. }
  206. unsigned char nmea_checksum(const char *str) {
  207. unsigned char cs = 0;
  208. while (*str && *str != '*') {
  209. cs ^= *str++; /* NMEA checksum is quite primitive, but still should catch simple bitstream errors or truncated lines */
  210. }
  211. return cs;
  212. }
  213. time_t gps_parse(const char *str) { /* Get all required data from NMEA sentences */
  214. signed int len = strlen(str)-2; /* remove final \r\n */
  215. const char *checksum;
  216. unsigned char calc_checksum, inc_checksum;
  217. signed int i;
  218. char c;
  219. if (len < 4)
  220. return 0; /* each message must contain $.*xx where . is one or more actual data characters and xx is the checksum */
  221. for (i = len-1; i && i >= (len-2); i--) { /* find checksum */
  222. if (str[i] == '*')
  223. break;
  224. }
  225. checksum = str+i+1; /* skip * character */
  226. inc_checksum = 0; /* parse hex string */
  227. while (*checksum && isalnum(*checksum)) {
  228. inc_checksum *= 16;
  229. c = *checksum++;
  230. if (c >= '0' && c <= '9') {
  231. inc_checksum += c - '0';
  232. } else if (c >= 'a' && c <= 'f') {
  233. inc_checksum += c - 'a' + 10;
  234. } else if (c >= 'A' && c <= 'F') {
  235. inc_checksum += c - 'A' + 10;
  236. } else {
  237. xputs_P(PSTR("Invalid NMEA: malformed checksum\r\n"));
  238. return 0; /* invalid checksum character */
  239. }
  240. }
  241. str++; /* drop initial $ */
  242. calc_checksum = nmea_checksum(str);
  243. if (inc_checksum != calc_checksum) {
  244. xputs_P(PSTR("Invalid NMEA checksum received\r\n"));
  245. return 0;
  246. }
  247. if (!gp_comp(str, PSTR("GPRMC")) || !gp_comp(str, PSTR("GNRMC")) || !gp_comp(str, PSTR("BDRMC")) || !gp_comp(str, PSTR("GARMC"))) {
  248. return gp_rmc_parse(str);
  249. }
  250. if (!gp_comp(str, PSTR("GPGGA")) || !gp_comp(str, PSTR("GNGGA")) || !gp_comp(str, PSTR("BDGGA")) || !gp_comp(str, PSTR("GAGGA"))) {
  251. gp_gga_parse(str);
  252. return 0;
  253. }
  254. if (!gp_comp(str, PSTR("GPVTG")) || !gp_comp(str, PSTR("GNVTG"))) {
  255. gp_vtg_parse(str);
  256. return 0;
  257. }
  258. if (!System.gps_initialized && !gp_comp(str, PSTR("PMTK011"))) {
  259. gps_initialize();
  260. return 0;
  261. }
  262. if (!gp_comp(str, PSTR("PMTK001"))) {
  263. pmtk001_parse(str);
  264. return 0;
  265. }
  266. return 0;
  267. }
  268. void uart0_put_wrap(int c) {
  269. uart0_put((char)c);
  270. }
  271. void gps_initialize(void) {
  272. /*
  273. * PMTK355: query gnss search mode (will fail if only GPS is supported)
  274. * PMTK353: set gnss search mode (GPS/Galileo/Glonass/Beidou)
  275. * PMTK313: enable SBAS
  276. */
  277. if (!System.gps_initialized)
  278. xfprintf(uart0_put_wrap, PSTR("$PMTK355*31\r\n"));
  279. if (get_flag(CONFFLAG_ENABLE_SBAS))
  280. xfprintf(uart0_put_wrap, PSTR("$PMTK313,1*2E\r\n"));
  281. else
  282. xfprintf(uart0_put_wrap, PSTR("$PMTK313,0*2F\r\n"));
  283. if (!System.gps_only) {
  284. switch (System.conf.gnss_mode) {
  285. default:
  286. case GNSS_MODE_GPS_GLONASS_GALILEO:
  287. xfprintf(uart0_put_wrap, PSTR("$PMTK353,1,1,1,0,0*2A\r\n"));
  288. break;
  289. case GNSS_MODE_GPS:
  290. xfprintf(uart0_put_wrap, PSTR("$PMTK353,1,0,0,0,0*2A\r\n"));
  291. break;
  292. case GNSS_MODE_GPS_GALILEO:
  293. xfprintf(uart0_put_wrap, PSTR("$PMTK353,1,0,1,0,0*2B\r\n"));
  294. break;
  295. case GNSS_MODE_GALILEO:
  296. xfprintf(uart0_put_wrap, PSTR("$PMTK353,0,0,1,0,0*2A\r\n"));
  297. break;
  298. case GNSS_MODE_GPS_BEIDOU:
  299. xfprintf(uart0_put_wrap, PSTR("$PMTK353,1,0,0,0,1*2B\r\n"));
  300. break;
  301. case GNSS_MODE_BEIDOU:
  302. xfprintf(uart0_put_wrap, PSTR("$PMTK353,0,0,0,0,1*2A\r\n"));
  303. break;
  304. }
  305. } else {
  306. System.conf.gnss_mode = GNSS_MODE_GPS;
  307. }
  308. xputs_P(PSTR("GPS init sent\r\n"));
  309. System.gps_initialized = 1;
  310. }
  311. void check_min_sat_limit(void) {
  312. if (System.conf.min_sats > 6 && (System.conf.gnss_mode == GNSS_MODE_GPS || System.conf.gnss_mode == GNSS_MODE_GALILEO || System.conf.gnss_mode == GNSS_MODE_BEIDOU)) {
  313. /* by geometry, max visible number is 6..12 */
  314. System.conf.min_sats = 6;
  315. }
  316. }