diff options
-rw-r--r-- | driver_ubx.c | 91 |
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"); |