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