From 9f2969c6db22f261ce72f142e2d470585d1e7b60 Mon Sep 17 00:00:00 2001 From: "Gary E. Miller" Date: Tue, 23 Apr 2019 13:08:22 -0700 Subject: driver_ubx: Cleanup HPPPOSxx and POSxx decodes. --- driver_ubx.c | 87 +++++++++++++++++++++++++++++++++++++++--------------------- 1 file changed, 57 insertions(+), 30 deletions(-) (limited to 'driver_ubx.c') diff --git a/driver_ubx.c b/driver_ubx.c index 2d17ccad..1956e636 100644 --- a/driver_ubx.c +++ b/driver_ubx.c @@ -166,18 +166,18 @@ ubx_msg_nav_hpposecef(struct gps_device_t *session, unsigned char *buf, version = getub(buf, 0); session->driver.ubx.iTOW = getleu32(buf, 4); - session->newdata.ecef.x = getles32(buf, 8) / 100.0; - session->newdata.ecef.y = getles32(buf, 12) / 100.0; - session->newdata.ecef.z = getles32(buf, 16) / 100.0; - /* now the correction factors */ - session->newdata.ecef.x += getsb(buf, 20) / 10000.0; - session->newdata.ecef.y += getsb(buf, 21) / 10000.0; - session->newdata.ecef.z += getsb(buf, 22) / 10000.0; - session->newdata.ecef.pAcc = getleu32(buf, 24) / 10000.0; + session->newdata.ecef.x = ((getles32(buf, 8) + + (getsb(buf, 20) * 1e-2)) * 1e-2); + session->newdata.ecef.y = ((getles32(buf, 12) + + (getsb(buf, 21) * 1e-2)) * 1e-2); + session->newdata.ecef.z = ((getles32(buf, 16) + + (getsb(buf, 22) * 1e-2)) * 1e-2); + + session->newdata.ecef.pAcc = getleu32(buf, 24) * 1e-4; /* (long long) cast for 32-bit compat */ gpsd_log(&session->context->errout, LOG_DATA, - "UBX-NAV-HPPOSECEF: version %d iTOW=%lld ECEF x=%.3f y=%.3f z=%.3f " - "pAcc=%.3f\n", + "UBX-NAV-HPPOSECEF: version %d iTOW=%lld ECEF x=%.4f y=%.4f z=%.4f " + "pAcc=%.4f\n", version, (long long)session->driver.ubx.iTOW, session->newdata.ecef.x, @@ -198,24 +198,37 @@ ubx_msg_nav_hpposllh(struct gps_device_t *session, unsigned char *buf, size_t data_len UNUSED) { int version; + gps_mask_t mask = 0; if (36 > data_len) { gpsd_log(&session->context->errout, LOG_WARN, "Runt NAV-HPPOSLLH message, payload len %zd", data_len); - return 0; + return mask; } - gps_mask_t mask = ONLINE_SET | HERR_SET | VERR_SET; + mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET; version = getub(buf, 0); session->driver.ubx.iTOW = getles32(buf, 4); - /* FIXME: should also get time, lat/lon/alt */ - /* Horizontal accuracy estimate in mm, unknown est type */ - session->newdata.eph = (double)(getleu32(buf, 28) / 10000.0); - /* Vertical accuracy estimate in mm, unknown est type */ - session->newdata.epv = (double)(getleu32(buf, 32) / 10000.0); + session->newdata.longitude = (1e-7 * (getles32(buf, 8) + + (getsb(buf, 24) * 1e-2))); + session->newdata.latitude = (1e-7 * (getles32(buf, 12) + \ + (getsb(buf, 25) * 1e-2))); + session->newdata.altitude = (1e-3 * (getles32(buf, 20) + \ + (getsb(buf, 27) * 1e-2))); + + /* Horizontal accuracy estimate in .1 mm, unknown est type */ + session->newdata.eph = getleu32(buf, 28) * 1e-4; + /* Vertical accuracy estimate in .1 mm, unknown est type */ + session->newdata.epv = getleu32(buf, 32) * 1e-4; + gpsd_log(&session->context->errout, LOG_DATA, - "UBX-NAV-HPPOSECEF: version %d\n", version); + "UBX-NAV-HPPOSLLH: version %d iTOW=%lld lat=%.4f lon=%.4f alt=%.4f\n", + version, + (long long)session->driver.ubx.iTOW, + session->newdata.latitude, + session->newdata.longitude, + session->newdata.altitude); return mask; } @@ -224,7 +237,7 @@ ubx_msg_nav_hpposllh(struct gps_device_t *session, unsigned char *buf, */ static gps_mask_t ubx_msg_nav_posecef(struct gps_device_t *session, unsigned char *buf, - size_t data_len) + size_t data_len) { gps_mask_t mask = ECEF_SET; @@ -235,10 +248,12 @@ ubx_msg_nav_posecef(struct gps_device_t *session, unsigned char *buf, } session->driver.ubx.iTOW = getleu32(buf, 0); - 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; + /* all in cm */ + session->newdata.ecef.x = getles32(buf, 4) * 1e-2; + session->newdata.ecef.y = getles32(buf, 8) * 1e-2; + session->newdata.ecef.z = getles32(buf, 12) * 1e-2; + session->newdata.ecef.pAcc = getleu32(buf, 16) * 1e-2; + /* (long long) cast for 32-bit compat */ gpsd_log(&session->context->errout, LOG_DATA, "UBX-NAV-POSECEF: iTOW=%lld ECEF x=%.2f y=%.2f z=%.2f pAcc=%.2f\n", @@ -376,8 +391,9 @@ ubx_msg_nav_pvt(struct gps_device_t *session, unsigned char *buf, /* let gpsd_error_model() do the rest */ mask |= HERR_SET | SPEEDERR_SET | VERR_SET; + gpsd_log(&session->context->errout, LOG_DATA, - "NAV_PVT: flags=%02x time=%.2f lat=%.2f lon=%.2f alt=%.2f\n" + "NAV-PVT: flags=%02x time=%.2f lat=%.2f lon=%.2f alt=%.2f\n" " track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n", flags, session->newdata.time, @@ -501,6 +517,7 @@ ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, session->gpsdata.status = STATUS_DGPS_FIX; mask |= MODE_SET | STATUS_SET; + gpsd_log(&session->context->errout, LOG_DATA, "UBX-NAV-SOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f\n" " speed=%.2f climb=%.2f mode=%d status=%d used=%d\n", @@ -650,13 +667,23 @@ ubx_msg_nav_posllh(struct gps_device_t *session, unsigned char *buf, mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET; session->driver.ubx.iTOW = getles32(buf, 0); - session->newdata.longitude = 1e-7 * (int32_t)getles32(buf, 4); - session->newdata.latitude = 1e-7 * (int32_t)getles32(buf, 8); - session->newdata.altitude = 1e-3 * (int32_t)getles32(buf, 16); + session->newdata.longitude = 1e-7 * getles32(buf, 4); + session->newdata.latitude = 1e-7 * getles32(buf, 8); + session->newdata.altitude = 1e-3 * getles32(buf, 16); /* Horizontal accuracy estimate in mm, unknown type */ - session->newdata.eph = (double)(getleu32(buf, 20) / 1000.0); + session->newdata.eph = getleu32(buf, 20) * 1e-3; /* Vertical accuracy estimate in mm, unknown type */ - session->newdata.epv = (double)(getleu32(buf, 24) / 1000.0); + session->newdata.epv = getleu32(buf, 24) * 1e-3; + + gpsd_log(&session->context->errout, LOG_DATA, + "UBX-NAV-POSLLH: iTOW=%lld lat=%.3f lon=%.3f alt=%.3f " + "eph %.3f epv %.3f\n", + (long long)session->driver.ubx.iTOW, + session->newdata.latitude, + session->newdata.longitude, + session->newdata.altitude, + session->newdata.eph, + session->newdata.epv); return mask; } @@ -2147,7 +2174,7 @@ const struct gps_type_t driver_ubx = { .flags = DRIVER_STICKY, /* remember this */ .trigger = NULL, /* Number of satellite channels supported by the device */ - .channels = 50, + .channels = 50, .probe_detect = NULL, /* Startup-time device detector */ /* Packet getter (using default routine) */ .get_packet = generic_get, -- cgit v1.2.1