diff options
-rw-r--r-- | driver_ubx.c | 87 | ||||
-rw-r--r-- | test/daemon/ublox-zed-f9p-hp.log.chk | 2 |
2 files changed, 58 insertions, 31 deletions
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, diff --git a/test/daemon/ublox-zed-f9p-hp.log.chk b/test/daemon/ublox-zed-f9p-hp.log.chk index 06afe600..403c1593 100644 --- a/test/daemon/ublox-zed-f9p-hp.log.chk +++ b/test/daemon/ublox-zed-f9p-hp.log.chk @@ -146,7 +146,7 @@ $GPGGA,211941.00,4552.6512,S,17030.0064,E,1,16,0.77,25.13,M,,,*33 $GPRMC,211941.00,A,4552.6512,S,17030.0064,E,0.6201,217.876,220419,,*15
$GPGSA,A,3,2,12,25,29,,,,,,,,,1.2,0.8,1.0*05
$GPGBS,211941.00,8.500,9.149,12.506,,,,*77
-{"class":"TPV","mode":3,"time":"2019-04-22T21:19:41.000Z","leapseconds":18,"ept":0.005,"lat":-45.877520200,"lon":170.500107100,"alt":25.130,"epx":8.500,"epy":9.149,"epv":12.506,"track":217.8765,"speed":0.319,"climb":0.105,"eps":-0.14,"epc":25.75,"ecefx":-4387120.41,"ecefy":734143.24,"ecefz":-4555799.18,"ecefvx":-0.03,"ecefvy":0.31,"ecefvz":0.07,"ecefpAcc":19.39,"ecefvAcc":0.62,"eph":14.891,"sep":23.750}
+{"class":"TPV","mode":3,"time":"2019-04-22T21:19:41.000Z","leapseconds":18,"ept":0.005,"lat":-45.877520200,"lon":170.500107100,"alt":25.130,"epx":8.500,"epy":9.149,"epv":12.506,"track":217.8765,"speed":0.319,"climb":0.105,"eps":-0.14,"epc":25.75,"ecefx":-4387120.41,"ecefy":734143.24,"ecefz":-4555799.19,"ecefvx":-0.03,"ecefvy":0.31,"ecefvz":0.07,"ecefpAcc":19.39,"ecefvAcc":0.62,"eph":14.891,"sep":23.750}
$GPZDA,211942.00,22,04,2019,00,00*65
$GPGGA,211942.00,4552.6517,S,17030.0061,E,1,16,0.77,24.79,M,,,*3D
$GPRMC,211942.00,A,4552.6517,S,17030.0061,E,0.7911,217.876,220419,,*1D
|