From cabe757bb9049387144763ea5ed79358a28ae43d Mon Sep 17 00:00:00 2001 From: Marco Oliverio Date: Sun, 4 Sep 2022 08:56:40 +0200 Subject: [PATCH] library: add minmea parsing library --- include/minmea.h | 295 ++++++++++++++++++++ src/minmea.c | 679 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 974 insertions(+) create mode 100644 include/minmea.h create mode 100644 src/minmea.c diff --git a/include/minmea.h b/include/minmea.h new file mode 100644 index 0000000..494b218 --- /dev/null +++ b/include/minmea.h @@ -0,0 +1,295 @@ +/* + * 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. + */ + +#ifndef MINMEA_H +#define MINMEA_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include +#ifdef MINMEA_INCLUDE_COMPAT +#include +#endif + +#ifndef MINMEA_MAX_SENTENCE_LENGTH +#define MINMEA_MAX_SENTENCE_LENGTH 80 +#endif + +enum minmea_sentence_id { + MINMEA_INVALID = -1, + MINMEA_UNKNOWN = 0, + MINMEA_SENTENCE_GBS, + MINMEA_SENTENCE_GGA, + MINMEA_SENTENCE_GLL, + MINMEA_SENTENCE_GSA, + MINMEA_SENTENCE_GST, + MINMEA_SENTENCE_GSV, + MINMEA_SENTENCE_RMC, + MINMEA_SENTENCE_VTG, + MINMEA_SENTENCE_ZDA, +}; + +struct minmea_float { + int_least32_t value; + int_least32_t scale; +}; + +struct minmea_date { + int day; + int month; + int year; +}; + +struct minmea_time { + int hours; + int minutes; + int seconds; + int microseconds; +}; + +struct minmea_sentence_gbs { + struct minmea_time time; + struct minmea_float err_latitude; + struct minmea_float err_longitude; + struct minmea_float err_altitude; + int svid; + struct minmea_float prob; + struct minmea_float bias; + struct minmea_float stddev; +}; + +struct minmea_sentence_rmc { + struct minmea_time time; + bool valid; + struct minmea_float latitude; + struct minmea_float longitude; + struct minmea_float speed; + struct minmea_float course; + struct minmea_date date; + struct minmea_float variation; +}; + +struct minmea_sentence_gga { + struct minmea_time time; + struct minmea_float latitude; + struct minmea_float longitude; + int fix_quality; + int satellites_tracked; + struct minmea_float hdop; + struct minmea_float altitude; char altitude_units; + struct minmea_float height; char height_units; + struct minmea_float dgps_age; +}; + +enum minmea_gll_status { + MINMEA_GLL_STATUS_DATA_VALID = 'A', + MINMEA_GLL_STATUS_DATA_NOT_VALID = 'V', +}; + +// FAA mode added to some fields in NMEA 2.3. +enum minmea_faa_mode { + MINMEA_FAA_MODE_AUTONOMOUS = 'A', + MINMEA_FAA_MODE_DIFFERENTIAL = 'D', + MINMEA_FAA_MODE_ESTIMATED = 'E', + MINMEA_FAA_MODE_MANUAL = 'M', + MINMEA_FAA_MODE_SIMULATED = 'S', + MINMEA_FAA_MODE_NOT_VALID = 'N', + MINMEA_FAA_MODE_PRECISE = 'P', +}; + +struct minmea_sentence_gll { + struct minmea_float latitude; + struct minmea_float longitude; + struct minmea_time time; + char status; + char mode; +}; + +struct minmea_sentence_gst { + struct minmea_time time; + struct minmea_float rms_deviation; + struct minmea_float semi_major_deviation; + struct minmea_float semi_minor_deviation; + struct minmea_float semi_major_orientation; + struct minmea_float latitude_error_deviation; + struct minmea_float longitude_error_deviation; + struct minmea_float altitude_error_deviation; +}; + +enum minmea_gsa_mode { + MINMEA_GPGSA_MODE_AUTO = 'A', + MINMEA_GPGSA_MODE_FORCED = 'M', +}; + +enum minmea_gsa_fix_type { + MINMEA_GPGSA_FIX_NONE = 1, + MINMEA_GPGSA_FIX_2D = 2, + MINMEA_GPGSA_FIX_3D = 3, +}; + +struct minmea_sentence_gsa { + char mode; + int fix_type; + int sats[12]; + struct minmea_float pdop; + struct minmea_float hdop; + struct minmea_float vdop; +}; + +struct minmea_sat_info { + int nr; + int elevation; + int azimuth; + int snr; +}; + +struct minmea_sentence_gsv { + int total_msgs; + int msg_nr; + int total_sats; + struct minmea_sat_info sats[4]; +}; + +struct minmea_sentence_vtg { + struct minmea_float true_track_degrees; + struct minmea_float magnetic_track_degrees; + struct minmea_float speed_knots; + struct minmea_float speed_kph; + enum minmea_faa_mode faa_mode; +}; + +struct minmea_sentence_zda { + struct minmea_time time; + struct minmea_date date; + int hour_offset; + int minute_offset; +}; + +/** + * Calculate raw sentence checksum. Does not check sentence integrity. + */ +uint8_t minmea_checksum(const char *sentence); + +/** + * Check sentence validity and checksum. Returns true for valid sentences. + */ +bool minmea_check(const char *sentence, bool strict); + +/** + * Determine talker identifier. + */ +bool minmea_talker_id(char talker[3], const char *sentence); + +/** + * Determine sentence identifier. + */ +enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict); + +/** + * Scanf-like processor for NMEA sentences. Supports the following formats: + * c - single character (char *) + * d - direction, returned as 1/-1, default 0 (int *) + * f - fractional, returned as value + scale (struct minmea_float *) + * i - decimal, default zero (int *) + * s - string (char *) + * t - talker identifier and type (char *) + * D - date (struct minmea_date *) + * T - time stamp (struct minmea_time *) + * _ - ignore this field + * ; - following fields are optional + * Returns true on success. See library source code for details. + */ +bool minmea_scan(const char *sentence, const char *format, ...); + +/* + * Parse a specific type of sentence. Return true on success. + */ +bool minmea_parse_gbs(struct minmea_sentence_gbs *frame, const char *sentence); +bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence); +bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence); +bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence); +bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence); +bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence); +bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence); +bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence); +bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence); + +/** + * Convert GPS UTC date/time representation to a UNIX calendar time. + */ +int minmea_getdatetime(struct tm *tm, const struct minmea_date *date, const struct minmea_time *time_); + +/** + * Convert GPS UTC date/time representation to a UNIX timestamp. + */ +int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_); + +/** + * Rescale a fixed-point value to a different scale. Rounds towards zero. + */ +static inline int_least32_t minmea_rescale(const struct minmea_float *f, int_least32_t new_scale) +{ + if (f->scale == 0) + return 0; + if (f->scale == new_scale) + return f->value; + if (f->scale > new_scale) + return (f->value + ((f->value > 0) - (f->value < 0)) * f->scale/new_scale/2) / (f->scale/new_scale); + else + return f->value * (new_scale/f->scale); +} + +/** + * Convert a fixed-point value to a floating-point value. + * Returns NaN for "unknown" values. + */ +static inline float minmea_tofloat(const struct minmea_float *f) +{ + if (f->scale == 0) + return NAN; + return (float) f->value / (float) f->scale; +} + +/** + * Convert a raw coordinate to a floating point DD.DDD... value. + * Returns NaN for "unknown" values. + */ +static inline float minmea_tocoord(const struct minmea_float *f) +{ + if (f->scale == 0) + return NAN; + if (f->scale > (INT_LEAST32_MAX / 100)) + return NAN; + if (f->scale < (INT_LEAST32_MIN / 100)) + return NAN; + int_least32_t degrees = f->value / (f->scale * 100); + int_least32_t minutes = f->value % (f->scale * 100); + return (float) degrees + (float) minutes / (60 * f->scale); +} + +/** + * Check whether a character belongs to the set of characters allowed in a + * sentence data field. + */ +static inline bool minmea_isfield(char c) { + return isprint((unsigned char) c) && c != ',' && c != '*'; +} + +#ifdef __cplusplus +} +#endif + +#endif /* MINMEA_H */ + +/* vim: set ts=4 sw=4 et: */ diff --git a/src/minmea.c b/src/minmea.c new file mode 100644 index 0000000..c96b392 --- /dev/null +++ b/src/minmea.c @@ -0,0 +1,679 @@ +/* + * 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. + */ + +#include "minmea.h" + +#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; + + // 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. + while (*sentence == '\r' || *sentence == '\n') { + sentence++; + } + + if (*sentence) { + return false; + } + + return true; +} + +bool minmea_scan(const char *sentence, const char *format, ...) +{ + bool result = false; + bool optional = false; + + if (sentence == NULL) + return 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; + + char dArr[] = {field[0], field[1], '\0'}; + char mArr[] = {field[2], field[3], '\0'}; + char yArr[] = {field[4], field[5], '\0'}; + d = strtol(dArr, NULL, 10); + m = strtol(mArr, NULL, 10); + y = strtol(yArr, 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; + + char hArr[] = {field[0], field[1], '\0'}; + char iArr[] = {field[2], field[3], '\0'}; + char sArr[] = {field[4], field[5], '\0'}; + h = strtol(hArr, NULL, 10); + i = strtol(iArr, NULL, 10); + s = strtol(sArr, NULL, 10); + field += 6; + + // Extra: fractional time. Saved as microseconds. + if (*field++ == '.') { + uint32_t value = 0; + uint32_t scale = 1000000LU; + 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; + } + } + + 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, "GBS")) + return MINMEA_SENTENCE_GBS; + if (!strcmp(type+2, "GGA")) + return MINMEA_SENTENCE_GGA; + if (!strcmp(type+2, "GLL")) + return MINMEA_SENTENCE_GLL; + if (!strcmp(type+2, "GSA")) + return MINMEA_SENTENCE_GSA; + if (!strcmp(type+2, "GST")) + return MINMEA_SENTENCE_GST; + if (!strcmp(type+2, "GSV")) + return MINMEA_SENTENCE_GSV; + if (!strcmp(type+2, "RMC")) + return MINMEA_SENTENCE_RMC; + if (!strcmp(type+2, "VTG")) + return MINMEA_SENTENCE_VTG; + if (!strcmp(type+2, "ZDA")) + return MINMEA_SENTENCE_ZDA; + + return MINMEA_UNKNOWN; +} + +bool minmea_parse_gbs(struct minmea_sentence_gbs *frame, const char *sentence) +{ + // $GNGBS,170556.00,3.0,2.9,8.3,,,,*5C + char type[6]; + if (!minmea_scan(sentence, "tTfffifff", + type, + &frame->time, + &frame->err_latitude, + &frame->err_longitude, + &frame->err_altitude, + &frame->svid, + &frame->prob, + &frame->bias, + &frame->stddev + )) + return false; + if (strcmp(type+2, "GBS")) + return false; + + return true; +} + +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, "tTfdfdiiffcfcf_", + 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; +} + +bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence) +{ + // $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + // $GPVTG,156.1,T,140.9,M,0.0,N,0.0,K*41 + // $GPVTG,096.5,T,083.5,M,0.0,N,0.0,K,D*22 + // $GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F + char type[6]; + char c_true, c_magnetic, c_knots, c_kph, c_faa_mode; + + if (!minmea_scan(sentence, "t;fcfcfcfcc", + type, + &frame->true_track_degrees, + &c_true, + &frame->magnetic_track_degrees, + &c_magnetic, + &frame->speed_knots, + &c_knots, + &frame->speed_kph, + &c_kph, + &c_faa_mode)) + return false; + if (strcmp(type+2, "VTG")) + return false; + // values are only valid with the accompanying characters + if (c_true != 'T') + frame->true_track_degrees.scale = 0; + if (c_magnetic != 'M') + frame->magnetic_track_degrees.scale = 0; + if (c_knots != 'N') + frame->speed_knots.scale = 0; + if (c_kph != 'K') + frame->speed_kph.scale = 0; + frame->faa_mode = (enum minmea_faa_mode)c_faa_mode; + + return true; +} + +bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence) +{ + // $GPZDA,201530.00,04,07,2002,00,00*60 + char type[6]; + + if(!minmea_scan(sentence, "tTiiiii", + type, + &frame->time, + &frame->date.day, + &frame->date.month, + &frame->date.year, + &frame->hour_offset, + &frame->minute_offset)) + return false; + if (strcmp(type+2, "ZDA")) + return false; + + // check offsets + if (abs(frame->hour_offset) > 13 || + frame->minute_offset > 59 || + frame->minute_offset < 0) + return false; + + return true; +} + +int minmea_getdatetime(struct tm *tm, const struct minmea_date *date, const struct minmea_time *time_) +{ + if (date->year == -1 || time_->hours == -1) + return -1; + + memset(tm, 0, sizeof(*tm)); + if (date->year < 80) { + tm->tm_year = 2000 + date->year - 1900; // 2000-2079 + } else if (date->year >= 1900) { + tm->tm_year = date->year - 1900; // 4 digit year, use directly + } else { + tm->tm_year = date->year; // 1980-1999 + } + 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; + + return 0; +} + +int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_) +{ + struct tm tm; + if (minmea_getdatetime(&tm, date, time_)) + return -1; + + time_t timestamp = timegm(&tm); /* See README.md if your system lacks timegm(). */ + if (timestamp != (time_t)-1) { + ts->tv_sec = timestamp; + ts->tv_nsec = time_->microseconds * 1000; + return 0; + } else { + return -1; + } +} + +/* vim: set ts=4 sw=4 et: */