[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[gpsd-dev] [PATCH] Support UBX NAV-PVT
|
From: |
Clark Li |
|
Subject: |
[gpsd-dev] [PATCH] Support UBX NAV-PVT |
|
Date: |
Wed, 19 Jul 2017 12:13:44 +0000 |
NAV-SOL has only been retained for backwards compatibility; users are
recommended to use the UBX-NAV-PVT message in preference.
Check valid flags of iTOW and fTOW and process fTOW whose range is
+-500us
---
driver_ubx.c | 172 +++++++++++++++++++++++++++++++++------
driver_ubx.h | 11 +++
test/daemon/ublox-aek-4t.log.chk | 2 +-
3 files changed, 161 insertions(+), 24 deletions(-)
diff --git a/driver_ubx.c b/driver_ubx.c index 88be6c07..358d0a61 100644
--- a/driver_ubx.c
+++ b/driver_ubx.c
@@ -130,19 +130,129 @@ static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t
*session, unsigned char *buf,
size_t data_len)
{
- unsigned int flags;
+ uint8_t valid;
+ uint8_t flags;
+ uint8_t navmode;
+ struct tm unpacked_date;
+ double subseconds;
+ double hacc, vacc, sacc;
+ int *status = &session->gpsdata.status;
+ int *mode = &session->newdata.mode;
gps_mask_t mask = 0;
if (data_len != 92)
- return 0;
+ return 0;
+ valid = (unsigned int)getub(buf, 11);
+ navmode = (unsigned char)getub(buf, 20);
flags = (unsigned int)getub(buf, 21);
- /* TODO: finish decoding UBX_MON_PVT
- * no need until depreacaed UBX_MON_SOL is dead
- */
+ switch (navmode)
+ {
+ case UBX_MODE_TMONLY:
+ {
+ if (*mode != MODE_NO_FIX) {
+ *mode = MODE_NO_FIX;
+ mask |= MODE_SET;
+ }
+ if (*status != STATUS_NO_FIX) {
+ *status = STATUS_NO_FIX;
+ mask |= STATUS_SET;
+ }
+ break;
+ }
+ case UBX_MODE_3D:
+ case UBX_MODE_GPSDR:
+ {
+ if (*mode != MODE_3D) {
+ *mode = MODE_3D;
+ mask |= MODE_SET;
+ }
+ if ((flags & UBX_NAV_PVT_FLAG_DGPS) == UBX_NAV_PVT_FLAG_DGPS) {
+ if (*status != STATUS_DGPS_FIX) {
+ *status = STATUS_DGPS_FIX;
+ mask |= STATUS_SET;
+ }
+ } else {
+ if (*status != STATUS_FIX) {
+ *status = STATUS_FIX;
+ mask |= STATUS_SET;
+ }
+ }
+ mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLEAR_IS
+ | REPORT_IS;
+ break;
+ }
+ case UBX_MODE_2D:
+ case UBX_MODE_DR: /* consider this too as 2D */
+ {
+ if (*mode != MODE_2D) {
+ *mode = MODE_2D;
+ mask |= MODE_SET;
+ };
+ if (*status != STATUS_FIX) {
+ *status = STATUS_FIX;
+ mask |= STATUS_SET;
+ }
+ mask |= LATLON_SET | SPEED_SET;
+ break;
+ }
+ default:
+ {
+ if (*mode != MODE_NO_FIX) {
+ *mode = MODE_NO_FIX;
+ mask |= MODE_SET;
+ };
+ if (*status != STATUS_NO_FIX) {
+ *status = STATUS_NO_FIX;
+ mask |= STATUS_SET;
+ }
+ break;
+ }
+ }
+
+ if ((valid & UBX_NAV_PVT_VALID_DATE_TIME) == UBX_NAV_PVT_VALID_DATE_TIME) {
+ unpacked_date.tm_year = (uint16_t)getleu16(buf, 4) - 1900;
+ unpacked_date.tm_mon = (uint8_t)getub(buf, 6) - 1;
+ unpacked_date.tm_mday = (uint8_t)getub(buf, 7);
+ unpacked_date.tm_hour = (uint8_t)getub(buf, 8);
+ unpacked_date.tm_min = (uint8_t)getub(buf, 9);
+ unpacked_date.tm_sec = (uint8_t)getub(buf, 10);
+ unpacked_date.tm_isdst = 0;
+ unpacked_date.tm_wday = 0;
+ unpacked_date.tm_yday = 0;
+ subseconds = 1e-9 * (int32_t)getles32(buf, 16);
+ session->newdata.time = \
+ (timestamp_t)mkgmtime(&unpacked_date) + subseconds;
+ mask |= TIME_SET | NTPTIME_IS;
+ }
+
+ session->newdata.longitude = 1e-7 * (int32_t)getles32(buf, 24);
+ session->newdata.latitude = 1e-7 * (int32_t)getles32(buf, 28);
+ session->newdata.altitude = 1e-3 * (int32_t)getles32(buf, 32);
+ session->newdata.speed = 1e-3 * (int32_t)getles32(buf, 60);
+ session->newdata.track = 1e-5 * (int32_t)getles32(buf, 64);
+ hacc = (double)(getles32(buf, 40) / 1000.0);
+ vacc = (double)(getles32(buf, 44) / 1000.0);
+ sacc = (double)(getles32(buf, 48) / 1000.0);
+ // epx ^ 2 + epy ^ 2 = hacc ^ 2
+ session->newdata.epx = session->newdata.epy = 0.707 * hacc;
+ session->newdata.epv = vacc;
+ session->newdata.eps = sacc;
+ mask |= HERR_SET | VERR_SET | SPEEDERR_SET;
gpsd_log(&session->context->errout, LOG_DATA,
- "NAV-PVT: flags:%02x\n", flags);
+ "NAV_PVT: flags=%02x time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f
speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
+ flags,
+ session->newdata.time,
+ session->newdata.latitude,
+ session->newdata.longitude,
+ session->newdata.altitude,
+ session->newdata.track,
+ session->newdata.speed,
+ session->newdata.climb,
+ session->newdata.mode,
+ session->gpsdata.status,
+ session->gpsdata.satellites_used);
return mask;
}
@@ -166,10 +276,12 @@ ubx_msg_nav_sol(struct gps_device_t *session, unsigned
char *buf,
#define DATE_VALID (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)
if ((flags & DATE_VALID) == DATE_VALID) {
unsigned short gw;
- unsigned int tow;
- tow = (unsigned int)getleu32(buf, 0);
+ unsigned int iTOW;
+ int fTOW;
+ iTOW = (unsigned int)getleu32(buf, 0);
+ fTOW = (int)getles32(buf, 4);
gw = (unsigned short)getles16(buf, 8);
- session->newdata.time = gpsd_gpstime_resolve(session, gw, tow / 1000.0);
+ session->newdata.time = gpsd_gpstime_resolve(session, gw, (iTOW *
+1e-3) + (fTOW * 1e-9));
mask |= TIME_SET | NTPTIME_IS;
}
#undef DATE_VALID
@@ -292,24 +404,38 @@ static gps_mask_t
ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf,
size_t data_len)
{
- unsigned int gw, tow, flags;
+ unsigned int gw, iTOW, flags;
+ int fTOW;
+ gps_mask_t mask = 0;
if (data_len != 16)
- return 0;
+ return 0;
- tow = (unsigned int)getleu32(buf, 0);
- gw = (unsigned int)getles16(buf, 8);
flags = (unsigned int)getub(buf, 11);
- if ((flags & 0x7) != 0)
- session->context->leap_seconds = (int)getub(buf, 10);
- session->newdata.time = gpsd_gpstime_resolve(session,
- (unsigned short int)gw,
- (double)tow / 1000.0);
+
+ // Valid leap seconds
+ if ((flags & UBX_TIMEGPS_VALID_LEAP_SECOND) ==
UBX_TIMEGPS_VALID_LEAP_SECOND)
+ session->context->leap_seconds = (int)getub(buf, 10);
+
+ // Valid GPS time of week and week number #define VALID_TIME
+(UBX_TIMEGPS_VALID_TIME | UBX_TIMEGPS_VALID_WEEK)
+ if ((flags & VALID_TIME) == VALID_TIME) #undef VALID_TIME
+ {
+ iTOW = (unsigned int)getleu32(buf, 0);
+ fTOW = (int)getles32(buf, 4);
+ gw = (unsigned int)getles16(buf, 8);
+ session->newdata.time =
+ gpsd_gpstime_resolve(session,
+ (unsigned short int)gw,
+ ((double)iTOW * 1e-3) + ((double)fTOW * 1e-9));
+ mask |= (TIME_SET | NTPTIME_IS);
+ }
gpsd_log(&session->context->errout, LOG_DATA,
- "TIMEGPS: time=%.2f leap=%d, mask={TIME}\n",
- session->newdata.time, session->context->leap_seconds);
- return TIME_SET | NTPTIME_IS;
+ "TIMEGPS: time=%.2f mask={TIME}\n",
+ session->newdata.time);
+ return mask;
}
/**
@@ -486,8 +612,8 @@ 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");
- mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
- break;
+ mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len)
+ | (CLEAR_IS | REPORT_IS);
case UBX_NAV_POSUTM:
gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_POSUTM\n");
break;
diff --git a/driver_ubx.h b/driver_ubx.h index c23efdf7..99ef9b98 100644
--- a/driver_ubx.h
+++ b/driver_ubx.h
@@ -102,6 +102,10 @@ typedef enum {
#define UBX_SOL_VALID_WEEK 0x04
#define UBX_SOL_VALID_TIME 0x08
+#define UBX_TIMEGPS_VALID_TIME 0x01
+#define UBX_TIMEGPS_VALID_WEEK 0x02
+#define UBX_TIMEGPS_VALID_LEAP_SECOND 0x04
+
/* from UBX_NAV_SVINFO */
#define UBX_SAT_USED 0x01
#define UBX_SAT_DGPS 0x02
@@ -118,4 +122,11 @@ typedef enum {
#define UBX_SIG_CDCRLK2 6
#define UBX_SIG_NAVMSG 7
+#define UBX_NAV_PVT_VALID_DATE 0x01
+#define UBX_NAV_PVT_VALID_TIME 0x02
+#define UBX_NAV_PVT_VALID_DATE_TIME (UBX_NAV_PVT_VALID_DATE |
+UBX_NAV_PVT_VALID_TIME)
+
+#define UBX_NAV_PVT_FLAG_GPS_FIX_OK 0x01
+#define UBX_NAV_PVT_FLAG_DGPS 0x02
+
#endif /* _GPSD_UBX_H_ */
diff --git a/test/daemon/ublox-aek-4t.log.chk b/test/daemon/ublox-aek-4t.log.chk
index 45680ea6..31acd979 100644
--- a/test/daemon/ublox-aek-4t.log.chk
+++ b/test/daemon/ublox-aek-4t.log.chk
@@ -26,4 +26,4 @@ $GPZDA,231127.00,23,12,2007,00,00*65
$GPGGA,231126,2037.7569,N,08704.0845,W,2,07,1.67,75.75,M,-13.865,M,,*4C
$GPRMC,231126,A,2037.7569,N,08704.0845,W,0.0000,0.000,231207,,*36
$GPGSA,A,3,15,2,30,5,12,18,135,,,,,,2.6,1.7,2.0*0F
-{"class":"TPV","status":2,"mode":3,"time":"2007-12-23T23:11:26.999Z","ept":0.005,"lat":20.629281718,"lon":-87.068075424,"alt":75.754,"epx":2.973,"epy":6.138,"epv":11.443,"track":0.0000,"speed":0.000,"climb":0.000,"eps":0.17,"epc":22.89}
+{"class":"TPV","status":2,"mode":3,"time":"2007-12-23T23:11:26.999Z","e
+pt":0.005,"lat":20.629281718,"lon":-87.068075424,"alt":75.754,"epx":2.9
+73,"epy":6.138,"epv":11.443,"track":0.0000,"speed":0.000,"climb":0.000,
+"eps":0.17,"epc":22.88}
--
2.13.0
This communication may contain confidential or copyright information. If you
are not an intended recipient, you must not keep, forward, copy, use, save or
rely on this communication, and any such action is unauthorized and prohibited.
If you have received this communication in error, please reply to this email to
notify the sender of its incorrect delivery and then delete both the original
message and your reply.
- [gpsd-dev] [PATCH] Support UBX NAV-PVT,
Clark Li <=