Now available: The Design and Implementation of the FreeBSD Operating System (Second Edition)
[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]
FreeBSD/Linux Kernel Cross Reference
sys/kern/tty_nmea.c
Version:
- FREEBSD - FREEBSD-13-STABLE - FREEBSD-13-0 - FREEBSD-12-STABLE - FREEBSD-12-0 - FREEBSD-11-STABLE - FREEBSD-11-0 - FREEBSD-10-STABLE - FREEBSD-10-0 - FREEBSD-9-STABLE - FREEBSD-9-0 - FREEBSD-8-STABLE - FREEBSD-8-0 - FREEBSD-7-STABLE - FREEBSD-7-0 - FREEBSD-6-STABLE - FREEBSD-6-0 - FREEBSD-5-STABLE - FREEBSD-5-0 - FREEBSD-4-STABLE - FREEBSD-3-STABLE - FREEBSD22 - l41 - OPENBSD - linux-2.6 - MK84 - PLAN9 - xnu-8792
SearchContext: - none - 3 - 10
SearchContext: - none - 3 - 10
1 /* $OpenBSD: tty_nmea.c,v 1.51 2022/04/02 22:45:18 mlarkin Exp $ */ 2 3 /* 4 * Copyright (c) 2006, 2007, 2008 Marc Balmer <mbalmer@openbsd.org> 5 * 6 * Permission to use, copy, modify, and distribute this software for any 7 * purpose with or without fee is hereby granted, provided that the above 8 * copyright notice and this permission notice appear in all copies. 9 * 10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 17 */ 18 19 /* 20 * A tty line discipline to decode NMEA 0183 data to get the time 21 * and GPS position data 22 */ 23 24 #include <sys/param.h> 25 #include <sys/systm.h> 26 #include <sys/malloc.h> 27 #include <sys/sensors.h> 28 #include <sys/tty.h> 29 #include <sys/conf.h> 30 #include <sys/time.h> 31 32 #ifdef NMEA_DEBUG 33 #define DPRINTFN(n, x) do { if (nmeadebug > (n)) printf x; } while (0) 34 int nmeadebug = 0; 35 #else 36 #define DPRINTFN(n, x) 37 #endif 38 #define DPRINTF(x) DPRINTFN(0, x) 39 40 void nmeaattach(int); 41 42 #define NMEAMAX 82 43 #define MAXFLDS 32 44 #define KNOTTOMS (51444 / 100) 45 #ifdef NMEA_DEBUG 46 #define TRUSTTIME 30 47 #else 48 #define TRUSTTIME (10 * 60) /* 10 minutes */ 49 #endif 50 51 int nmea_count, nmea_nxid; 52 53 struct nmea { 54 char cbuf[NMEAMAX]; /* receive buffer */ 55 struct ksensor time; /* the timedelta sensor */ 56 struct ksensor signal; /* signal status */ 57 struct ksensor latitude; 58 struct ksensor longitude; 59 struct ksensor altitude; 60 struct ksensor speed; 61 struct ksensordev timedev; 62 struct timespec ts; /* current timestamp */ 63 struct timespec lts; /* timestamp of last '$' seen */ 64 struct timeout nmea_tout; /* invalidate sensor */ 65 int64_t gap; /* gap between two sentences */ 66 #ifdef NMEA_DEBUG 67 int gapno; 68 #endif 69 int64_t last; /* last time rcvd */ 70 int sync; /* if 1, waiting for '$' */ 71 int pos; /* position in rcv buffer */ 72 int no_pps; /* no PPS although requested */ 73 char mode; /* GPS mode */ 74 }; 75 76 /* NMEA decoding */ 77 void nmea_scan(struct nmea *, struct tty *); 78 void nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt); 79 void nmea_decode_gga(struct nmea *, struct tty *, char *fld[], int fldcnt); 80 81 /* date and time conversion */ 82 int nmea_date_to_nano(char *s, int64_t *nano); 83 int nmea_time_to_nano(char *s, int64_t *nano); 84 85 /* longitude and latitude conversion */ 86 int nmea_degrees(int64_t *dst, char *src, int neg); 87 int nmea_atoi(int64_t *dst, char *src); 88 89 /* degrade the timedelta sensor */ 90 void nmea_timeout(void *); 91 92 void 93 nmeaattach(int dummy) 94 { 95 /* noop */ 96 } 97 98 int 99 nmeaopen(dev_t dev, struct tty *tp, struct proc *p) 100 { 101 struct nmea *np; 102 int error; 103 104 if (tp->t_line == NMEADISC) 105 return (ENODEV); 106 if ((error = suser(p)) != 0) 107 return (error); 108 np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK | M_ZERO); 109 snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d", 110 nmea_nxid++); 111 nmea_count++; 112 np->time.status = SENSOR_S_UNKNOWN; 113 np->time.type = SENSOR_TIMEDELTA; 114 np->time.flags = SENSOR_FINVALID; 115 sensor_attach(&np->timedev, &np->time); 116 117 np->signal.type = SENSOR_INDICATOR; 118 np->signal.status = SENSOR_S_UNKNOWN; 119 np->signal.value = 0; 120 strlcpy(np->signal.desc, "Signal", sizeof(np->signal.desc)); 121 sensor_attach(&np->timedev, &np->signal); 122 123 np->latitude.type = SENSOR_ANGLE; 124 np->latitude.status = SENSOR_S_UNKNOWN; 125 np->latitude.flags = SENSOR_FINVALID; 126 np->latitude.value = 0; 127 strlcpy(np->latitude.desc, "Latitude", sizeof(np->latitude.desc)); 128 sensor_attach(&np->timedev, &np->latitude); 129 130 np->longitude.type = SENSOR_ANGLE; 131 np->longitude.status = SENSOR_S_UNKNOWN; 132 np->longitude.flags = SENSOR_FINVALID; 133 np->longitude.value = 0; 134 strlcpy(np->longitude.desc, "Longitude", sizeof(np->longitude.desc)); 135 sensor_attach(&np->timedev, &np->longitude); 136 137 np->altitude.type = SENSOR_DISTANCE; 138 np->altitude.status = SENSOR_S_UNKNOWN; 139 np->altitude.flags = SENSOR_FINVALID; 140 np->altitude.value = 0; 141 strlcpy(np->altitude.desc, "Altitude", sizeof(np->altitude.desc)); 142 sensor_attach(&np->timedev, &np->altitude); 143 144 np->speed.type = SENSOR_VELOCITY; 145 np->speed.status = SENSOR_S_UNKNOWN; 146 np->speed.flags = SENSOR_FINVALID; 147 np->speed.value = 0; 148 strlcpy(np->speed.desc, "Ground speed", sizeof(np->speed.desc)); 149 sensor_attach(&np->timedev, &np->speed); 150 151 np->sync = 1; 152 tp->t_sc = (caddr_t)np; 153 154 error = linesw[TTYDISC].l_open(dev, tp, p); 155 if (error) { 156 free(np, M_DEVBUF, sizeof(*np)); 157 tp->t_sc = NULL; 158 } else { 159 sensordev_install(&np->timedev); 160 timeout_set(&np->nmea_tout, nmea_timeout, np); 161 } 162 return (error); 163 } 164 165 int 166 nmeaclose(struct tty *tp, int flags, struct proc *p) 167 { 168 struct nmea *np = (struct nmea *)tp->t_sc; 169 170 tp->t_line = TTYDISC; /* switch back to termios */ 171 timeout_del(&np->nmea_tout); 172 sensordev_deinstall(&np->timedev); 173 free(np, M_DEVBUF, sizeof(*np)); 174 tp->t_sc = NULL; 175 nmea_count--; 176 if (nmea_count == 0) 177 nmea_nxid = 0; 178 return (linesw[TTYDISC].l_close(tp, flags, p)); 179 } 180 181 /* Collect NMEA sentences from the tty. */ 182 int 183 nmeainput(int c, struct tty *tp) 184 { 185 struct nmea *np = (struct nmea *)tp->t_sc; 186 struct timespec ts; 187 int64_t gap; 188 long tmin, tmax; 189 190 switch (c) { 191 case '$': 192 nanotime(&ts); 193 np->pos = np->sync = 0; 194 gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) - 195 (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec); 196 197 np->lts.tv_sec = ts.tv_sec; 198 np->lts.tv_nsec = ts.tv_nsec; 199 200 if (gap <= np->gap) 201 break; 202 203 np->ts.tv_sec = ts.tv_sec; 204 np->ts.tv_nsec = ts.tv_nsec; 205 206 #ifdef NMEA_DEBUG 207 if (nmeadebug > 0) { 208 linesw[TTYDISC].l_rint('[', tp); 209 linesw[TTYDISC].l_rint('' + np->gapno++, tp); 210 linesw[TTYDISC].l_rint(']', tp); 211 } 212 #endif 213 np->gap = gap; 214 215 /* 216 * If a tty timestamp is available, make sure its value is 217 * reasonable by comparing against the timestamp just taken. 218 * If they differ by more than 2 seconds, assume no PPS signal 219 * is present, note the fact, and keep using the timestamp 220 * value. When this happens, the sensor state is set to 221 * CRITICAL later when the GPRMC sentence is decoded. 222 */ 223 if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR | 224 TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) { 225 tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec); 226 tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec); 227 if (tmax - tmin > 1) 228 np->no_pps = 1; 229 else { 230 np->ts.tv_sec = tp->t_tv.tv_sec; 231 np->ts.tv_nsec = tp->t_tv.tv_usec * 232 1000L; 233 np->no_pps = 0; 234 } 235 } 236 break; 237 case '\r': 238 case '\n': 239 if (!np->sync) { 240 np->cbuf[np->pos] = '\0'; 241 nmea_scan(np, tp); 242 np->sync = 1; 243 } 244 break; 245 default: 246 if (!np->sync && np->pos < (NMEAMAX - 1)) 247 np->cbuf[np->pos++] = c; 248 break; 249 } 250 /* pass data to termios */ 251 return (linesw[TTYDISC].l_rint(c, tp)); 252 } 253 254 /* Scan the NMEA sentence just received. */ 255 void 256 nmea_scan(struct nmea *np, struct tty *tp) 257 { 258 int fldcnt = 0, cksum = 0, msgcksum, n; 259 char *fld[MAXFLDS], *cs; 260 261 /* split into fields and calculate the checksum */ 262 fld[fldcnt++] = &np->cbuf[0]; /* message type */ 263 for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) { 264 switch (np->cbuf[n]) { 265 case '*': 266 np->cbuf[n] = '\0'; 267 cs = &np->cbuf[n + 1]; 268 break; 269 case ',': 270 if (fldcnt < MAXFLDS) { 271 cksum ^= np->cbuf[n]; 272 np->cbuf[n] = '\0'; 273 fld[fldcnt++] = &np->cbuf[n + 1]; 274 } else { 275 DPRINTF(("nr of fields in %s sentence exceeds " 276 "maximum of %d\n", fld[0], MAXFLDS)); 277 return; 278 } 279 break; 280 default: 281 cksum ^= np->cbuf[n]; 282 } 283 } 284 285 /* 286 * we only look at the messages coming from well-known sources or 'talkers', 287 * distinguished by the two-chars prefix, the most common being: 288 * GPS (GP) 289 * Glonass (GL) 290 * BeiDou (BD) 291 * Galileo (GA) 292 * 'Any kind/a mix of GNSS systems' (GN) 293 */ 294 if (strncmp(fld[0], "BD", 2) && 295 strncmp(fld[0], "GA", 2) && 296 strncmp(fld[0], "GL", 2) && 297 strncmp(fld[0], "GN", 2) && 298 strncmp(fld[0], "GP", 2)) 299 return; 300 301 /* we look for the RMC & GGA messages */ 302 if (strncmp(fld[0] + 2, "RMC", 3) && 303 strncmp(fld[0] + 2, "GGA", 3)) 304 return; 305 306 /* if we have a checksum, verify it */ 307 if (cs != NULL) { 308 msgcksum = 0; 309 while (*cs) { 310 if ((*cs >= '' && *cs <= '9') || 311 (*cs >= 'A' && *cs <= 'F')) { 312 if (msgcksum) 313 msgcksum <<= 4; 314 if (*cs >= '' && *cs<= '9') 315 msgcksum += *cs - ''; 316 else if (*cs >= 'A' && *cs <= 'F') 317 msgcksum += 10 + *cs - 'A'; 318 cs++; 319 } else { 320 DPRINTF(("bad char %c in checksum\n", *cs)); 321 return; 322 } 323 } 324 if (msgcksum != cksum) { 325 DPRINTF(("checksum mismatch\n")); 326 return; 327 } 328 } 329 if (strncmp(fld[0] + 2, "RMC", 3) == 0) 330 nmea_gprmc(np, tp, fld, fldcnt); 331 if (strncmp(fld[0] + 2, "GGA", 3) == 0) 332 nmea_decode_gga(np, tp, fld, fldcnt); 333 } 334 335 /* Decode the recommended minimum specific GPS/TRANSIT data. */ 336 void 337 nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt) 338 { 339 int64_t date_nano, time_nano, nmea_now; 340 int jumped = 0; 341 342 if (fldcnt < 12 || fldcnt > 14) { 343 DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt)); 344 return; 345 } 346 if (nmea_time_to_nano(fld[1], &time_nano)) { 347 DPRINTF(("gprmc: illegal time, %s\n", fld[1])); 348 return; 349 } 350 if (nmea_date_to_nano(fld[9], &date_nano)) { 351 DPRINTF(("gprmc: illegal date, %s\n", fld[9])); 352 return; 353 } 354 nmea_now = date_nano + time_nano; 355 if (nmea_now <= np->last) { 356 DPRINTF(("gprmc: time not monotonically increasing\n")); 357 jumped = 1; 358 } 359 np->last = nmea_now; 360 np->gap = 0LL; 361 #ifdef NMEA_DEBUG 362 if (np->time.status == SENSOR_S_UNKNOWN) { 363 np->time.status = SENSOR_S_OK; 364 timeout_add_sec(&np->nmea_tout, TRUSTTIME); 365 } 366 np->gapno = 0; 367 if (nmeadebug > 0) { 368 linesw[TTYDISC].l_rint('[', tp); 369 linesw[TTYDISC].l_rint('C', tp); 370 linesw[TTYDISC].l_rint(']', tp); 371 } 372 #endif 373 374 np->time.value = np->ts.tv_sec * 1000000000LL + 375 np->ts.tv_nsec - nmea_now; 376 np->time.tv.tv_sec = np->ts.tv_sec; 377 np->time.tv.tv_usec = np->ts.tv_nsec / 1000L; 378 379 if (fldcnt < 13) 380 strlcpy(np->time.desc, "GPS", sizeof(np->time.desc)); 381 else if (*fld[12] != np->mode) { 382 np->mode = *fld[12]; 383 switch (np->mode) { 384 case 'S': 385 strlcpy(np->time.desc, "GPS simulated", 386 sizeof(np->time.desc)); 387 break; 388 case 'E': 389 strlcpy(np->time.desc, "GPS estimated", 390 sizeof(np->time.desc)); 391 break; 392 case 'A': 393 strlcpy(np->time.desc, "GPS autonomous", 394 sizeof(np->time.desc)); 395 break; 396 case 'D': 397 strlcpy(np->time.desc, "GPS differential", 398 sizeof(np->time.desc)); 399 break; 400 case 'N': 401 strlcpy(np->time.desc, "GPS invalid", 402 sizeof(np->time.desc)); 403 break; 404 default: 405 strlcpy(np->time.desc, "GPS unknown", 406 sizeof(np->time.desc)); 407 DPRINTF(("gprmc: unknown mode '%c'\n", np->mode)); 408 } 409 } 410 switch (*fld[2]) { 411 case 'A': /* The GPS has a fix, (re)arm the timeout. */ 412 /* XXX is 'D' also a valid state? */ 413 np->time.status = SENSOR_S_OK; 414 np->signal.value = 1; 415 np->signal.status = SENSOR_S_OK; 416 np->latitude.status = SENSOR_S_OK; 417 np->longitude.status = SENSOR_S_OK; 418 np->speed.status = SENSOR_S_OK; 419 np->time.flags &= ~SENSOR_FINVALID; 420 np->latitude.flags &= ~SENSOR_FINVALID; 421 np->longitude.flags &= ~SENSOR_FINVALID; 422 np->speed.flags &= ~SENSOR_FINVALID; 423 break; 424 case 'V': /* 425 * The GPS indicates a warning status, do not add to 426 * the timeout, if the condition persist, the sensor 427 * will be degraded. Signal the condition through 428 * the signal sensor. 429 */ 430 np->signal.value = 0; 431 np->signal.status = SENSOR_S_CRIT; 432 np->latitude.status = SENSOR_S_WARN; 433 np->longitude.status = SENSOR_S_WARN; 434 np->speed.status = SENSOR_S_WARN; 435 break; 436 } 437 if (nmea_degrees(&np->latitude.value, fld[3], *fld[4] == 'S' ? 1 : 0)) 438 np->latitude.status = SENSOR_S_WARN; 439 if (nmea_degrees(&np->longitude.value,fld[5], *fld[6] == 'W' ? 1 : 0)) 440 np->longitude.status = SENSOR_S_WARN; 441 442 if (nmea_atoi(&np->speed.value, fld[7])) 443 np->speed.status = SENSOR_S_WARN; 444 /* convert from knot to um/s */ 445 np->speed.value *= KNOTTOMS; 446 447 if (jumped) 448 np->time.status = SENSOR_S_WARN; 449 if (np->time.status == SENSOR_S_OK) 450 timeout_add_sec(&np->nmea_tout, TRUSTTIME); 451 /* 452 * If tty timestamping is requested, but no PPS signal is present, set 453 * the sensor state to CRITICAL. 454 */ 455 if (np->no_pps) 456 np->time.status = SENSOR_S_CRIT; 457 } 458 459 /* Decode the GPS fix data for altitude. 460 * - field 9 is the altitude in meters 461 * $GNGGA,085901.00,1234.5678,N,00987.12345,E,1,12,0.84,1040.9,M,47.4,M,,*4B 462 */ 463 void 464 nmea_decode_gga(struct nmea *np, struct tty *tp, char *fld[], int fldcnt) 465 { 466 if (fldcnt != 15) { 467 DPRINTF(("GGA: field count mismatch, %d\n", fldcnt)); 468 return; 469 } 470 #ifdef NMEA_DEBUG 471 if (nmeadebug > 0) { 472 linesw[TTYDISC].l_rint('[', tp); 473 linesw[TTYDISC].l_rint('C', tp); 474 linesw[TTYDISC].l_rint(']', tp); 475 } 476 #endif 477 478 np->altitude.status = SENSOR_S_OK; 479 if (nmea_atoi(&np->altitude.value, fld[9])) 480 np->altitude.status = SENSOR_S_WARN; 481 482 /* convert to uMeter */ 483 np->altitude.value *= 1000; 484 np->altitude.flags &= ~SENSOR_FINVALID; 485 } 486 487 /* 488 * Convert nmea integer/decimal values in the form of XXXX.Y to an integer value 489 * if it's a meter/altitude value, will be returned as mm 490 */ 491 int 492 nmea_atoi(int64_t *dst, char *src) 493 { 494 char *p; 495 int i = 3; /* take 3 digits */ 496 *dst = 0; 497 498 for (p = src; *p && *p != '.' && *p >= '' && *p <= '9' ; ) 499 *dst = *dst * 10 + (*p++ - ''); 500 501 /* *p should be '.' at that point */ 502 if (*p != '.') 503 return -1; /* no decimal point, or bogus value ? */ 504 p++; 505 506 /* read digits after decimal point, stop at first non-digit */ 507 for (; *p && i > 0 && *p >= '' && *p <= '9' ; i--) 508 *dst = *dst * 10 + (*p++ - ''); 509 510 for (; i > 0 ; i--) 511 *dst *= 10; 512 513 DPRINTFN(2,("%s -> %lld\n", src, *dst)); 514 return 0; 515 } 516 517 /* 518 * Convert a nmea position in the form DDDMM.MMMM to an 519 * angle sensor value (degrees*1000000) 520 */ 521 int 522 nmea_degrees(int64_t *dst, char *src, int neg) 523 { 524 size_t ppos; 525 int i, n; 526 int64_t deg = 0, min = 0; 527 char *p; 528 529 while (*src == '') 530 ++src; /* skip leading zeroes */ 531 532 for (p = src, ppos = 0; *p; ppos++) 533 if (*p++ == '.') 534 break; 535 536 if (*p == '\0') 537 return (-1); /* no decimal point */ 538 539 for (n = 0; *src && n + 2 < ppos; n++) 540 deg = deg * 10 + (*src++ - ''); 541 542 for (; *src && n < ppos; n++) 543 min = min * 10 + (*src++ - ''); 544 545 src++; /* skip decimal point */ 546 547 for (; *src && n < (ppos + 4); n++) 548 min = min * 10 + (*src++ - ''); 549 550 for (i=0; i < 6 + ppos - n; i++) 551 min *= 10; 552 553 deg = deg * 1000000 + (min/60); 554 555 *dst = neg ? -deg : deg; 556 return (0); 557 } 558 559 /* 560 * Convert a NMEA 0183 formatted date string to seconds since the epoch. 561 * The string must be of the form DDMMYY. 562 * Return 0 on success, -1 if illegal characters are encountered. 563 */ 564 int 565 nmea_date_to_nano(char *s, int64_t *nano) 566 { 567 struct clock_ymdhms ymd; 568 time_t secs; 569 char *p; 570 int n; 571 572 /* make sure the input contains only numbers and is six digits long */ 573 for (n = 0, p = s; n < 6 && *p && *p >= '' && *p <= '9'; n++, p++) 574 ; 575 if (n != 6 || (*p != '\0')) 576 return (-1); 577 578 ymd.dt_year = 2000 + (s[4] - '') * 10 + (s[5] - ''); 579 ymd.dt_mon = (s[2] - '') * 10 + (s[3] - ''); 580 ymd.dt_day = (s[0] - '') * 10 + (s[1] - ''); 581 ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0; 582 583 secs = clock_ymdhms_to_secs(&ymd); 584 *nano = secs * 1000000000LL; 585 return (0); 586 } 587 588 /* 589 * Convert NMEA 0183 formatted time string to nanoseconds since midnight. 590 * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615). 591 * Return 0 on success, -1 if illegal characters are encountered. 592 */ 593 int 594 nmea_time_to_nano(char *s, int64_t *nano) 595 { 596 long fac = 36000L, div = 6L, secs = 0L, frac = 0L; 597 char ul = '2'; 598 int n; 599 600 for (n = 0, secs = 0; fac && *s && *s >= '' && *s <= ul; s++, n++) { 601 secs += (*s - '') * fac; 602 div = 16 - div; 603 fac /= div; 604 switch (n) { 605 case 0: 606 if (*s <= '1') 607 ul = '9'; 608 else 609 ul = '3'; 610 break; 611 case 1: 612 case 3: 613 ul = '5'; 614 break; 615 case 2: 616 case 4: 617 ul = '9'; 618 break; 619 } 620 } 621 if (fac) 622 return (-1); 623 624 /* Handle the fractions of a second, up to a maximum of 6 digits. */ 625 div = 1L; 626 if (*s == '.') { 627 for (++s; div < 1000000 && *s && *s >= '' && *s <= '9'; s++) { 628 frac *= 10; 629 frac += (*s - ''); 630 div *= 10; 631 } 632 } 633 634 if (*s != '\0') 635 return (-1); 636 637 *nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div); 638 return (0); 639 } 640 641 /* 642 * Degrade the sensor state if we received no NMEA sentences for more than 643 * TRUSTTIME seconds. 644 */ 645 void 646 nmea_timeout(void *xnp) 647 { 648 struct nmea *np = xnp; 649 650 np->signal.value = 0; 651 np->signal.status = SENSOR_S_CRIT; 652 if (np->time.status == SENSOR_S_OK) { 653 np->time.status = SENSOR_S_WARN; 654 np->latitude.status = SENSOR_S_WARN; 655 np->longitude.status = SENSOR_S_WARN; 656 np->altitude.status = SENSOR_S_WARN; 657 np->speed.status = SENSOR_S_WARN; 658 /* 659 * further degrade in TRUSTTIME seconds if no new valid NMEA 660 * sentences are received. 661 */ 662 timeout_add_sec(&np->nmea_tout, TRUSTTIME); 663 } else { 664 np->time.status = SENSOR_S_CRIT; 665 np->latitude.status = SENSOR_S_CRIT; 666 np->longitude.status = SENSOR_S_CRIT; 667 np->altitude.status = SENSOR_S_CRIT; 668 np->speed.status = SENSOR_S_CRIT; 669 } 670 }
Cache object: 85f747a0b4edcd42f2b777e0dbbb166d
[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]