| 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: */ |