On Sat, Nov 03, 2018 at 05:00:47PM +0100, Landry Breuil wrote:
> Hi,
New diff adding speed and # of satellites, uses SENSOR_VELOCITY as
posted in the previous diff, and relies on SENSOR_DISTANCE being shown
as meters. I redid the message type matching to account for
constellation types which was commented out in the previous diff. More
proofreading welcome, especially in the conversions and the handrolled
atoi implem...
That gives:
nmea0.indicator0 On OK Signal
nmea0.raw0 5 raw OK Nb satellites
nmea0.raw1 1 raw OK GPS fix
nmea0.raw2 2030 raw WARNING HDOP
nmea0.raw3 2330 raw WARNING VDOP
nmea0.raw4 3080 raw WARNING PDOP
nmea0.timedelta0 30.311 ms OK GPS autonomous
nmea0.angle0 xx.yyyy degrees OK Latitude
nmea0.angle1 z.wwww degrees OK Longitude
nmea0.distance0 381.00000 m OK Altitude
nmea0.velocity0 0.319 m/s OK Ground speed
I need to do more velocity testing when moving.. as GPS is not reliable
when *not* moving :)
When there's no signal (ie, staying inside without satellite coverage)
nmea0.indicator0 Off CRITICAL Signal
nmea0.raw0 0 raw OK Nb satellites
nmea0.raw1 0 raw CRITICAL Invalid
nmea0.raw2 99990 raw WARNING HDOP
nmea0.raw3 99990 raw WARNING VDOP
nmea0.raw4 99990 raw WARNING PDOP
nmea0.timedelta0 27.195 ms OK GPS invalid
nmea0.angle0 xx.yyyy degrees WARNING Latitude
nmea0.angle1 z.wwww degrees WARNING Longitude
nmea0.distance0 0.00000 m WARNING Altitude
nmea0.velocity0 0.000 m/s WARNING Ground speed
Most statuses should just be CRITICAL, and i think in that case the position
is 'last known'.
Landry
Index: kern/tty_nmea.c
===================================================================
RCS file: /cvs/src/sys/kern/tty_nmea.c,v
retrieving revision 1.47
diff -u -r1.47 tty_nmea.c
--- kern/tty_nmea.c 1 Sep 2018 06:09:26 -0000 1.47
+++ kern/tty_nmea.c 3 Nov 2018 19:08:47 -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;
}
}
