Debian Codegraph @

minmea.c

raw
1 /*
2 * Copyright © 2014 Kosma Moczek <[email protected]>
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 && strncmp(sentence, "\n", 1) && strncmp(sentence, "\r\n", 2))
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 for (int f=0; f<5; f++)
250 if (!minmea_isfield(field[1+f]))
251 goto parse_error;
252
253 char *buf = va_arg(ap, char *);
254 memcpy(buf, field+1, 5);
255 buf[5] = '\0';
256 } break;
257
258 case 'D': { // Date (int, int, int), -1 if empty.
259 struct minmea_date *date = va_arg(ap, struct minmea_date *);
260
261 int d = -1, m = -1, y = -1;
262
263 if (field && minmea_isfield(*field)) {
264 // Always six digits.
265 for (int f=0; f<6; f++)
266 if (!isdigit((unsigned char) field[f]))
267 goto parse_error;
268
269 char dArr[] = {field[0], field[1], '\0'};
270 char mArr[] = {field[2], field[3], '\0'};
271 char yArr[] = {field[4], field[5], '\0'};
272 d = strtol(dArr, NULL, 10);
273 m = strtol(mArr, NULL, 10);
274 y = strtol(yArr, NULL, 10);
275 }
276
277 date->day = d;
278 date->month = m;
279 date->year = y;
280 } break;
281
282 case 'T': { // Time (int, int, int, int), -1 if empty.
283 struct minmea_time *time_ = va_arg(ap, struct minmea_time *);
284
285 int h = -1, i = -1, s = -1, u = -1;
286
287 if (field && minmea_isfield(*field)) {
288 // Minimum required: integer time.
289 for (int f=0; f<6; f++)
290 if (!isdigit((unsigned char) field[f]))
291 goto parse_error;
292
293 char hArr[] = {field[0], field[1], '\0'};
294 char iArr[] = {field[2], field[3], '\0'};
295 char sArr[] = {field[4], field[5], '\0'};
296 h = strtol(hArr, NULL, 10);
297 i = strtol(iArr, NULL, 10);
298 s = strtol(sArr, NULL, 10);
299 field += 6;
300
301 // Extra: fractional time. Saved as microseconds.
302 if (*field++ == '.') {
303 uint32_t value = 0;
304 uint32_t scale = 1000000LU;
305 while (isdigit((unsigned char) *field) && scale > 1) {
306 value = (value * 10) + (*field++ - '0');
307 scale /= 10;
308 }
309 u = value * scale;
310 } else {
311 u = 0;
312 }
313 }
314
315 time_->hours = h;
316 time_->minutes = i;
317 time_->seconds = s;
318 time_->microseconds = u;
319 } break;
320
321 case '_': { // Ignore the field.
322 } break;
323
324 default: { // Unknown.
325 goto parse_error;
326 } break;
327 }
328
329 next_field();
330 }
331
332 result = true;
333
334 parse_error:
335 va_end(ap);
336 return result;
337 }
338
339 bool minmea_talker_id(char talker[3], const char *sentence)
340 {
341 char type[6];
342 if (!minmea_scan(sentence, "t", type))
343 return false;
344
345 talker[0] = type[0];
346 talker[1] = type[1];
347 talker[2] = '\0';
348
349 return true;
350 }
351
352 enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict)
353 {
354 if (!minmea_check(sentence, strict))
355 return MINMEA_INVALID;
356
357 char type[6];
358 if (!minmea_scan(sentence, "t", type))
359 return MINMEA_INVALID;
360
361 if (!strcmp(type+2, "RMC"))
362 return MINMEA_SENTENCE_RMC;
363 if (!strcmp(type+2, "GGA"))
364 return MINMEA_SENTENCE_GGA;
365 if (!strcmp(type+2, "GSA"))
366 return MINMEA_SENTENCE_GSA;
367 if (!strcmp(type+2, "GLL"))
368 return MINMEA_SENTENCE_GLL;
369 if (!strcmp(type+2, "GST"))
370 return MINMEA_SENTENCE_GST;
371 if (!strcmp(type+2, "GSV"))
372 return MINMEA_SENTENCE_GSV;
373 if (!strcmp(type+2, "VTG"))
374 return MINMEA_SENTENCE_VTG;
375 if (!strcmp(type+2, "ZDA"))
376 return MINMEA_SENTENCE_ZDA;
377
378 return MINMEA_UNKNOWN;
379 }
380
381 bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence)
382 {
383 // $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62
384 char type[6];
385 char validity;
386 int latitude_direction;
387 int longitude_direction;
388 int variation_direction;
389 if (!minmea_scan(sentence, "tTcfdfdffDfd",
390 type,
391 &frame->time,
392 &validity,
393 &frame->latitude, &latitude_direction,
394 &frame->longitude, &longitude_direction,
395 &frame->speed,
396 &frame->course,
397 &frame->date,
398 &frame->variation, &variation_direction))
399 return false;
400 if (strcmp(type+2, "RMC"))
401 return false;
402
403 frame->valid = (validity == 'A');
404 frame->latitude.value *= latitude_direction;
405 frame->longitude.value *= longitude_direction;
406 frame->variation.value *= variation_direction;
407
408 return true;
409 }
410
411 bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence)
412 {
413 // $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
414 char type[6];
415 int latitude_direction;
416 int longitude_direction;
417
418 if (!minmea_scan(sentence, "tTfdfdiiffcfci_",
419 type,
420 &frame->time,
421 &frame->latitude, &latitude_direction,
422 &frame->longitude, &longitude_direction,
423 &frame->fix_quality,
424 &frame->satellites_tracked,
425 &frame->hdop,
426 &frame->altitude, &frame->altitude_units,
427 &frame->height, &frame->height_units,
428 &frame->dgps_age))
429 return false;
430 if (strcmp(type+2, "GGA"))
431 return false;
432
433 frame->latitude.value *= latitude_direction;
434 frame->longitude.value *= longitude_direction;
435
436 return true;
437 }
438
439 bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence)
440 {
441 // $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
442 char type[6];
443
444 if (!minmea_scan(sentence, "tciiiiiiiiiiiiifff",
445 type,
446 &frame->mode,
447 &frame->fix_type,
448 &frame->sats[0],
449 &frame->sats[1],
450 &frame->sats[2],
451 &frame->sats[3],
452 &frame->sats[4],
453 &frame->sats[5],
454 &frame->sats[6],
455 &frame->sats[7],
456 &frame->sats[8],
457 &frame->sats[9],
458 &frame->sats[10],
459 &frame->sats[11],
460 &frame->pdop,
461 &frame->hdop,
462 &frame->vdop))
463 return false;
464 if (strcmp(type+2, "GSA"))
465 return false;
466
467 return true;
468 }
469
470 bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence)
471 {
472 // $GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41$;
473 char type[6];
474 int latitude_direction;
475 int longitude_direction;
476
477 if (!minmea_scan(sentence, "tfdfdTc;c",
478 type,
479 &frame->latitude, &latitude_direction,
480 &frame->longitude, &longitude_direction,
481 &frame->time,
482 &frame->status,
483 &frame->mode))
484 return false;
485 if (strcmp(type+2, "GLL"))
486 return false;
487
488 frame->latitude.value *= latitude_direction;
489 frame->longitude.value *= longitude_direction;
490
491 return true;
492 }
493
494 bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence)
495 {
496 // $GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58
497 char type[6];
498
499 if (!minmea_scan(sentence, "tTfffffff",
500 type,
501 &frame->time,
502 &frame->rms_deviation,
503 &frame->semi_major_deviation,
504 &frame->semi_minor_deviation,
505 &frame->semi_major_orientation,
506 &frame->latitude_error_deviation,
507 &frame->longitude_error_deviation,
508 &frame->altitude_error_deviation))
509 return false;
510 if (strcmp(type+2, "GST"))
511 return false;
512
513 return true;
514 }
515
516 bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence)
517 {
518 // $GPGSV,3,1,11,03,03,111,00,04,15,270,00,06,01,010,00,13,06,292,00*74
519 // $GPGSV,3,3,11,22,42,067,42,24,14,311,43,27,05,244,00,,,,*4D
520 // $GPGSV,4,2,11,08,51,203,30,09,45,215,28*75
521 // $GPGSV,4,4,13,39,31,170,27*40
522 // $GPGSV,4,4,13*7B
523 char type[6];
524
525 if (!minmea_scan(sentence, "tiii;iiiiiiiiiiiiiiii",
526 type,
527 &frame->total_msgs,
528 &frame->msg_nr,
529 &frame->total_sats,
530 &frame->sats[0].nr,
531 &frame->sats[0].elevation,
532 &frame->sats[0].azimuth,
533 &frame->sats[0].snr,
534 &frame->sats[1].nr,
535 &frame->sats[1].elevation,
536 &frame->sats[1].azimuth,
537 &frame->sats[1].snr,
538 &frame->sats[2].nr,
539 &frame->sats[2].elevation,
540 &frame->sats[2].azimuth,
541 &frame->sats[2].snr,
542 &frame->sats[3].nr,
543 &frame->sats[3].elevation,
544 &frame->sats[3].azimuth,
545 &frame->sats[3].snr
546 )) {
547 return false;
548 }
549 if (strcmp(type+2, "GSV"))
550 return false;
551
552 return true;
553 }
554
555 bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence)
556 {
557 // $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
558 // $GPVTG,156.1,T,140.9,M,0.0,N,0.0,K*41
559 // $GPVTG,096.5,T,083.5,M,0.0,N,0.0,K,D*22
560 // $GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F
561 char type[6];
562 char c_true, c_magnetic, c_knots, c_kph, c_faa_mode;
563
564 if (!minmea_scan(sentence, "tfcfcfcfc;c",
565 type,
566 &frame->true_track_degrees,
567 &c_true,
568 &frame->magnetic_track_degrees,
569 &c_magnetic,
570 &frame->speed_knots,
571 &c_knots,
572 &frame->speed_kph,
573 &c_kph,
574 &c_faa_mode))
575 return false;
576 if (strcmp(type+2, "VTG"))
577 return false;
578 // check chars
579 if (c_true != 'T' ||
580 c_magnetic != 'M' ||
581 c_knots != 'N' ||
582 c_kph != 'K')
583 return false;
584 frame->faa_mode = c_faa_mode;
585
586 return true;
587 }
588
589 bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence)
590 {
591 // $GPZDA,201530.00,04,07,2002,00,00*60
592 char type[6];
593
594 if(!minmea_scan(sentence, "tTiiiii",
595 type,
596 &frame->time,
597 &frame->date.day,
598 &frame->date.month,
599 &frame->date.year,
600 &frame->hour_offset,
601 &frame->minute_offset))
602 return false;
603 if (strcmp(type+2, "ZDA"))
604 return false;
605
606 // check offsets
607 if (abs(frame->hour_offset) > 13 ||
608 frame->minute_offset > 59 ||
609 frame->minute_offset < 0)
610 return false;
611
612 return true;
613 }
614
615 int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_)
616 {
617 if (date->year == -1 || time_->hours == -1)
618 return -1;
619
620 struct tm tm;
621 memset(&tm, 0, sizeof(tm));
622 if (date->year < 80) {
623 tm.tm_year = 2000 + date->year - 1900; // 2000-2079
624 } else if (date->year >= 1900) {
625 tm.tm_year = date->year - 1900; // 4 digit year, use directly
626 } else {
627 tm.tm_year = date->year; // 1980-1999
628 }
629 tm.tm_mon = date->month - 1;
630 tm.tm_mday = date->day;
631 tm.tm_hour = time_->hours;
632 tm.tm_min = time_->minutes;
633 tm.tm_sec = time_->seconds;
634
635 time_t timestamp = timegm(&tm); /* See README.md if your system lacks timegm(). */
636 if (timestamp != -1) {
637 ts->tv_sec = timestamp;
638 ts->tv_nsec = time_->microseconds * 1000;
639 return 0;
640 } else {
641 return -1;
642 }
643 }
644
645 /* vim: set ts=4 sw=4 et: */