diff options
author | Gary E. Miller <gem@rellim.com> | 2019-04-22 20:09:20 -0700 |
---|---|---|
committer | Gary E. Miller <gem@rellim.com> | 2019-04-22 20:09:20 -0700 |
commit | 958a4c0c3b84e7be4b1805beafbc183788be9e6e (patch) | |
tree | 4bc30318f5dbfa777bc88e136684746a7c0c2ab5 | |
parent | be8b170d30835954e9e1a8e4a515a36b2918f552 (diff) | |
download | gpsd-958a4c0c3b84e7be4b1805beafbc183788be9e6e.tar.gz |
driver_ubx: Add decode for UBX-NAV-HPPOSECEF.
Does nothing until JSON ECEF gets goes to mm, from cm.
-rw-r--r-- | driver_ubx.c | 42 |
1 files changed, 41 insertions, 1 deletions
diff --git a/driver_ubx.c b/driver_ubx.c index fbb8efee..3844539d 100644 --- a/driver_ubx.c +++ b/driver_ubx.c @@ -149,6 +149,45 @@ ubx_msg_mon_ver(struct gps_device_t *session, unsigned char *buf, } /* + * UBX-NAV-HPPOSECEF - High Precision Position Solution in ECEF + */ +static gps_mask_t +ubx_msg_nav_hpposecef(struct gps_device_t *session, unsigned char *buf, + size_t data_len) +{ + gps_mask_t mask = ECEF_SET; + int version; + + if (28 > data_len) { + gpsd_log(&session->context->errout, LOG_WARN, + "Runt UBX-NAV-HPPOSECEF message, payload len %zd", data_len); + return 0; + } + + 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; + /* (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", + version, + (long long)session->driver.ubx.iTOW, + session->newdata.ecef.x, + session->newdata.ecef.y, + session->newdata.ecef.z, + session->newdata.ecef.pAcc); + return mask; +} + +/* * Navigation Position ECEF message */ static gps_mask_t @@ -159,7 +198,7 @@ ubx_msg_nav_posecef(struct gps_device_t *session, unsigned char *buf, if (20 > data_len) { gpsd_log(&session->context->errout, LOG_WARN, - "Runt NAV POSECEF message, payload len %zd", data_len); + "Runt UBX-NAV-POSECEF message, payload len %zd", data_len); return 0; } @@ -1392,6 +1431,7 @@ gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf, break; case UBX_NAV_HPPOSECEF: gpsd_log(&session->context->errout, LOG_DATA, "UBX-NAV-HPPOSECEF\n"); + mask = ubx_msg_nav_hpposecef(session, &buf[UBX_PREFIX_LEN], data_len); break; case UBX_NAV_HPPOSLLH: gpsd_log(&session->context->errout, LOG_DATA, "UBX-NAV-HPPOSLLH\n"); |