Very preliminary cut at decoding UBX-MON-PVT

1 files changed, 90 insertions, 1 deletions

diff --git a/driver_ubx.c b/driver_ubx.c
index ccc133aa..5922e48c 100644
--- a/driver_ubx.c
+++ b/driver_ubx.c
@@ -61,6 +61,8 @@
 
 static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
 			    size_t len);
+static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t *session,
+				  unsigned char *buf, size_t data_len);
 static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
 				  unsigned char *buf, size_t data_len);
 static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
@@ -76,6 +78,91 @@ static void ubx_mode(struct gps_device_t *session, int mode);
 #endif /* RECONFIGURE_ENABLE */
 
 /**
+ * Navigation Position Velocity Time  solution message
+ */
+static gps_mask_t
+ubx_msg_nav_pvt(struct gps_device_t *session, unsigned char *buf,
+		size_t data_len)
+{
+    unsigned int flags;
+    gps_mask_t mask = 0;
+
+    if (data_len != 92)
+	return 0;
+
+    flags = (unsigned int)getub(buf, 21);
+
+#if 0
+    double epx, epy, epz, evx, evy, evz;
+    unsigned char navmode;
+
+    mask = 0;
+    if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) {
+	unsigned short gw;
+	unsigned int tow;
+	tow = (unsigned int)getleu32(buf, 0);
+	gw = (unsigned short)getles16(buf, 8);
+	session->newdata.time = gpsd_gpstime_resolve(session, gw, tow / 1000.0);
+	mask |= TIME_SET | PPSTIME_IS;
+    }
+
+    epx = (double)(getles32(buf, 12) / 100.0);
+    epy = (double)(getles32(buf, 16) / 100.0);
+    epz = (double)(getles32(buf, 20) / 100.0);
+    evx = (double)(getles32(buf, 28) / 100.0);
+    evy = (double)(getles32(buf, 32) / 100.0);
+    evz = (double)(getles32(buf, 36) / 100.0);
+    ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation,
+		     epx, epy, epz, evx, evy, evz);
+    mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET;
+
+    if (session->driver.ubx.last_herr > 0.0) {
+	session->newdata.epx = session->newdata.epy = session->driver.ubx.last_herr;
+	mask |= HERR_SET;
+	session->driver.ubx.last_herr = 0.0;
+    }
+
+    if (session->driver.ubx.last_verr > 0.0) {
+	session->newdata.epv = session->driver.ubx.last_verr;
+	mask |= VERR_SET;
+	session->driver.ubx.last_verr = 0.0;
+    }
+
+    session->newdata.eps = (double)(getles32(buf, 40) / 100.0);
+    mask |= SPEEDERR_SET;
+
+    /* Better to have a single point of truth about DOPs */
+    //session->gpsdata.dop.pdop = (double)(getleu16(buf, 44)/100.0);
+    session->gpsdata.satellites_used = (int)getub(buf, 47);
+
+    navmode = (unsigned char)getub(buf, 10);
+    switch (navmode) {
+    case UBX_MODE_TMONLY:
+    case UBX_MODE_3D:
+	session->newdata.mode = MODE_3D;
+	break;
+    case UBX_MODE_2D:
+    case UBX_MODE_DR:		/* consider this too as 2D */
+    case UBX_MODE_GPSDR:	/* FIX-ME: DR-aided GPS may be valid 3D */
+	session->newdata.mode = MODE_2D;
+	break;
+    default:
+	session->newdata.mode = MODE_NO_FIX;
+    }
+
+    if ((flags & UBX_SOL_FLAG_DGPS) != 0)
+	session->gpsdata.status = STATUS_DGPS_FIX;
+    else if (session->newdata.mode != MODE_NO_FIX)
+	session->gpsdata.status = STATUS_FIX;
+
+    mask |= MODE_SET | STATUS_SET;
+# endif
+    gpsd_log(&session->context->errout, LOG_DATA,
+	     "NAV-PVT: flags:%02x\n", flags);
+    return mask;
+}
+
+/**
  * Navigation solution message
  */
 static gps_mask_t
@@ -412,7 +499,9 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
 	break;
     case UBX_NAV_PVT:
 	gpsd_log(&session->context->errout, LOG_PROG, "UBX_NAV_PVT\n");
-	/* FIXME decode UBX_NAV_PVT */
+	mask =
+	    ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN],
+			    data_len) | (CLEAR_IS | REPORT_IS);
         break;
     case UBX_NAV_POSUTM:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_POSUTM\n");