Hi,
so now that the sensor types got adjusted, here's the tty_nmea.c diff
again, adding 7 new sensors:
- # satellites (raw)
- GPS/DGPS fix (or not) (raw)
- HDOP/VDOP/PDOP (raw)
- Altitude (m)
- Speed (m/s)
i'd welcome eyes on the logic, and especially on the nmea_atoi()
implementation - it is inspired by nmea_degrees() just below but it
feels awkward. I can of course only add altitude/speed as a start..
And testing with other nmea devices would be great ! So far i've tested
this in various static locations and recording data points when driving,
and it matched the gps data from my smartphone.
Landry
Index: tty_nmea.c
===================================================================
RCS file: /cvs/src/sys/kern/tty_nmea.c,v
retrieving revision 1.47
diff -u -r1.47 tty_nmea.c
--- tty_nmea.c 1 Sep 2018 06:09:26 -0000 1.47
+++ tty_nmea.c 28 Dec 2018 20:47:20 -0000
@@ -29,6 +29,10 @@
#ifdef NMEA_DEBUG
#define DPRINTFN(n, x) do { if (nmeadebug > (n)) printf x; } while (0)
int nmeadebug = 0;
+/*
+ * 1 = print interesting messages
+ * 2 = print all messages
+ */
#else
#define DPRINTFN(n, x)
#endif
@@ -52,6 +56,13 @@
struct ksensor signal; /* signal status */
struct ksensor latitude;
struct ksensor longitude;
+ struct ksensor altitude;
+ struct ksensor speed;
+ struct ksensor nsat;
+ struct ksensor quality;
+ struct ksensor hdop;
+ struct ksensor vdop;
+ struct ksensor pdop;
struct ksensordev timedev;
struct timespec ts; /* current timestamp */
struct timespec lts; /* timestamp of last '$' seen */
@@ -70,6 +81,8 @@
/* NMEA decoding */
void nmea_scan(struct nmea *, struct tty *);
void nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt);
+void nmea_decode_gsa(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);
@@ -77,6 +90,7 @@
/* longitude and latitude conversion */
int nmea_degrees(int64_t *dst, char *src, int neg);
+int nmea_atoi(int64_t *dst, char *src, int decimal);
/* degrade the timedelta sensor */
void nmea_timeout(void *);
@@ -126,6 +140,55 @@
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->nsat.type = SENSOR_INTEGER;
+ np->nsat.status = SENSOR_S_UNKNOWN;
+ np->nsat.flags = SENSOR_FINVALID;
+ np->nsat.value = 0;
+ strlcpy(np->nsat.desc, "Nb satellites", sizeof(np->nsat.desc));
+ sensor_attach(&np->timedev, &np->nsat);
+
+ np->quality.type = SENSOR_INTEGER;
+ np->quality.status = SENSOR_S_UNKNOWN;
+ np->quality.flags = SENSOR_FINVALID;
+ np->quality.value = 0;
+ strlcpy(np->quality.desc, "Fix Quality", sizeof(np->quality.desc));
+ sensor_attach(&np->timedev, &np->quality);
+
+ np->hdop.type = SENSOR_INTEGER;
+ np->hdop.status = SENSOR_S_UNKNOWN;
+ np->hdop.flags = SENSOR_FINVALID;
+ np->hdop.value = 0;
+ strlcpy(np->hdop.desc, "HDOP", sizeof(np->hdop.desc));
+ sensor_attach(&np->timedev, &np->hdop);
+
+ np->vdop.type = SENSOR_INTEGER;
+ np->vdop.status = SENSOR_S_UNKNOWN;
+ np->vdop.flags = SENSOR_FINVALID;
+ np->vdop.value = 0;
+ strlcpy(np->vdop.desc, "VDOP", sizeof(np->vdop.desc));
+ sensor_attach(&np->timedev, &np->vdop);
+
+ np->pdop.type = SENSOR_INTEGER;
+ np->pdop.status = SENSOR_S_UNKNOWN;
+ np->pdop.flags = SENSOR_FINVALID;
+ np->pdop.value = 0;
+ strlcpy(np->pdop.desc, "PDOP", sizeof(np->pdop.desc));
+ sensor_attach(&np->timedev, &np->pdop);
+
np->sync = 1;
tp->t_sc = (caddr_t)np;
@@ -182,7 +245,7 @@
np->ts.tv_nsec = ts.tv_nsec;
#ifdef NMEA_DEBUG
- if (nmeadebug > 0) {
+ if (nmeadebug > 1) {
linesw[TTYDISC].l_rint('[', tp);
linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
linesw[TTYDISC].l_rint(']', tp);
@@ -261,7 +324,7 @@
}
/*
- * we only look at the RMC message, which can come from different
'talkers',
+ * 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)
@@ -269,11 +332,17 @@
* Galileo (GA)
* 'Any kind/a mix of GNSS systems' (GN)
*/
- if (strcmp(fld[0], "BDRMC") &&
- strcmp(fld[0], "GARMC") &&
- strcmp(fld[0], "GLRMC") &&
- strcmp(fld[0], "GNRMC") &&
- strcmp(fld[0], "GPRMC"))
+ 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, GSA & GGA messages */
+ if (strncmp(fld[0] + 2, "RMC", 3) &&
+ strncmp(fld[0] + 2, "GSA", 3) &&
+ strncmp(fld[0] + 2, "GGA", 3))
return;
/* if we have a checksum, verify it */
@@ -299,7 +368,12 @@
return;
}
}
- nmea_gprmc(np, tp, fld, fldcnt);
+ if (strncmp(fld[0] + 2, "RMC", 3) == 0)
+ nmea_gprmc(np, tp, fld, fldcnt);
+ if (strncmp(fld[0] + 2, "GSA", 3) == 0)
+ nmea_decode_gsa(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. */
@@ -385,9 +459,11 @@
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
@@ -399,6 +475,7 @@
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))
@@ -406,6 +483,11 @@
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], 1))
+ np->speed.status = SENSOR_S_WARN;
+ /* convert from knot to um/s */
+ np->speed.value *= (1000 / 1.9438);
+
if (jumped)
np->time.status = SENSOR_S_WARN;
if (np->time.status == SENSOR_S_OK)
@@ -418,6 +500,120 @@
np->time.status = SENSOR_S_CRIT;
}
+/* Decode the GPS fix data for altitude and fix quality. */
+void
+nmea_decode_gga(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
+{
+ if (fldcnt != 14 && 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], 1))
+ np->altitude.status = SENSOR_S_WARN;
+
+ /* convert to uMeter */
+ np->altitude.value *= 1000;
+ np->altitude.flags &= ~SENSOR_FINVALID;
+
+ np->nsat.status = SENSOR_S_OK;
+ if (nmea_atoi(&np->nsat.value, fld[7], 0))
+ np->nsat.status = SENSOR_S_WARN;
+ np->nsat.flags &= ~SENSOR_FINVALID;
+
+ np->quality.status = SENSOR_S_OK;
+ np->quality.flags &= ~SENSOR_FINVALID;
+ np->quality.value = *fld[6] - '0';
+
+ switch (np->quality.value) {
+ case 2:
+ strlcpy(np->quality.desc, "DGPS fix",
+ sizeof(np->quality.desc));
+ break;
+ case 1:
+ strlcpy(np->quality.desc, "GPS fix",
+ sizeof(np->quality.desc));
+ break;
+ case 0:
+ default :
+ np->quality.status = SENSOR_S_CRIT;
+ strlcpy(np->quality.desc, "Invalid",
+ sizeof(np->quality.desc));
+ break;
+ }
+}
+
+/* Decode the GPS DOP data. */
+void
+nmea_decode_gsa(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
+{
+ if (fldcnt != 17 && fldcnt != 18) {
+ DPRINTF(("GSA: 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->hdop.status = SENSOR_S_OK;
+ if (nmea_atoi(&np->hdop.value, fld[16], 1) || np->hdop.value > 2000)
+ np->hdop.status = SENSOR_S_WARN;
+ np->hdop.flags &= ~SENSOR_FINVALID;
+
+ np->vdop.status = SENSOR_S_OK;
+ if (nmea_atoi(&np->vdop.value, fld[17], 1) || np->vdop.value > 2000)
+ np->vdop.status = SENSOR_S_WARN;
+ np->vdop.flags &= ~SENSOR_FINVALID;
+
+ np->pdop.status = SENSOR_S_OK;
+ if (nmea_atoi(&np->pdop.value, fld[15], 1) || np->pdop.value > 2000)
+ np->pdop.status = SENSOR_S_WARN;
+ np->pdop.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, int decimal)
+{
+ char *p;
+ int i = 3; /* take 3 digits */
+ *dst = 0;
+
+ for (p = src; *p && *p != '.' && *p >= '0' && *p <= '9' ; )
+ *dst = *dst * 10 + (*p++ - '0');
+
+ if (! decimal)
+ return (0);
+ /* *p should be '.' at that point */
+ if (*p != '.')
+ return (-1); /* no decimal point, or bogus value ? */
+ p++;
+
+ /* read digits after decimal point */
+ 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)
@@ -557,6 +753,13 @@
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;
+ np->nsat.status = SENSOR_S_WARN;
+ np->hdop.status = SENSOR_S_WARN;
+ np->vdop.status = SENSOR_S_WARN;
+ np->pdop.status = SENSOR_S_WARN;
+ np->quality.status = SENSOR_S_WARN;
/*
* further degrade in TRUSTTIME seconds if no new valid NMEA
* sentences are received.
@@ -566,5 +769,12 @@
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;
+ np->nsat.status = SENSOR_S_CRIT;
+ np->hdop.status = SENSOR_S_CRIT;
+ np->vdop.status = SENSOR_S_CRIT;
+ np->pdop.status = SENSOR_S_CRIT;
+ np->quality.status = SENSOR_S_CRIT;
}
}
