diff options
-rw-r--r-- | driver_ubx.c | 100 | ||||
-rw-r--r-- | gps.h | 17 | ||||
-rw-r--r-- | gpsd_json.c | 32 | ||||
-rw-r--r-- | gpsutils.c | 24 | ||||
-rw-r--r-- | libgpsd_core.c | 5 |
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__ @@ -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]; @@ -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 |