Add ECEF support to u-blocx driver, data comes out in json.

More work to do so cgps, xgps, etc. see the new data.

5 files changed, 160 insertions, 18 deletions

diff --git a/driver_ubx.c b/driver_ubx.c
index 05edc594..252708d7 100644
--- a/driver_ubx.c
+++ b/driver_ubx.c
@@ -62,6 +62,8 @@ static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
 static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
 				  unsigned char *buf, size_t data_len);
 static void ubx_msg_inf(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+static gps_mask_t ubx_msg_nav_posecef(struct gps_device_t *session,
+				      unsigned char *buf, size_t data_len);
 static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t *session,
 				  unsigned char *buf, size_t data_len);
 static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf);
@@ -71,6 +73,8 @@ static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session,
 				     unsigned char *buf, size_t data_len);
 static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session,
 				      unsigned char *buf, size_t data_len);
+static gps_mask_t ubx_msg_nav_velecef(struct gps_device_t *session,
+				      unsigned char *buf, size_t data_len);
 static void ubx_msg_mon_ver(struct gps_device_t *session,
 				      unsigned char *buf, size_t data_len);
 #ifdef RECONFIGURE_ENABLE
@@ -123,6 +127,38 @@ ubx_msg_mon_ver(struct gps_device_t *session, unsigned char *buf,
              (int)sizeof(obuf), obuf);
 }
 
+/*
+ * Navigation Position ECEF message
+ */
+static gps_mask_t
+ubx_msg_nav_posecef(struct gps_device_t *session, unsigned char *buf,
+		size_t data_len)
+{
+    gps_mask_t mask = ECEF_SET;
+    double fTOW;
+
+    if (data_len < 20) {
+	gpsd_log(&session->context->errout, LOG_WARN,
+		 "Invalid NAV POSECEF message, payload len %ld", data_len);
+	return 0;
+    }
+
+    fTOW = getleu32(buf, 0) / 1000.0;
+    session->newdata.ecef.valid = true;
+    session->newdata.ecef.x = getles32(buf, 4) / 100.0;
+    session->newdata.ecef.y = getles32(buf, 8) / 100.0;
+    session->newdata.ecef.z = getles32(buf, 12) / 100.0;
+    session->newdata.ecef.pAcc = getleu32(buf, 16) / 100.0;
+    gpsd_log(&session->context->errout, LOG_DATA,
+	"UBX_NAV_POSECEF: fTOW=%.3f ECEF x=%.2f y=%.2f z=%.2f pAcc=%.2f\n",
+	fTOW,
+	session->newdata.ecef.x,
+	session->newdata.ecef.y,
+	session->newdata.ecef.z,
+	session->newdata.ecef.pAcc);
+    return mask;
+}
+
 /**
  * Navigation Position Velocity Time  solution message
  */
@@ -179,8 +215,7 @@ ubx_msg_nav_pvt(struct gps_device_t *session, unsigned char *buf,
 		mask |= STATUS_SET;
 	    }
 	}
-	mask |=	  LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLEAR_IS
-		| REPORT_IS;
+	mask |=	  LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET;
 	break;
     }
     case UBX_MODE_2D:
@@ -338,7 +373,7 @@ ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf,
 
     mask |= MODE_SET | STATUS_SET;
     gpsd_log(&session->context->errout, LOG_DATA,
-	     "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
+	     "UBX_NAV_SOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
 	     session->newdata.time,
 	     session->newdata.latitude,
 	     session->newdata.longitude,
@@ -490,6 +525,38 @@ ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf,
 }
 
 /*
+ * Velocity Position ECEF message
+ */
+static gps_mask_t
+ubx_msg_nav_velecef(struct gps_device_t *session, unsigned char *buf,
+		size_t data_len)
+{
+    gps_mask_t mask = VECEF_SET;
+    double fTOW;
+
+    if (data_len < 20) {
+	gpsd_log(&session->context->errout, LOG_WARN,
+		 "Invalid NAV VELECEF message, payload len %ld", data_len);
+	return 0;
+    }
+
+    fTOW = getleu32(buf, 0) / 1000.0;
+    session->newdata.ecef.valid = true;
+    session->newdata.ecef.vx = getles32(buf, 4) / 100.0;
+    session->newdata.ecef.vy = getles32(buf, 8) / 100.0;
+    session->newdata.ecef.vz = getles32(buf, 12) / 100.0;
+    session->newdata.ecef.vAcc = getleu32(buf, 16) / 100.0;
+    gpsd_log(&session->context->errout, LOG_DATA,
+	"UBX_NAV_VELECEF: fTOW=%.3f ECEF vx=%.2f vy=%.2f vz=%.2f vAcc=%.2f\n",
+	fTOW,
+	session->newdata.ecef.vx,
+	session->newdata.ecef.vy,
+	session->newdata.ecef.vz,
+	session->newdata.ecef.vAcc);
+    return mask;
+}
+
+/*
  * SBAS Info
  */
 static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf)
@@ -584,9 +651,11 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
     /* extract message id and length */
     msgid = (buf[2] << 8) | buf[3];
     data_len = (size_t) getles16(buf, 4);
+
     switch (msgid) {
     case UBX_NAV_POSECEF:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_POSECEF\n");
+	mask = ubx_msg_nav_posecef(session, &buf[UBX_PREFIX_LEN], data_len);
 	break;
     case UBX_NAV_POSLLH:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_POSLLH\n");
@@ -596,6 +665,7 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_STATUS\n");
 	break;
     case UBX_NAV_DOP:
+        /* DOP seems to be the last NAV sent in a cycle */
 	gpsd_log(&session->context->errout, LOG_PROG, "UBX_NAV_DOP\n");
 	mask = ubx_msg_nav_dop(session, &buf[UBX_PREFIX_LEN], data_len);
 	break;
@@ -603,19 +673,19 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
         /* UBX-NAV-SOL deprecated, use UBX-NAV-PVT instead */
 	gpsd_log(&session->context->errout, LOG_PROG, "UBX_NAV_SOL\n");
 	mask =
-	    ubx_msg_nav_sol(session, &buf[UBX_PREFIX_LEN],
-			    data_len) | (CLEAR_IS | REPORT_IS);
+	    ubx_msg_nav_sol(session, &buf[UBX_PREFIX_LEN], data_len);
 	break;
     case UBX_NAV_PVT:
 	gpsd_log(&session->context->errout, LOG_PROG, "UBX_NAV_PVT\n");
-	mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len)
-	     | (CLEAR_IS | REPORT_IS);
+	mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
         break;
     case UBX_NAV_POSUTM:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_POSUTM\n");
 	break;
     case UBX_NAV_VELECEF:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_VELECEF\n");
+	mask = ubx_msg_nav_velecef(session, &buf[UBX_PREFIX_LEN], data_len)
+	     | REPORT_IS;
 	break;
     case UBX_NAV_VELNED:
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_VELNED\n");
@@ -828,7 +898,7 @@ static ssize_t ubx_control_send(struct gps_device_t *session, char *msg,
 
 static void ubx_init_query(struct gps_device_t *session)
 {
-    /* MON_VER: query for version information */
+    /* UBX-MON-VER: query for version information */
     (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
 }
 
@@ -849,6 +919,7 @@ static void ubx_event_hook(struct gps_device_t *session, event_t event)
 	msg[5] = 0x00;
 	msg[6] = 0x00;
 	msg[7] = 0x00;
+        /* UBX-CFG-SBAS */
 	(void)ubx_write(session, 0x06u, 0x16, msg, 8);
 
 #ifdef RECONFIGURE_ENABLE
@@ -871,7 +942,7 @@ static void ubx_event_hook(struct gps_device_t *session, event_t event)
 	gpsd_log(&session->context->errout, LOG_DATA, "UBX revert\n");
 
 	/* Reverting all in one fast and reliable reset */
-	(void)ubx_write(session, 0x06, 0x04, msg, 4);	/* CFG-RST */
+	(void)ubx_write(session, 0x06, 0x04, msg, 4);	/* UBX-CFG-RST */
     }
 }
 
@@ -973,6 +1044,9 @@ static void ubx_cfg_prt(struct gps_device_t *session,
     /* enable all input protocols by default */
     buf[12] = NMEA_PROTOCOL_MASK | UBX_PROTOCOL_MASK | RTCM_PROTOCOL_MASK;
 
+    gpsd_log(&session->context->errout, LOG_DATA,
+	"UBX ubx_cfg_prt mode:%d, port:%d\n", mode, buf[0]);
+
     /* selectively enable output protocols */
     if (mode == MODE_NMEA) {
 	/*
@@ -1092,6 +1166,14 @@ static void ubx_cfg_prt(struct gps_device_t *session,
 	msg[1] = 0x32;		/* msg id  = NAV-SBAS */
 	msg[2] = 0x0a;		/* rate */
 	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
+	msg[0] = 0x01;		/* class */
+	msg[1] = 0x01;		/* msg id  = UBX-NAV-POSECEF */
+	msg[2] = 0x01;		/* rate */
+	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
+	msg[0] = 0x01;		/* class */
+	msg[1] = 0x11;		/* msg id  = UBX-NAV-VELECEF */
+	msg[2] = 0x01;		/* rate */
+	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
 
 
 #ifdef __UNUSED__
diff --git a/gps.h b/gps.h
index 91587334..4d3729e9 100644
--- a/gps.h
+++ b/gps.h
@@ -36,7 +36,7 @@ extern "C" {
  *       structure has changed to make working with the satellites-used
  *       bits less confusing. (January 2015, release 3.12).
  * 6.1 - Add navdata_t for more (nmea2000) info.
- * 7.0 - add gps_fix_t.ecef
+ * 7.0 - add gps_fix_t.ecef (February 2018)
  */
 #define GPSD_API_MAJOR_VERSION	7	/* bump on incompatible changes */
 #define GPSD_API_MINOR_VERSION	0	/* bump on compatible changes */
@@ -97,9 +97,14 @@ struct gps_fix_t {
     double eps;		/* Speed uncertainty, meters/sec */
     double climb;       /* Vertical speed, meters/sec */
     double epc;		/* Vertical speed uncertainty */
-    struct {		/* ECEF data */
-	bool valid;		/* is message well-formed? */
-	double x, y, z;
+
+    /* ECEF data, all data in meters, and meters/second, or NaN */
+    struct {
+	bool valid;	        /* is ECEF data valid */
+	double x, y, z; 	/* ECEF x, y, z */
+	double vx, vy, vz;	/* ECEF x, y, z velocity */
+	double pAcc;            /* 3D Position Accuracy Estimate, probably SEP */
+	double vAcc;            /* Velocity Accuracy Estimate, probably SEP */
     } ecef;
 };
 
@@ -2002,7 +2007,9 @@ struct gps_data_t {
 #define PPS_SET 	(1llu<<33)
 #define NAVDATA_SET     (1llu<<34)
 #define OSCILLATOR_SET	(1llu<<35)
-#define SET_HIGH_BIT	36
+#define ECEF_SET	(1llu<<36)
+#define VECEF_SET	(1llu<<37)
+#define SET_HIGH_BIT	38
     timestamp_t online;		/* NZ if GPS is on line, 0 if not.
 				 *
 				 * Note: gpsd clears this time when sentences
diff --git a/gpsd_json.c b/gpsd_json.c
index 825851e3..cfc4412c 100644
--- a/gpsd_json.c
+++ b/gpsd_json.c
@@ -189,8 +189,36 @@ void json_tpv_dump(const struct gps_device_t *session,
 	    str_appendf(reply, replylen, "\"epd\":%.4f,", gpsdata->fix.epd);
 	if (isnan(gpsdata->fix.eps) == 0)
 	    str_appendf(reply, replylen, "\"eps\":%.2f,", gpsdata->fix.eps);
-	if ((gpsdata->fix.mode >= MODE_3D) && isnan(gpsdata->fix.epc) == 0)
-	    str_appendf(reply, replylen, "\"epc\":%.2f,", gpsdata->fix.epc);
+	if (gpsdata->fix.mode >= MODE_3D) {
+            if (isnan(gpsdata->fix.epc) == 0)
+		str_appendf(reply, replylen, "\"epc\":%.2f,", gpsdata->fix.epc);
+	    if ( 1 || gpsdata->fix.ecef.valid) {
+		if (0 == isnan(gpsdata->fix.ecef.x))
+		    str_appendf(reply, replylen, "\"ecefx\":%.2f,",
+                                gpsdata->fix.ecef.x);
+		if (0 == isnan(gpsdata->fix.ecef.y))
+		    str_appendf(reply, replylen, "\"ecefy\":%.2f,",
+                                gpsdata->fix.ecef.y);
+		if (0 == isnan(gpsdata->fix.ecef.z))
+		    str_appendf(reply, replylen, "\"ecefz\":%.2f,",
+                                gpsdata->fix.ecef.z);
+		if (0 == isnan(gpsdata->fix.ecef.vx))
+		    str_appendf(reply, replylen, "\"ecefvx\":%.2f,",
+                                gpsdata->fix.ecef.vx);
+		if (0 == isnan(gpsdata->fix.ecef.vy))
+		    str_appendf(reply, replylen, "\"ecefvy\":%.2f,",
+                                gpsdata->fix.ecef.vy);
+		if (0 == isnan(gpsdata->fix.ecef.vz))
+		    str_appendf(reply, replylen, "\"ecefvz\":%.2f,",
+                                gpsdata->fix.ecef.vz);
+		if (0 == isnan(gpsdata->fix.ecef.pAcc))
+		    str_appendf(reply, replylen, "\"ecefpAcc\":%.2f,",
+                                gpsdata->fix.ecef.pAcc);
+		if (0 == isnan(gpsdata->fix.ecef.vAcc))
+		    str_appendf(reply, replylen, "\"ecefvAcc\":%.2f,",
+                                gpsdata->fix.ecef.vAcc);
+            }
+        }
 #ifdef TIMING_ENABLE
 	if (policy->timing) {
 	    char rtime_str[TIMESPEC_LEN];
diff --git a/gpsutils.c b/gpsutils.c
index 7c6dbd0a..09f0112d 100644
--- a/gpsutils.c
+++ b/gpsutils.c
@@ -237,6 +237,7 @@ done:
 void gps_clear_fix(struct gps_fix_t *fixp)
 /* stuff a fix structure with recognizable out-of-band values */
 {
+    memset(fixp, 0, sizeof(struct gps_fix_t));
     fixp->time = NAN;
     fixp->mode = MODE_NOT_SEEN;
     fixp->latitude = fixp->longitude = NAN;
@@ -251,6 +252,15 @@ void gps_clear_fix(struct gps_fix_t *fixp)
     fixp->epd = NAN;
     fixp->eps = NAN;
     fixp->epc = NAN;
+    /* clear ECEF too */
+    fixp->ecef.x = NAN;
+    fixp->ecef.y = NAN;
+    fixp->ecef.z = NAN;
+    fixp->ecef.vx = NAN;
+    fixp->ecef.vy = NAN;
+    fixp->ecef.vz = NAN;
+    fixp->ecef.pAcc = NAN;
+    fixp->ecef.vAcc = NAN;
 }
 
 void gps_clear_dop( struct dop_t *dop)
@@ -292,6 +302,20 @@ void gps_merge_fix(struct gps_fix_t *to,
 	to->epv = from->epv;
     if ((transfer & SPEEDERR_SET) != 0)
 	to->eps = from->eps;
+    if ((transfer & ECEF_SET) != 0) {
+	to->ecef.valid = true;
+	to->ecef.x = from->ecef.x;
+	to->ecef.y = from->ecef.y;
+	to->ecef.z = from->ecef.z;
+	to->ecef.pAcc = from->ecef.pAcc;
+    }
+    if ((transfer & VECEF_SET) != 0) {
+	to->ecef.valid = true;
+	to->ecef.vx = from->ecef.vx;
+	to->ecef.vy = from->ecef.vy;
+	to->ecef.vz = from->ecef.vz;
+	to->ecef.vAcc = from->ecef.vAcc;
+    }
 }
 
 /* NOTE: timestamp_t is a double, so this is only precise to
diff --git a/libgpsd_core.c b/libgpsd_core.c
index 77992f35..86fa9485 100644
--- a/libgpsd_core.c
+++ b/libgpsd_core.c
@@ -1429,8 +1429,9 @@ gps_mask_t gpsd_poll(struct gps_device_t *session)
 	/* don't downgrade mode if holding previous fix */
 	if (session->gpsdata.fix.mode > session->newdata.mode)
 	    session->gpsdata.set &= ~MODE_SET;
-	//gpsd_log(&session->context->errout, LOG_PROG,
-	//              "transfer mask: %02x\n", session->gpsdata.set);
+	/* gpsd_log(&session->context->errout, LOG_PROG,
+	                 "transfer mask: %s\n",
+                         gps_maskdump(session->gpsdata.set)); */
 	gps_merge_fix(&session->gpsdata.fix,
 		      session->gpsdata.set, &session->newdata);
 #ifndef NOFLOATS_ENABLE