Firmware SDK
 All Data Structures Functions Variables Typedefs Enumerations Enumerator Groups Pages
minmea.c
1 /*
2  * Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
3  * This program is free software. It comes without any warranty, to the extent
4  * permitted by applicable law. You can redistribute it and/or modify it under
5  * the terms of the Do What The Fuck You Want To Public License, Version 2, as
6  * published by Sam Hocevar. See the COPYING file for more details.
7  */
8 
9 #include "minmea.h"
10 
11 #include <stdlib.h>
12 #include <string.h>
13 #include <ctype.h>
14 #include <stdarg.h>
15 #include <time.h>
16 
17 #define boolstr(s) ((s) ? "true" : "false")
18 
19 static int hex2int(char c)
20 {
21  if (c >= '0' && c <= '9')
22  return c - '0';
23  if (c >= 'A' && c <= 'F')
24  return c - 'A' + 10;
25  if (c >= 'a' && c <= 'f')
26  return c - 'a' + 10;
27  return -1;
28 }
29 
30 uint8_t minmea_checksum(const char *sentence)
31 {
32  // Support senteces with or without the starting dollar sign.
33  if (*sentence == '$')
34  sentence++;
35 
36  uint8_t checksum = 0x00;
37 
38  // The optional checksum is an XOR of all bytes between "$" and "*".
39  while (*sentence && *sentence != '*')
40  checksum ^= *sentence++;
41 
42  return checksum;
43 }
44 
45 bool minmea_check(const char *sentence, bool strict)
46 {
47  uint8_t checksum = 0x00;
48 
49  // Sequence length is limited.
50  if (strlen(sentence) > MINMEA_MAX_LENGTH + 3)
51  return false;
52 
53  // A valid sentence starts with "$".
54  if (*sentence++ != '$')
55  return false;
56 
57  // The optional checksum is an XOR of all bytes between "$" and "*".
58  while (*sentence && *sentence != '*' && isprint((unsigned char) *sentence))
59  checksum ^= *sentence++;
60 
61  // If checksum is present...
62  if (*sentence == '*') {
63  // Extract checksum.
64  sentence++;
65  int upper = hex2int(*sentence++);
66  if (upper == -1)
67  return false;
68  int lower = hex2int(*sentence++);
69  if (lower == -1)
70  return false;
71  int expected = upper << 4 | lower;
72 
73  // Check for checksum mismatch.
74  if (checksum != expected)
75  return false;
76  } else if (strict) {
77  // Discard non-checksummed frames in strict mode.
78  return false;
79  }
80 
81  // The only stuff allowed at this point is a newline.
82  if (*sentence && strcmp(sentence, "\n") && strcmp(sentence, "\r\n"))
83  return false;
84 
85  return true;
86 }
87 
88 static inline bool minmea_isfield(char c) {
89  return isprint((unsigned char) c) && c != ',' && c != '*';
90 }
91 
92 bool minmea_scan(const char *sentence, const char *format, ...)
93 {
94  bool result = false;
95  bool optional = false;
96  va_list ap;
97  va_start(ap, format);
98 
99  const char *field = sentence;
100 #define next_field() \
101  do { \
102  /* Progress to the next field. */ \
103  while (minmea_isfield(*sentence)) \
104  sentence++; \
105  /* Make sure there is a field there. */ \
106  if (*sentence == ',') { \
107  sentence++; \
108  field = sentence; \
109  } else { \
110  field = NULL; \
111  } \
112  } while (0)
113 
114  while (*format) {
115  char type = *format++;
116 
117  if (type == ';') {
118  // All further fields are optional.
119  optional = true;
120  continue;
121  }
122 
123  if (!field && !optional) {
124  // Field requested but we ran out if input. Bail out.
125  goto parse_error;
126  }
127 
128  switch (type) {
129  case 'c': { // Single character field (char).
130  char value = '\0';
131 
132  if (field && minmea_isfield(*field))
133  value = *field;
134 
135  *va_arg(ap, char *) = value;
136  } break;
137 
138  case 'd': { // Single character direction field (int).
139  int value = 0;
140 
141  if (field && minmea_isfield(*field)) {
142  switch (*field) {
143  case 'N':
144  case 'E':
145  value = 1;
146  break;
147  case 'S':
148  case 'W':
149  value = -1;
150  break;
151  default:
152  goto parse_error;
153  }
154  }
155 
156  *va_arg(ap, int *) = value;
157  } break;
158 
159  case 'f': { // Fractional value with scale (struct minmea_float).
160  int sign = 0;
161  int_least32_t value = -1;
162  int_least32_t scale = 0;
163 
164  if (field) {
165  while (minmea_isfield(*field)) {
166  if (*field == '+' && !sign && value == -1) {
167  sign = 1;
168  } else if (*field == '-' && !sign && value == -1) {
169  sign = -1;
170  } else if (isdigit((unsigned char) *field)) {
171  int digit = *field - '0';
172  if (value == -1)
173  value = 0;
174  if (value > (INT_LEAST32_MAX-digit) / 10) {
175  /* we ran out of bits, what do we do? */
176  if (scale) {
177  /* truncate extra precision */
178  break;
179  } else {
180  /* integer overflow. bail out. */
181  goto parse_error;
182  }
183  }
184  value = (10 * value) + digit;
185  if (scale)
186  scale *= 10;
187  } else if (*field == '.' && scale == 0) {
188  scale = 1;
189  } else if (*field == ' ') {
190  /* Allow spaces at the start of the field. Not NMEA
191  * conformant, but some modules do this. */
192  if (sign != 0 || value != -1 || scale != 0)
193  goto parse_error;
194  } else {
195  goto parse_error;
196  }
197  field++;
198  }
199  }
200 
201  if ((sign || scale) && value == -1)
202  goto parse_error;
203 
204  if (value == -1) {
205  /* No digits were scanned. */
206  value = 0;
207  scale = 0;
208  } else if (scale == 0) {
209  /* No decimal point. */
210  scale = 1;
211  }
212  if (sign)
213  value *= sign;
214 
215  *va_arg(ap, struct minmea_float *) = (struct minmea_float) {value, scale};
216  } break;
217 
218  case 'i': { // Integer value, default 0 (int).
219  int value = 0;
220 
221  if (field) {
222  char *endptr;
223  value = strtol(field, &endptr, 10);
224  if (minmea_isfield(*endptr))
225  goto parse_error;
226  }
227 
228  *va_arg(ap, int *) = value;
229  } break;
230 
231  case 's': { // String value (char *).
232  char *buf = va_arg(ap, char *);
233 
234  if (field) {
235  while (minmea_isfield(*field))
236  *buf++ = *field++;
237  }
238 
239  *buf = '\0';
240  } break;
241 
242  case 't': { // NMEA talker+sentence identifier (char *).
243  // This field is always mandatory.
244  if (!field)
245  goto parse_error;
246 
247  if (field[0] != '$')
248  goto parse_error;
249  int type_length = 5;
250  if (field[1] == 'P')
251  type_length = 4;
252  for (int f=0; f<type_length; f++)
253  if (!minmea_isfield(field[1+f]))
254  goto parse_error;
255 
256  char *buf = va_arg(ap, char *);
257  memcpy(buf, field+1, type_length);
258  buf[type_length] = '\0';
259  } break;
260 
261  case 'D': { // Date (int, int, int), -1 if empty.
262  struct minmea_date *date = va_arg(ap, struct minmea_date *);
263 
264  int d = -1, m = -1, y = -1;
265 
266  if (field && minmea_isfield(*field)) {
267  // Always six digits.
268  for (int f=0; f<6; f++)
269  if (!isdigit((unsigned char) field[f]))
270  goto parse_error;
271 
272  char dArr[] = {field[0], field[1], '\0'};
273  char mArr[] = {field[2], field[3], '\0'};
274  char yArr[] = {field[4], field[5], '\0'};
275  d = strtol(dArr, NULL, 10);
276  m = strtol(mArr, NULL, 10);
277  y = strtol(yArr, NULL, 10);
278  }
279 
280  date->day = d;
281  date->month = m;
282  date->year = y;
283  } break;
284 
285  case 'T': { // Time (int, int, int, int), -1 if empty.
286  struct minmea_time *time_ = va_arg(ap, struct minmea_time *);
287 
288  int h = -1, i = -1, s = -1, u = -1;
289 
290  if (field && minmea_isfield(*field)) {
291  // Minimum required: integer time.
292  for (int f=0; f<6; f++)
293  if (!isdigit((unsigned char) field[f]))
294  goto parse_error;
295 
296  char hArr[] = {field[0], field[1], '\0'};
297  char iArr[] = {field[2], field[3], '\0'};
298  char sArr[] = {field[4], field[5], '\0'};
299  h = strtol(hArr, NULL, 10);
300  i = strtol(iArr, NULL, 10);
301  s = strtol(sArr, NULL, 10);
302  field += 6;
303 
304  // Extra: fractional time. Saved as microseconds.
305  if (*field++ == '.') {
306  uint32_t value = 0;
307  uint32_t scale = 1000000LU;
308  while (isdigit((unsigned char) *field) && scale > 1) {
309  value = (value * 10) + (*field++ - '0');
310  scale /= 10;
311  }
312  u = value * scale;
313  } else {
314  u = 0;
315  }
316  }
317 
318  time_->hours = h;
319  time_->minutes = i;
320  time_->seconds = s;
321  time_->microseconds = u;
322  } break;
323 
324  case '_': { // Ignore the field.
325  } break;
326 
327  default: { // Unknown.
328  goto parse_error;
329  }
330  }
331 
332  next_field();
333  }
334 
335  result = true;
336 
337 parse_error:
338  va_end(ap);
339  return result;
340 }
341 
342 bool minmea_talker_id(char talker[3], const char *sentence)
343 {
344  char type[6];
345  if (!minmea_scan(sentence, "t", type))
346  return false;
347 
348  talker[0] = type[0];
349  talker[1] = type[1];
350  talker[2] = '\0';
351 
352  return true;
353 }
354 
355 enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict)
356 {
357  if (!minmea_check(sentence, strict))
358  return MINMEA_INVALID;
359 
360  char type[6];
361  if (!minmea_scan(sentence, "t", type))
362  return MINMEA_INVALID;
363 
364  if (!strcmp(type+2, "RMC"))
365  return MINMEA_SENTENCE_RMC;
366  if (!strcmp(type+2, "GGA"))
367  return MINMEA_SENTENCE_GGA;
368  if (!strcmp(type+2, "GSA"))
369  return MINMEA_SENTENCE_GSA;
370  if (!strcmp(type+2, "GLL"))
371  return MINMEA_SENTENCE_GLL;
372  if (!strcmp(type+2, "GST"))
373  return MINMEA_SENTENCE_GST;
374  if (!strcmp(type+2, "GSV"))
375  return MINMEA_SENTENCE_GSV;
376  if (!strcmp(type+2, "VTG"))
377  return MINMEA_SENTENCE_VTG;
378  if (!strcmp(type+2, "ZDA"))
379  return MINMEA_SENTENCE_ZDA;
380  if (!strcmp(type, "PUBX"))
381  return MINMEA_SENTENCE_PUBX;
382 
383  return MINMEA_UNKNOWN;
384 }
385 
386 bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence)
387 {
388  // $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62
389  char type[6];
390  char validity;
391  int latitude_direction;
392  int longitude_direction;
393  int variation_direction;
394  if (!minmea_scan(sentence, "tTcfdfdffDfd",
395  type,
396  &frame->time,
397  &validity,
398  &frame->latitude, &latitude_direction,
399  &frame->longitude, &longitude_direction,
400  &frame->speed,
401  &frame->course,
402  &frame->date,
403  &frame->variation, &variation_direction))
404  return false;
405  if (strcmp(type+2, "RMC"))
406  return false;
407 
408  frame->valid = (validity == 'A');
409  frame->latitude.value *= latitude_direction;
410  frame->longitude.value *= longitude_direction;
411  frame->variation.value *= variation_direction;
412 
413  return true;
414 }
415 
416 bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence)
417 {
418  // $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
419  char type[6];
420  int latitude_direction;
421  int longitude_direction;
422 
423  if (!minmea_scan(sentence, "tTfdfdiiffcfci_",
424  type,
425  &frame->time,
426  &frame->latitude, &latitude_direction,
427  &frame->longitude, &longitude_direction,
428  &frame->fix_quality,
429  &frame->satellites_tracked,
430  &frame->hdop,
431  &frame->altitude, &frame->altitude_units,
432  &frame->height, &frame->height_units,
433  &frame->dgps_age))
434  return false;
435  if (strcmp(type+2, "GGA"))
436  return false;
437 
438  frame->latitude.value *= latitude_direction;
439  frame->longitude.value *= longitude_direction;
440 
441  return true;
442 }
443 
444 bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence)
445 {
446  // $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
447  char type[6];
448 
449  if (!minmea_scan(sentence, "tciiiiiiiiiiiiifff",
450  type,
451  &frame->mode,
452  &frame->fix_type,
453  &frame->sats[0],
454  &frame->sats[1],
455  &frame->sats[2],
456  &frame->sats[3],
457  &frame->sats[4],
458  &frame->sats[5],
459  &frame->sats[6],
460  &frame->sats[7],
461  &frame->sats[8],
462  &frame->sats[9],
463  &frame->sats[10],
464  &frame->sats[11],
465  &frame->pdop,
466  &frame->hdop,
467  &frame->vdop))
468  return false;
469  if (strcmp(type+2, "GSA"))
470  return false;
471 
472  return true;
473 }
474 
475 bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence)
476 {
477  // $GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41$;
478  char type[6];
479  int latitude_direction;
480  int longitude_direction;
481 
482  if (!minmea_scan(sentence, "tfdfdTc;c",
483  type,
484  &frame->latitude, &latitude_direction,
485  &frame->longitude, &longitude_direction,
486  &frame->time,
487  &frame->status,
488  &frame->mode))
489  return false;
490  if (strcmp(type+2, "GLL"))
491  return false;
492 
493  frame->latitude.value *= latitude_direction;
494  frame->longitude.value *= longitude_direction;
495 
496  return true;
497 }
498 
499 bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence)
500 {
501  // $GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58
502  char type[6];
503 
504  if (!minmea_scan(sentence, "tTfffffff",
505  type,
506  &frame->time,
507  &frame->rms_deviation,
508  &frame->semi_major_deviation,
509  &frame->semi_minor_deviation,
510  &frame->semi_major_orientation,
511  &frame->latitude_error_deviation,
512  &frame->longitude_error_deviation,
513  &frame->altitude_error_deviation))
514  return false;
515  if (strcmp(type+2, "GST"))
516  return false;
517 
518  return true;
519 }
520 
521 bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence)
522 {
523  // $GPGSV,3,1,11,03,03,111,00,04,15,270,00,06,01,010,00,13,06,292,00*74
524  // $GPGSV,3,3,11,22,42,067,42,24,14,311,43,27,05,244,00,,,,*4D
525  // $GPGSV,4,2,11,08,51,203,30,09,45,215,28*75
526  // $GPGSV,4,4,13,39,31,170,27*40
527  // $GPGSV,4,4,13*7B
528  char type[6];
529 
530  if (!minmea_scan(sentence, "tiii;iiiiiiiiiiiiiiii",
531  type,
532  &frame->total_msgs,
533  &frame->msg_nr,
534  &frame->total_sats,
535  &frame->sats[0].nr,
536  &frame->sats[0].elevation,
537  &frame->sats[0].azimuth,
538  &frame->sats[0].snr,
539  &frame->sats[1].nr,
540  &frame->sats[1].elevation,
541  &frame->sats[1].azimuth,
542  &frame->sats[1].snr,
543  &frame->sats[2].nr,
544  &frame->sats[2].elevation,
545  &frame->sats[2].azimuth,
546  &frame->sats[2].snr,
547  &frame->sats[3].nr,
548  &frame->sats[3].elevation,
549  &frame->sats[3].azimuth,
550  &frame->sats[3].snr
551  )) {
552  return false;
553  }
554  if (strcmp(type+2, "GSV"))
555  return false;
556 
557  return true;
558 }
559 
560 bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence)
561 {
562  // $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
563  // $GPVTG,156.1,T,140.9,M,0.0,N,0.0,K*41
564  // $GPVTG,096.5,T,083.5,M,0.0,N,0.0,K,D*22
565  // $GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F
566  char type[6];
567  char c_true, c_magnetic, c_knots, c_kph, c_faa_mode;
568 
569  if (!minmea_scan(sentence, "tfcfcfcfc;c",
570  type,
571  &frame->true_track_degrees,
572  &c_true,
573  &frame->magnetic_track_degrees,
574  &c_magnetic,
575  &frame->speed_knots,
576  &c_knots,
577  &frame->speed_kph,
578  &c_kph,
579  &c_faa_mode))
580  return false;
581  if (strcmp(type+2, "VTG"))
582  return false;
583  // check chars
584  if (c_true != 'T' ||
585  c_magnetic != 'M' ||
586  c_knots != 'N' ||
587  c_kph != 'K')
588  return false;
589  frame->faa_mode = (enum minmea_faa_mode)c_faa_mode;
590 
591  return true;
592 }
593 
594 bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence)
595 {
596  // $GPZDA,201530.00,04,07,2002,00,00*60
597  char type[6];
598 
599  if(!minmea_scan(sentence, "tTiiiii",
600  type,
601  &frame->time,
602  &frame->date.day,
603  &frame->date.month,
604  &frame->date.year,
605  &frame->hour_offset,
606  &frame->minute_offset))
607  return false;
608  if (strcmp(type+2, "ZDA"))
609  return false;
610 
611  // check offsets
612  if (abs(frame->hour_offset) > 13 ||
613  frame->minute_offset > 59 ||
614  frame->minute_offset < 0)
615  return false;
616 
617  return true;
618 }
619 
620 bool minmea_parse_pubx(struct minmea_sentence_pubx *frame, const char *sentence)
621 {
622  // $PUBX,00,175056.00,4951.55890,N,01819.12489,E,285.013,G3,8.0,6.2,0.363,182.11,0.059,,2.28,1.83,1.80,8,0,0*6C
623  char type[6];
624  int latitude_direction;
625  int longitude_direction;
626  char status[3];
627 
628  if(!minmea_scan(sentence, "tiTfdfdfsfffffifffiii",
629  type,
630  &frame->msg_id,
631  &frame->time,
632  &frame->latitude, &latitude_direction,
633  &frame->longitude, &longitude_direction,
634  &frame->altitude,
635  status,
636  &frame->h_accuracy,
637  &frame->v_accuracy,
638  &frame->speed,
639  &frame->course,
640  &frame->velocity,
641  &frame->age,
642  &frame->hdop,
643  &frame->vdop,
644  &frame->tdop,
645  &frame->satellites,
646  &frame->reserved,
647  &frame->dr))
648  return false;
649  if (strcmp(type, "PUBX"))
650  return false;
651 
652  frame->latitude.value *= latitude_direction;
653  frame->longitude.value *= longitude_direction;
654 
655  // No fix
656  frame->status = 0;
657  // Dead reckoning only solution
658  if (strcmp(status, "DR") == 0)
659  frame->status = 1;
660  // Stand alone 2D solution
661  if (strcmp(status, "G2") == 0)
662  frame->status = 2;
663  // Stand alone 3D solution
664  if (strcmp(status, "G3") == 0)
665  frame->status = 3;
666  // Differential 2D solution
667  if (strcmp(status, "D2") == 0)
668  frame->status = 4;
669  // Differential 3D solution
670  if (strcmp(status, "D3") == 0)
671  frame->status = 5;
672  // Combined GPS + dead reckoning solution
673  if (strcmp(status, "RK") == 0)
674  frame->status = 6;
675  // Time only solution
676  if (strcmp(status, "TT") == 0)
677  frame->status = 7;
678 
679  return true;
680 }
681 
682 /* vim: set ts=4 sw=4 et: */