Edit
IABSD.fr/src/sys/kern/tty_nmea.c
mlarkin
- sys/kern/tty_nmea.c
-
/* $OpenBSD: tty_nmea.c,v 1.51 2022/04/02 22:45:18 mlarkin Exp $ */ /* * Copyright (c) 2006, 2007, 2008 Marc Balmer <mbalmer@openbsd.org> * * Permission to use, copy, modify, and distribute this software for any * purpose with or without fee is hereby granted, provided that the above * copyright notice and this permission notice appear in all copies. * * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */ /* * A tty line discipline to decode NMEA 0183 data to get the time * and GPS position data */ #include <sys/param.h> #include <sys/systm.h> #include <sys/malloc.h> #include <sys/sensors.h> #include <sys/tty.h> #include <sys/conf.h> #include <sys/time.h> #ifdef NMEA_DEBUG #define DPRINTFN(n, x) do { if (nmeadebug > (n)) printf x; } while (0) int nmeadebug = 0; #else #define DPRINTFN(n, x) #endif #define DPRINTF(x) DPRINTFN(0, x) void nmeaattach(int); #define NMEAMAX 82 #define MAXFLDS 32 #define KNOTTOMS (51444 / 100) #ifdef NMEA_DEBUG #define TRUSTTIME 30 #else #define TRUSTTIME (10 * 60) /* 10 minutes */ #endif int nmea_count, nmea_nxid; struct nmea { char cbuf[NMEAMAX]; /* receive buffer */ struct ksensor time; /* the timedelta sensor */ struct ksensor signal; /* signal status */ struct ksensor latitude; struct ksensor longitude; struct ksensor altitude; struct ksensor speed; struct ksensordev timedev; struct timespec ts; /* current timestamp */ struct timespec lts; /* timestamp of last '$' seen */ struct timeout nmea_tout; /* invalidate sensor */ int64_t gap; /* gap between two sentences */ #ifdef NMEA_DEBUG int gapno; #endif int64_t last; /* last time rcvd */ int sync; /* if 1, waiting for '$' */ int pos; /* position in rcv buffer */ int no_pps; /* no PPS although requested */ char mode; /* GPS mode */ }; /* NMEA decoding */ void nmea_scan(struct nmea *, struct tty *); void nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt); void nmea_decode_gga(struct nmea *, struct tty *, char *fld[], int fldcnt); /* date and time conversion */ int nmea_date_to_nano(char *s, int64_t *nano); int nmea_time_to_nano(char *s, int64_t *nano); /* longitude and latitude conversion */ int nmea_degrees(int64_t *dst, char *src, int neg); int nmea_atoi(int64_t *dst, char *src); /* degrade the timedelta sensor */ void nmea_timeout(void *); void nmeaattach(int dummy) { /* noop */ } int nmeaopen(dev_t dev, struct tty *tp, struct proc *p) { struct nmea *np; int error; if (tp->t_line == NMEADISC) return (ENODEV); if ((error = suser(p)) != 0) return (error); np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK | M_ZERO); snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d", nmea_nxid++); nmea_count++; np->time.status = SENSOR_S_UNKNOWN; np->time.type = SENSOR_TIMEDELTA; np->time.flags = SENSOR_FINVALID; sensor_attach(&np->timedev, &np->time); np->signal.type = SENSOR_INDICATOR; np->signal.status = SENSOR_S_UNKNOWN; np->signal.value = 0; strlcpy(np->signal.desc, "Signal", sizeof(np->signal.desc)); sensor_attach(&np->timedev, &np->signal); np->latitude.type = SENSOR_ANGLE; np->latitude.status = SENSOR_S_UNKNOWN; np->latitude.flags = SENSOR_FINVALID; np->latitude.value = 0; strlcpy(np->latitude.desc, "Latitude", sizeof(np->latitude.desc)); sensor_attach(&np->timedev, &np->latitude); np->longitude.type = SENSOR_ANGLE; np->longitude.status = SENSOR_S_UNKNOWN; np->longitude.flags = SENSOR_FINVALID; np->longitude.value = 0; strlcpy(np->longitude.desc, "Longitude", sizeof(np->longitude.desc)); sensor_attach(&np->timedev, &np->longitude); np->altitude.type = SENSOR_DISTANCE; np->altitude.status = SENSOR_S_UNKNOWN; np->altitude.flags = SENSOR_FINVALID; np->altitude.value = 0; strlcpy(np->altitude.desc, "Altitude", sizeof(np->altitude.desc)); sensor_attach(&np->timedev, &np->altitude); np->speed.type = SENSOR_VELOCITY; np->speed.status = SENSOR_S_UNKNOWN; np->speed.flags = SENSOR_FINVALID; np->speed.value = 0; strlcpy(np->speed.desc, "Ground speed", sizeof(np->speed.desc)); sensor_attach(&np->timedev, &np->speed); np->sync = 1; tp->t_sc = (caddr_t)np; error = linesw[TTYDISC].l_open(dev, tp, p); if (error) { free(np, M_DEVBUF, sizeof(*np)); tp->t_sc = NULL; } else { sensordev_install(&np->timedev); timeout_set(&np->nmea_tout, nmea_timeout, np); } return (error); } int nmeaclose(struct tty *tp, int flags, struct proc *p) { struct nmea *np = (struct nmea *)tp->t_sc; tp->t_line = TTYDISC; /* switch back to termios */ timeout_del(&np->nmea_tout); sensordev_deinstall(&np->timedev); free(np, M_DEVBUF, sizeof(*np)); tp->t_sc = NULL; nmea_count--; if (nmea_count == 0) nmea_nxid = 0; return (linesw[TTYDISC].l_close(tp, flags, p)); } /* Collect NMEA sentences from the tty. */ int nmeainput(int c, struct tty *tp) { struct nmea *np = (struct nmea *)tp->t_sc; struct timespec ts; int64_t gap; long tmin, tmax; switch (c) { case '$': nanotime(&ts); np->pos = np->sync = 0; gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) - (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec); np->lts.tv_sec = ts.tv_sec; np->lts.tv_nsec = ts.tv_nsec; if (gap <= np->gap) break; np->ts.tv_sec = ts.tv_sec; np->ts.tv_nsec = ts.tv_nsec; #ifdef NMEA_DEBUG if (nmeadebug > 0) { linesw[TTYDISC].l_rint('[', tp); linesw[TTYDISC].l_rint('0' + np->gapno++, tp); linesw[TTYDISC].l_rint(']', tp); } #endif np->gap = gap; /* * If a tty timestamp is available, make sure its value is * reasonable by comparing against the timestamp just taken. * If they differ by more than 2 seconds, assume no PPS signal * is present, note the fact, and keep using the timestamp * value. When this happens, the sensor state is set to * CRITICAL later when the GPRMC sentence is decoded. */ if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR | TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) { tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec); tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec); if (tmax - tmin > 1) np->no_pps = 1; else { np->ts.tv_sec = tp->t_tv.tv_sec; np->ts.tv_nsec = tp->t_tv.tv_usec * 1000L; np->no_pps = 0; } } break; case '\r': case '\n': if (!np->sync) { np->cbuf[np->pos] = '\0'; nmea_scan(np, tp); np->sync = 1; } break; default: if (!np->sync && np->pos < (NMEAMAX - 1)) np->cbuf[np->pos++] = c; break; } /* pass data to termios */ return (linesw[TTYDISC].l_rint(c, tp)); } /* Scan the NMEA sentence just received. */ void nmea_scan(struct nmea *np, struct tty *tp) { int fldcnt = 0, cksum = 0, msgcksum, n; char *fld[MAXFLDS], *cs; /* split into fields and calculate the checksum */ fld[fldcnt++] = &np->cbuf[0]; /* message type */ for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) { switch (np->cbuf[n]) { case '*': np->cbuf[n] = '\0'; cs = &np->cbuf[n + 1]; break; case ',': if (fldcnt < MAXFLDS) { cksum ^= np->cbuf[n]; np->cbuf[n] = '\0'; fld[fldcnt++] = &np->cbuf[n + 1]; } else { DPRINTF(("nr of fields in %s sentence exceeds " "maximum of %d\n", fld[0], MAXFLDS)); return; } break; default: cksum ^= np->cbuf[n]; } } /* * we only look at the messages coming from well-known sources or 'talkers', * distinguished by the two-chars prefix, the most common being: * GPS (GP) * Glonass (GL) * BeiDou (BD) * Galileo (GA) * 'Any kind/a mix of GNSS systems' (GN) */ if (strncmp(fld[0], "BD", 2) && strncmp(fld[0], "GA", 2) && strncmp(fld[0], "GL", 2) && strncmp(fld[0], "GN", 2) && strncmp(fld[0], "GP", 2)) return; /* we look for the RMC & GGA messages */ if (strncmp(fld[0] + 2, "RMC", 3) && strncmp(fld[0] + 2, "GGA", 3)) return; /* if we have a checksum, verify it */ if (cs != NULL) { msgcksum = 0; while (*cs) { if ((*cs >= '0' && *cs <= '9') || (*cs >= 'A' && *cs <= 'F')) { if (msgcksum) msgcksum <<= 4; if (*cs >= '0' && *cs<= '9') msgcksum += *cs - '0'; else if (*cs >= 'A' && *cs <= 'F') msgcksum += 10 + *cs - 'A'; cs++; } else { DPRINTF(("bad char %c in checksum\n", *cs)); return; } } if (msgcksum != cksum) { DPRINTF(("checksum mismatch\n")); return; } } if (strncmp(fld[0] + 2, "RMC", 3) == 0) nmea_gprmc(np, tp, fld, fldcnt); if (strncmp(fld[0] + 2, "GGA", 3) == 0) nmea_decode_gga(np, tp, fld, fldcnt); } /* Decode the recommended minimum specific GPS/TRANSIT data. */ void nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt) { int64_t date_nano, time_nano, nmea_now; int jumped = 0; if (fldcnt < 12 || fldcnt > 14) { DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt)); return; } if (nmea_time_to_nano(fld[1], &time_nano)) { DPRINTF(("gprmc: illegal time, %s\n", fld[1])); return; } if (nmea_date_to_nano(fld[9], &date_nano)) { DPRINTF(("gprmc: illegal date, %s\n", fld[9])); return; } nmea_now = date_nano + time_nano; if (nmea_now <= np->last) { DPRINTF(("gprmc: time not monotonically increasing\n")); jumped = 1; } np->last = nmea_now; np->gap = 0LL; #ifdef NMEA_DEBUG if (np->time.status == SENSOR_S_UNKNOWN) { np->time.status = SENSOR_S_OK; timeout_add_sec(&np->nmea_tout, TRUSTTIME); } np->gapno = 0; if (nmeadebug > 0) { linesw[TTYDISC].l_rint('[', tp); linesw[TTYDISC].l_rint('C', tp); linesw[TTYDISC].l_rint(']', tp); } #endif np->time.value = np->ts.tv_sec * 1000000000LL + np->ts.tv_nsec - nmea_now; np->time.tv.tv_sec = np->ts.tv_sec; np->time.tv.tv_usec = np->ts.tv_nsec / 1000L; if (fldcnt < 13) strlcpy(np->time.desc, "GPS", sizeof(np->time.desc)); else if (*fld[12] != np->mode) { np->mode = *fld[12]; switch (np->mode) { case 'S': strlcpy(np->time.desc, "GPS simulated", sizeof(np->time.desc)); break; case 'E': strlcpy(np->time.desc, "GPS estimated", sizeof(np->time.desc)); break; case 'A': strlcpy(np->time.desc, "GPS autonomous", sizeof(np->time.desc)); break; case 'D': strlcpy(np->time.desc, "GPS differential", sizeof(np->time.desc)); break; case 'N': strlcpy(np->time.desc, "GPS invalid", sizeof(np->time.desc)); break; default: strlcpy(np->time.desc, "GPS unknown", sizeof(np->time.desc)); DPRINTF(("gprmc: unknown mode '%c'\n", np->mode)); } } switch (*fld[2]) { case 'A': /* The GPS has a fix, (re)arm the timeout. */ /* XXX is 'D' also a valid state? */ np->time.status = SENSOR_S_OK; np->signal.value = 1; np->signal.status = SENSOR_S_OK; np->latitude.status = SENSOR_S_OK; np->longitude.status = SENSOR_S_OK; np->speed.status = SENSOR_S_OK; np->time.flags &= ~SENSOR_FINVALID; np->latitude.flags &= ~SENSOR_FINVALID; np->longitude.flags &= ~SENSOR_FINVALID; np->speed.flags &= ~SENSOR_FINVALID; break; case 'V': /* * The GPS indicates a warning status, do not add to * the timeout, if the condition persist, the sensor * will be degraded. Signal the condition through * the signal sensor. */ np->signal.value = 0; np->signal.status = SENSOR_S_CRIT; np->latitude.status = SENSOR_S_WARN; np->longitude.status = SENSOR_S_WARN; np->speed.status = SENSOR_S_WARN; break; } if (nmea_degrees(&np->latitude.value, fld[3], *fld[4] == 'S' ? 1 : 0)) np->latitude.status = SENSOR_S_WARN; if (nmea_degrees(&np->longitude.value,fld[5], *fld[6] == 'W' ? 1 : 0)) np->longitude.status = SENSOR_S_WARN; if (nmea_atoi(&np->speed.value, fld[7])) np->speed.status = SENSOR_S_WARN; /* convert from knot to um/s */ np->speed.value *= KNOTTOMS; if (jumped) np->time.status = SENSOR_S_WARN; if (np->time.status == SENSOR_S_OK) timeout_add_sec(&np->nmea_tout, TRUSTTIME); /* * If tty timestamping is requested, but no PPS signal is present, set * the sensor state to CRITICAL. */ if (np->no_pps) np->time.status = SENSOR_S_CRIT; } /* Decode the GPS fix data for altitude. * - field 9 is the altitude in meters * $GNGGA,085901.00,1234.5678,N,00987.12345,E,1,12,0.84,1040.9,M,47.4,M,,*4B */ void nmea_decode_gga(struct nmea *np, struct tty *tp, char *fld[], int fldcnt) { if (fldcnt != 15) { DPRINTF(("GGA: field count mismatch, %d\n", fldcnt)); return; } #ifdef NMEA_DEBUG if (nmeadebug > 0) { linesw[TTYDISC].l_rint('[', tp); linesw[TTYDISC].l_rint('C', tp); linesw[TTYDISC].l_rint(']', tp); } #endif np->altitude.status = SENSOR_S_OK; if (nmea_atoi(&np->altitude.value, fld[9])) np->altitude.status = SENSOR_S_WARN; /* convert to uMeter */ np->altitude.value *= 1000; np->altitude.flags &= ~SENSOR_FINVALID; } /* * Convert nmea integer/decimal values in the form of XXXX.Y to an integer value * if it's a meter/altitude value, will be returned as mm */ int nmea_atoi(int64_t *dst, char *src) { char *p; int i = 3; /* take 3 digits */ *dst = 0; for (p = src; *p && *p != '.' && *p >= '0' && *p <= '9' ; ) *dst = *dst * 10 + (*p++ - '0'); /* *p should be '.' at that point */ if (*p != '.') return -1; /* no decimal point, or bogus value ? */ p++; /* read digits after decimal point, stop at first non-digit */ for (; *p && i > 0 && *p >= '0' && *p <= '9' ; i--) *dst = *dst * 10 + (*p++ - '0'); for (; i > 0 ; i--) *dst *= 10; DPRINTFN(2,("%s -> %lld\n", src, *dst)); return 0; } /* * Convert a nmea position in the form DDDMM.MMMM to an * angle sensor value (degrees*1000000) */ int nmea_degrees(int64_t *dst, char *src, int neg) { size_t ppos; int i, n; int64_t deg = 0, min = 0; char *p; while (*src == '0') ++src; /* skip leading zeroes */ for (p = src, ppos = 0; *p; ppos++) if (*p++ == '.') break; if (*p == '\0') return (-1); /* no decimal point */ for (n = 0; *src && n + 2 < ppos; n++) deg = deg * 10 + (*src++ - '0'); for (; *src && n < ppos; n++) min = min * 10 + (*src++ - '0'); src++; /* skip decimal point */ for (; *src && n < (ppos + 4); n++) min = min * 10 + (*src++ - '0'); for (i=0; i < 6 + ppos - n; i++) min *= 10; deg = deg * 1000000 + (min/60); *dst = neg ? -deg : deg; return (0); } /* * Convert a NMEA 0183 formatted date string to seconds since the epoch. * The string must be of the form DDMMYY. * Return 0 on success, -1 if illegal characters are encountered. */ int nmea_date_to_nano(char *s, int64_t *nano) { struct clock_ymdhms ymd; time_t secs; char *p; int n; /* make sure the input contains only numbers and is six digits long */ for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++) ; if (n != 6 || (*p != '\0')) return (-1); ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0'); ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0'); ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0'); ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0; secs = clock_ymdhms_to_secs(&ymd); *nano = secs * 1000000000LL; return (0); } /* * Convert NMEA 0183 formatted time string to nanoseconds since midnight. * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615). * Return 0 on success, -1 if illegal characters are encountered. */ int nmea_time_to_nano(char *s, int64_t *nano) { long fac = 36000L, div = 6L, secs = 0L, frac = 0L; char ul = '2'; int n; for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) { secs += (*s - '0') * fac; div = 16 - div; fac /= div; switch (n) { case 0: if (*s <= '1') ul = '9'; else ul = '3'; break; case 1: case 3: ul = '5'; break; case 2: case 4: ul = '9'; break; } } if (fac) return (-1); /* Handle the fractions of a second, up to a maximum of 6 digits. */ div = 1L; if (*s == '.') { for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) { frac *= 10; frac += (*s - '0'); div *= 10; } } if (*s != '\0') return (-1); *nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div); return (0); } /* * Degrade the sensor state if we received no NMEA sentences for more than * TRUSTTIME seconds. */ void nmea_timeout(void *xnp) { struct nmea *np = xnp; np->signal.value = 0; np->signal.status = SENSOR_S_CRIT; if (np->time.status == SENSOR_S_OK) { np->time.status = SENSOR_S_WARN; np->latitude.status = SENSOR_S_WARN; np->longitude.status = SENSOR_S_WARN; np->altitude.status = SENSOR_S_WARN; np->speed.status = SENSOR_S_WARN; /* * further degrade in TRUSTTIME seconds if no new valid NMEA * sentences are received. */ timeout_add_sec(&np->nmea_tout, TRUSTTIME); } else { np->time.status = SENSOR_S_CRIT; np->latitude.status = SENSOR_S_CRIT; np->longitude.status = SENSOR_S_CRIT; np->altitude.status = SENSOR_S_CRIT; np->speed.status = SENSOR_S_CRIT; } }