Christmas Goodies: a slightly functional UBX parser.

O messages are now generated, I think I'm pulling out the leap seconds
properly, and satellite parsing is well under way. Hacked on somewhere
between Cancun and Edmonton.

GPSD,O=- 1612911601.000 0.005 20.628509 -87.067571 14.89 ? ? 0.0000 0.000 0.000 ? ? ? 3

1 files changed, 135 insertions, 19 deletions

diff --git a/ubx.c b/ubx.c
index 92392795..c09df320 100644
--- a/ubx.c
+++ b/ubx.c
@@ -22,39 +22,152 @@
 #include "bits.h"
 
 gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len);
-gps_mask_t ubx_package_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len);
 
+static unsigned short gps_week = 0;
 /**
- * Navigation solution package
+ * Navigation solution message
  */
 gps_mask_t
-ubx_package_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len)
+ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len)
 {
-    unsigned short gps_week;
+    unsigned short gw;
     unsigned int tow;
+    double epx, epy, epz, evx, evy, evz;
+    unsigned char navmode, flags;
+    gps_mask_t mask = 0;
+    double t;
 
-    if (data_len < 52)
+    if (data_len != 52)
 	return 0;
 
-    gps_week = getsw(buf, 8);
-    tow = getul(buf, 4);
-    session->gpsdata.sentence_time = gpstime_to_unix(gps_week, tow) 
-			           - session->context->leap_seconds;
+    tow = getul(buf, 0);
+    gw = getsw(buf, 8);
+    if (gw > gps_week)
+	gps_week = gw;
+
+    t = gpstime_to_unix(gps_week, tow) - session->context->leap_seconds;
+    if (t > session->gpsdata.online){
+	session->gpsdata.online = t;
+	session->gpsdata.sentence_time = t;
+	session->gpsdata.fix.time = t;
+    }
+    mask |= TIME_SET | ONLINE_SET;
+
+    epx = (double)(getsl(buf, 12)/100.0);
+    epy = (double)(getsl(buf, 16)/100.0);
+    epz = (double)(getsl(buf, 20)/100.0);
+    evx = (double)(getsl(buf, 28)/100.0);
+    evy = (double)(getsl(buf, 32)/100.0);
+    evz = (double)(getsl(buf, 36)/100.0);
+    ecef_to_wgs84fix(&session->gpsdata, epx, epy, epz, evx, evy, evz);
+    mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET  ;
+    session->gpsdata.fix.eph = (double)(getsl(buf, 24)/100.0);
+    session->gpsdata.fix.eps = (double)(getsl(buf, 40)/100.0);
+    session->gpsdata.pdop = (double)(getuw(buf, 44)/100.0);
+    session->gpsdata.satellites_used = getub(buf, 47);
 #ifdef NTPSHM_ENABLE
     if (session->context->enable_ntpshm)
 	(void)ntpshm_put(session, session->gpsdata.sentence_time); /* TODO overhead */
 #endif
 
-    if (buf[10] == UBX_MODE_3D)
+    navmode = getub(buf, 10);
+    flags = getub(buf, 11);
+    switch (navmode){
+    case UBX_MODE_3D:
 	session->gpsdata.fix.mode = MODE_3D;
-    if (buf[10] == UBX_MODE_2D || 
-	buf[10] == UBX_MODE_DR ||	    /* consider this too as 2D */
-	buf[10] == UBX_MODE_GPSDR)	    /* conservative */
+	break;
+    case UBX_MODE_2D:
+    case UBX_MODE_DR:	    /* consider this too as 2D */
+    case UBX_MODE_GPSDR:    /* XXX DR-aided GPS may be valid 3D */
 	session->gpsdata.fix.mode = MODE_2D;
-    else
+	break;
+    default:
 	session->gpsdata.fix.mode = MODE_NO_FIX;
+    }
+
+    if (flags & UBX_SOL_FLAG_DGPS)
+	session->gpsdata.status = STATUS_DGPS_FIX;
+    else if (session->gpsdata.fix.mode != MODE_NO_FIX)
+	session->gpsdata.status = STATUS_FIX;
+
+    mask |= MODE_SET | STATUS_SET | CYCLE_START_SET | USED_SET ;
+
+    return mask;
+}
+
+/**
+ * GPS Leap Seconds
+ */
+gps_mask_t
+ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len)
+{
+    unsigned int tow;
+    unsigned short gw;
+    unsigned char flags;
+    double t;
+
+    if (data_len != 16)
+	return 0;
 
-    return 0;
+    tow = getul(buf, 0);
+    gw = getsw(buf, 8);
+    if (gw > gps_week)
+	gps_week = gw;
+
+    flags = getub(buf, 11);
+    if (flags & 0x7)
+    	session->context->leap_seconds = getub(buf, 10);
+
+    t = gpstime_to_unix(gps_week, tow) - session->context->leap_seconds;
+    if (t > session->gpsdata.online){
+	session->gpsdata.online = t;
+	session->gpsdata.sentence_time = t;
+	session->gpsdata.fix.time = t;
+    }
+
+    return TIME_SET | ONLINE_SET;
+}
+
+/**
+ * GPS Satellite Info
+ */
+gps_mask_t
+ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len)
+{
+    unsigned char i, st, nchan, nsv;
+    unsigned int tow;
+
+    if (data_len < 152 ) {
+	gpsd_report(LOG_PROG, "runt svinfo (datalen=%d)\n", data_len);
+	return 0;
+    }
+    tow = getul(buf, 0);
+//    session->gpsdata.sentence_time = gpstime_to_unix(gps_week, tow) 
+//				- session->context->leap_seconds;
+    gpsd_zero_satellites(&session->gpsdata);
+    nchan = getub(buf, 4);
+    st = nsv = 0;
+    for (i = 0; i < nchan; i++) {
+	int off = 8 + 12 * i;
+	bool good;
+	session->gpsdata.PRN[st]	= (int)getub(buf, off+1);
+	if (getub(buf, off+2) & 0x01)
+	    session->gpsdata.used[st] = session->gpsdata.PRN[st];
+
+	session->gpsdata.ss[st]		= (int)getub(buf, off+4);
+	session->gpsdata.elevation[st]	= (int)getsb(buf, off+5);
+	session->gpsdata.azimuth[st]	= (int)getsw(buf, off+6);
+	good = session->gpsdata.PRN[st]!=0 && 
+	    session->gpsdata.azimuth[st]!=0 && 
+	    session->gpsdata.elevation[st]!=0;
+	if (good!=0)
+	    st++;
+    }
+    session->gpsdata.satellites = st;
+    return SATELLITE_SET;
 }
 
 /*@ +charint @*/
@@ -62,12 +175,13 @@ gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t le
 {
     unsigned short data_len;
     unsigned short msgid;
+    gps_mask_t mask = 0;
 
     if (len < 6)    /* the packet at least contains a head of six bytes */
 	return 0;
 
     /* we may need to dump the raw packet */
-    gpsd_report(LOG_PROG, "UBX packet type %02hhx:%02hhx length %d: %s\n", 
+    gpsd_report(LOG_IO, "UBX packet type %02hhx:%02hhx length %d: %s\n", 
 		buf[2], buf[3], len, gpsd_hexdump(buf, len));
 
     /* extract message id and length */
@@ -89,7 +203,7 @@ gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t le
 	    break;
 	case UBX_NAV_SOL:
 	    gpsd_report(LOG_PROG, "UBX_NAV_SOL\n");
-            ubx_package_nav_sol(session, &buf[6], data_len);
+            mask = ubx_msg_nav_sol(session, &buf[6], data_len);
 	    break;
 	case UBX_NAV_POSUTM:
 	    gpsd_report(LOG_PROG, "UBX_NAV_POSUTM\n");
@@ -102,6 +216,7 @@ gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t le
 	    break;
 	case UBX_NAV_TIMEGPS:
 	    gpsd_report(LOG_PROG, "UBX_NAV_TIMEGPS\n");
+            mask = ubx_msg_nav_timegps(session, &buf[6], data_len);
 	    break;
 	case UBX_NAV_TIMEUTC:
 	    gpsd_report(LOG_PROG, "UBX_NAV_TIMEUTC\n");
@@ -111,6 +226,7 @@ gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t le
 	    break;
 	case UBX_NAV_SVINFO:
 	    gpsd_report(LOG_PROG, "UBX_NAV_SVINFO\n");
+            mask = ubx_msg_nav_svinfo(session, &buf[6], data_len);
 	    break;
 	case UBX_NAV_DGPS:
 	    gpsd_report(LOG_PROG, "UBX_NAV_DGPS\n");
@@ -182,7 +298,7 @@ gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t le
 	gpsd_report(LOG_WARN, "UBX: unknown packet id 0x%04hx (length: %d)\n", 
 		    msgid, len);
     }
-    return 0;
+    return mask;
 }
 /*@ -charint @*/
 
@@ -225,7 +341,7 @@ struct gps_type_t ubx_binary = {
     /* Response string that identifies device (not active) */
     .trigger          = NULL,
     /* Number of satellite channels supported by the device */
-    .channels         = 12,
+    .channels         = 16,
     /* Startup-time device detector */
     .probe_detect     = /*probe_detect*/ NULL,
     /* Wakeup to be done before each baud hunt */