/* * Copyright © 2014 Kosma Moczek * This program is free software. It comes without any warranty, to the extent * permitted by applicable law. You can redistribute it and/or modify it under * the terms of the Do What The Fuck You Want To Public License, Version 2, as * published by Sam Hocevar. See the COPYING file for more details. */ // We are lacking timegm, but we only handle UTC and use mktime instead. See // https://github.com/cloudyourcar/minmea-t/ README.md #define timegm mktime #include "minmea.h" #include #include #include #include #define boolstr(s) ((s) ? "true" : "false") static int hex2int(char c) { if (c >= '0' && c <= '9') return c - '0'; if (c >= 'A' && c <= 'F') return c - 'A' + 10; if (c >= 'a' && c <= 'f') return c - 'a' + 10; return -1; } uint8_t minmea_checksum(const char *sentence) { // Support senteces with or without the starting dollar sign. if (*sentence == '$') sentence++; uint8_t checksum = 0x00; // The optional checksum is an XOR of all bytes between "$" and "*". while (*sentence && *sentence != '*') checksum ^= *sentence++; return checksum; } bool minmea_check(const char *sentence, bool strict) { uint8_t checksum = 0x00; // Sequence length is limited. if (strlen(sentence) > MINMEA_MAX_LENGTH + 3) return false; // A valid sentence starts with "$". if (*sentence++ != '$') return false; // The optional checksum is an XOR of all bytes between "$" and "*". while (*sentence && *sentence != '*' && isprint((unsigned char) *sentence)) checksum ^= *sentence++; // If checksum is present... if (*sentence == '*') { // Extract checksum. sentence++; int upper = hex2int(*sentence++); if (upper == -1) return false; int lower = hex2int(*sentence++); if (lower == -1) return false; int expected = upper << 4 | lower; // Check for checksum mismatch. if (checksum != expected) return false; } else if (strict) { // Discard non-checksummed frames in strict mode. return false; } // The only stuff allowed at this point is a newline. if (*sentence && strcmp(sentence, "\n") && strcmp(sentence, "\r\n")) return false; return true; } static inline bool minmea_isfield(char c) { return isprint((unsigned char) c) && c != ',' && c != '*'; } bool minmea_scan(const char *sentence, const char *format, ...) { bool result = false; bool optional = false; va_list ap; va_start(ap, format); const char *field = sentence; #define next_field() \ do { \ /* Progress to the next field. */ \ while (minmea_isfield(*sentence)) \ sentence++; \ /* Make sure there is a field there. */ \ if (*sentence == ',') { \ sentence++; \ field = sentence; \ } else { \ field = NULL; \ } \ } while (0) while (*format) { char type = *format++; if (type == ';') { // All further fields are optional. optional = true; continue; } if (!field && !optional) { // Field requested but we ran out if input. Bail out. goto parse_error; } switch (type) { case 'c': { // Single character field (char). char value = '\0'; if (field && minmea_isfield(*field)) value = *field; *va_arg(ap, char *) = value; } break; case 'd': { // Single character direction field (int). int value = 0; if (field && minmea_isfield(*field)) { switch (*field) { case 'N': case 'E': value = 1; break; case 'S': case 'W': value = -1; break; default: goto parse_error; } } *va_arg(ap, int *) = value; } break; case 'f': { // Fractional value with scale (struct minmea_float). int sign = 0; int_least32_t value = -1; int_least32_t scale = 0; if (field) { while (minmea_isfield(*field)) { if (*field == '+' && !sign && value == -1) { sign = 1; } else if (*field == '-' && !sign && value == -1) { sign = -1; } else if (isdigit((unsigned char) *field)) { int digit = *field - '0'; if (value == -1) value = 0; if (value > (INT_LEAST32_MAX-digit) / 10) { /* we ran out of bits, what do we do? */ if (scale) { /* truncate extra precision */ break; } else { /* integer overflow. bail out. */ goto parse_error; } } value = (10 * value) + digit; if (scale) scale *= 10; } else if (*field == '.' && scale == 0) { scale = 1; } else if (*field == ' ') { /* Allow spaces at the start of the field. Not NMEA * conformant, but some modules do this. */ if (sign != 0 || value != -1 || scale != 0) goto parse_error; } else { goto parse_error; } field++; } } if ((sign || scale) && value == -1) goto parse_error; if (value == -1) { /* No digits were scanned. */ value = 0; scale = 0; } else if (scale == 0) { /* No decimal point. */ scale = 1; } if (sign) value *= sign; *va_arg(ap, struct minmea_float *) = (struct minmea_float) {value, scale}; } break; case 'i': { // Integer value, default 0 (int). int value = 0; if (field) { char *endptr; value = strtol(field, &endptr, 10); if (minmea_isfield(*endptr)) goto parse_error; } *va_arg(ap, int *) = value; } break; case 's': { // String value (char *). char *buf = va_arg(ap, char *); if (field) { while (minmea_isfield(*field)) *buf++ = *field++; } *buf = '\0'; } break; case 't': { // NMEA talker+sentence identifier (char *). // This field is always mandatory. if (!field) goto parse_error; if (field[0] != '$') goto parse_error; for (int f=0; f<5; f++) if (!minmea_isfield(field[1+f])) goto parse_error; char *buf = va_arg(ap, char *); memcpy(buf, field+1, 5); buf[5] = '\0'; } break; case 'D': { // Date (int, int, int), -1 if empty. struct minmea_date *date = va_arg(ap, struct minmea_date *); int d = -1, m = -1, y = -1; if (field && minmea_isfield(*field)) { // Always six digits. for (int f=0; f<6; f++) if (!isdigit((unsigned char) field[f])) goto parse_error; d = strtol((char[]) {field[0], field[1], '\0'}, NULL, 10); m = strtol((char[]) {field[2], field[3], '\0'}, NULL, 10); y = strtol((char[]) {field[4], field[5], '\0'}, NULL, 10); } date->day = d; date->month = m; date->year = y; } break; case 'T': { // Time (int, int, int, int), -1 if empty. struct minmea_time *time_ = va_arg(ap, struct minmea_time *); int h = -1, i = -1, s = -1, u = -1; if (field && minmea_isfield(*field)) { // Minimum required: integer time. for (int f=0; f<6; f++) if (!isdigit((unsigned char) field[f])) goto parse_error; h = strtol((char[]) {field[0], field[1], '\0'}, NULL, 10); i = strtol((char[]) {field[2], field[3], '\0'}, NULL, 10); s = strtol((char[]) {field[4], field[5], '\0'}, NULL, 10); field += 6; // Extra: fractional time. Saved as microseconds. if (*field++ == '.') { int value = 0; int scale = 1000000; while (isdigit((unsigned char) *field) && scale > 1) { value = (value * 10) + (*field++ - '0'); scale /= 10; } u = value * scale; } else { u = 0; } } time_->hours = h; time_->minutes = i; time_->seconds = s; time_->microseconds = u; } break; case '_': { // Ignore the field. } break; default: { // Unknown. goto parse_error; } break; } next_field(); } result = true; parse_error: va_end(ap); return result; } bool minmea_talker_id(char talker[3], const char *sentence) { char type[6]; if (!minmea_scan(sentence, "t", type)) return false; talker[0] = type[0]; talker[1] = type[1]; talker[2] = '\0'; return true; } enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict) { if (!minmea_check(sentence, strict)) return MINMEA_INVALID; char type[6]; if (!minmea_scan(sentence, "t", type)) return MINMEA_INVALID; if (!strcmp(type+2, "RMC")) return MINMEA_SENTENCE_RMC; if (!strcmp(type+2, "GGA")) return MINMEA_SENTENCE_GGA; if (!strcmp(type+2, "GSA")) return MINMEA_SENTENCE_GSA; if (!strcmp(type+2, "GLL")) return MINMEA_SENTENCE_GLL; if (!strcmp(type+2, "GST")) return MINMEA_SENTENCE_GST; if (!strcmp(type+2, "GSV")) return MINMEA_SENTENCE_GSV; return MINMEA_UNKNOWN; } bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence) { // $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62 char type[6]; char validity; int latitude_direction; int longitude_direction; int variation_direction; if (!minmea_scan(sentence, "tTcfdfdffDfd", type, &frame->time, &validity, &frame->latitude, &latitude_direction, &frame->longitude, &longitude_direction, &frame->speed, &frame->course, &frame->date, &frame->variation, &variation_direction)) return false; if (strcmp(type+2, "RMC")) return false; frame->valid = (validity == 'A'); frame->latitude.value *= latitude_direction; frame->longitude.value *= longitude_direction; frame->variation.value *= variation_direction; return true; } bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence) { // $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47 char type[6]; int latitude_direction; int longitude_direction; if (!minmea_scan(sentence, "tTfdfdiiffcfci_", type, &frame->time, &frame->latitude, &latitude_direction, &frame->longitude, &longitude_direction, &frame->fix_quality, &frame->satellites_tracked, &frame->hdop, &frame->altitude, &frame->altitude_units, &frame->height, &frame->height_units, &frame->dgps_age)) return false; if (strcmp(type+2, "GGA")) return false; frame->latitude.value *= latitude_direction; frame->longitude.value *= longitude_direction; return true; } bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence) { // $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39 char type[6]; if (!minmea_scan(sentence, "tciiiiiiiiiiiiifff", type, &frame->mode, &frame->fix_type, &frame->sats[0], &frame->sats[1], &frame->sats[2], &frame->sats[3], &frame->sats[4], &frame->sats[5], &frame->sats[6], &frame->sats[7], &frame->sats[8], &frame->sats[9], &frame->sats[10], &frame->sats[11], &frame->pdop, &frame->hdop, &frame->vdop)) return false; if (strcmp(type+2, "GSA")) return false; return true; } bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence) { // $GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41$; char type[6]; int latitude_direction; int longitude_direction; if (!minmea_scan(sentence, "tfdfdTc;c", type, &frame->latitude, &latitude_direction, &frame->longitude, &longitude_direction, &frame->time, &frame->status, &frame->mode)) return false; if (strcmp(type+2, "GLL")) return false; frame->latitude.value *= latitude_direction; frame->longitude.value *= longitude_direction; return true; } bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence) { // $GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58 char type[6]; if (!minmea_scan(sentence, "tTfffffff", type, &frame->time, &frame->rms_deviation, &frame->semi_major_deviation, &frame->semi_minor_deviation, &frame->semi_major_orientation, &frame->latitude_error_deviation, &frame->longitude_error_deviation, &frame->altitude_error_deviation)) return false; if (strcmp(type+2, "GST")) return false; return true; } bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence) { // $GPGSV,3,1,11,03,03,111,00,04,15,270,00,06,01,010,00,13,06,292,00*74 // $GPGSV,3,3,11,22,42,067,42,24,14,311,43,27,05,244,00,,,,*4D // $GPGSV,4,2,11,08,51,203,30,09,45,215,28*75 // $GPGSV,4,4,13,39,31,170,27*40 // $GPGSV,4,4,13*7B char type[6]; if (!minmea_scan(sentence, "tiii;iiiiiiiiiiiiiiii", type, &frame->total_msgs, &frame->msg_nr, &frame->total_sats, &frame->sats[0].nr, &frame->sats[0].elevation, &frame->sats[0].azimuth, &frame->sats[0].snr, &frame->sats[1].nr, &frame->sats[1].elevation, &frame->sats[1].azimuth, &frame->sats[1].snr, &frame->sats[2].nr, &frame->sats[2].elevation, &frame->sats[2].azimuth, &frame->sats[2].snr, &frame->sats[3].nr, &frame->sats[3].elevation, &frame->sats[3].azimuth, &frame->sats[3].snr )) { return false; } if (strcmp(type+2, "GSV")) return false; return true; } int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_) { if (date->year == -1 || time_->hours == -1) return -1; struct tm tm; memset(&tm, 0, sizeof(tm)); tm.tm_year = 2000 + date->year - 1900; tm.tm_mon = date->month - 1; tm.tm_mday = date->day; tm.tm_hour = time_->hours; tm.tm_min = time_->minutes; tm.tm_sec = time_->seconds; time_t timestamp = timegm(&tm); /* See README.md if your system lacks timegm(). */ if (timestamp != -1) { ts->tv_sec = timestamp; ts->tv_nsec = time_->microseconds * 1000; return 0; } else { return -1; } } /* vim: set ts=4 sw=4 et: */