Skip to content

Commit

Permalink
Support UBX NAV-PVT
Browse files Browse the repository at this point in the history 
NAV-SOL has only been retained for backwards compatibility;
users are recommended to use the UBX-NAV-PVT message in preference.

A regression test case using ublox-neo-m8n is also added.
  • Loading branch information
@clarkli86 @eric-s-raymond
clarkli86 authored and eric-s-raymond committed Sep 19, 2017
1 parent 3d0da91 commit 71a487d
Show file tree
Hide file tree
Showing 4 changed files with 244 additions and 6 deletions.
123 changes: 117 additions & 6 deletions driver_ubx.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

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);
// Assuming hacc == epx == epy is the best we can do
session->newdata.epx = session->newdata.epy = 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;
}

Expand Down Expand Up @@ -498,7 +608,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);
mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len)
| (CLEAR_IS | REPORT_IS);
break;
case UBX_NAV_POSUTM:
gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_POSUTM\n");
Expand Down
7 changes: 7 additions & 0 deletions driver_ubx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,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_ */
Binary file added test/daemon/ublox-neo-m8n.log
View file
Binary file not shown.
120 changes: 120 additions & 0 deletions test/daemon/ublox-neo-m8n.log.chk
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,120 @@
$GPZDA,085000.80,19,07,2017,00,00*68
$GPGGA,085000,3454.3736,S,13836.4872,E,2,00,,25.42,M,,,*00
$GPRMC,085000,A,3454.3736,S,13836.4872,E,0.0583,31.472,190717,,*08
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:00.800Z","ept":0.005,"lat":-34.906226300,"lon":138.608119700,"alt":25.424,"epx":1.132,"epy":1.132,"epv":1.602,"track":31.4718,"speed":0.030,"eps":-0.01}
$GPZDA,085001.00,19,07,2017,00,00*61
$GPGGA,085000,3454.3736,S,13836.4872,E,2,00,,25.38,M,,,*0D
$GPRMC,085000,A,3454.3736,S,13836.4872,E,0.0311,31.472,190717,,*05
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:00.000Z","ept":0.005,"lat":-34.906226600,"lon":138.608119900,"alt":25.378,"epx":1.133,"epy":1.133,"epv":1.603,"track":31.4718,"speed":0.016,"climb":-0.230,"eps":0.02,"epc":16.02}
$GPZDA,085001.20,19,07,2017,00,00*63
$GPGGA,085001,3454.3736,S,13836.4872,E,2,00,,25.31,M,,,*05
$GPRMC,085001,A,3454.3736,S,13836.4872,E,0.0311,31.472,190717,,*04
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:01.200Z","ept":0.005,"lat":-34.906226900,"lon":138.608119900,"alt":25.306,"epx":1.138,"epy":1.138,"epv":1.610,"track":31.4718,"speed":0.016,"climb":-0.360,"eps":-0.01,"epc":16.06}
$GPZDA,085001.40,19,07,2017,00,00*65
$GPGGA,085001,3454.3736,S,13836.4872,E,2,00,,25.26,M,,,*03
$GPRMC,085001,A,3454.3736,S,13836.4872,E,0.0583,31.472,190717,,*09
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:01.400Z","ept":0.005,"lat":-34.906227100,"lon":138.608120200,"alt":25.258,"epx":1.142,"epy":1.142,"epv":1.616,"track":31.4718,"speed":0.030,"climb":-0.240,"eps":0.02,"epc":16.13}
$GPZDA,085001.60,19,07,2017,00,00*67
$GPGGA,085001,3454.3736,S,13836.4872,E,2,00,,25.19,M,,,*0F
$GPRMC,085001,A,3454.3736,S,13836.4872,E,0.0408,31.472,190717,,*0B
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:01.600Z","ept":0.005,"lat":-34.906227500,"lon":138.608120300,"alt":25.194,"epx":1.144,"epy":1.144,"epv":1.619,"track":31.4718,"speed":0.021,"climb":-0.320,"eps":-0.02,"epc":16.17}
$GPZDA,085001.80,19,07,2017,00,00*69
$GPGGA,085001,3454.3737,S,13836.4872,E,2,00,,25.14,M,,,*03
$GPRMC,085001,A,3454.3737,S,13836.4872,E,0.0253,31.472,190717,,*02
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:01.800Z","ept":0.005,"lat":-34.906227800,"lon":138.608120300,"alt":25.136,"epx":1.144,"epy":1.144,"epv":1.619,"track":31.4718,"speed":0.013,"climb":-0.290,"eps":0.01,"epc":16.19}
$GPZDA,085002.00,19,07,2017,00,00*62
$GPGGA,085001,3454.3737,S,13836.4872,E,2,00,,25.07,M,,,*01
$GPRMC,085001,A,3454.3737,S,13836.4872,E,0.0739,31.472,190717,,*0B
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:01.000Z","ept":0.005,"lat":-34.906228100,"lon":138.608120400,"alt":25.073,"epx":1.145,"epy":1.145,"epv":1.620,"track":31.4718,"speed":0.038,"climb":-0.315,"eps":0.03,"epc":16.19}
$GPZDA,085002.20,19,07,2017,00,00*60
$GPGGA,085002,3454.3737,S,13836.4872,E,2,00,,25.01,M,,,*04
$GPRMC,085002,A,3454.3737,S,13836.4872,E,0.0058,31.472,190717,,*08
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:02.200Z","ept":0.005,"lat":-34.906228400,"lon":138.608120500,"alt":25.008,"epx":1.143,"epy":1.143,"epv":1.617,"track":31.4718,"speed":0.003,"climb":-0.325,"eps":-0.00,"epc":16.18}
$GPZDA,085002.40,19,07,2017,00,00*66
$GPGGA,085002,3454.3737,S,13836.4872,E,2,00,,24.95,M,,,*08
$GPRMC,085002,A,3454.3737,S,13836.4872,E,0.0428,31.472,190717,,*0B
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:02.400Z","ept":0.005,"lat":-34.906228700,"lon":138.608120500,"alt":24.952,"epx":1.133,"epy":1.133,"epv":1.603,"track":31.4718,"speed":0.022,"climb":-0.280,"eps":0.02,"epc":16.10}
$GPZDA,085002.60,19,07,2017,00,00*64
$GPGGA,085002,3454.3737,S,13836.4872,E,2,00,,24.89,M,,,*05
$GPRMC,085002,A,3454.3737,S,13836.4872,E,0.0233,31.472,190717,,*07
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:02.600Z","ept":0.005,"lat":-34.906229100,"lon":138.608120700,"alt":24.892,"epx":1.136,"epy":1.136,"epv":1.608,"track":31.4718,"speed":0.012,"climb":-0.300,"eps":-0.01,"epc":16.05}
$GPZDA,085002.80,19,07,2017,00,00*6A
$GPGGA,085002,3454.3738,S,13836.4872,E,2,00,,24.83,M,,,*00
$GPRMC,085002,A,3454.3738,S,13836.4872,E,0.0564,31.472,190717,,*0D
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:02.800Z","ept":0.005,"lat":-34.906229400,"lon":138.608120700,"alt":24.831,"epx":1.148,"epy":1.148,"epv":1.625,"track":31.4718,"speed":0.029,"climb":-0.305,"eps":0.03,"epc":16.16}
$GPZDA,085003.00,19,07,2017,00,00*63
$GPGGA,085002,3454.3738,S,13836.4873,E,2,00,,24.76,M,,,*0B
$GPRMC,085002,A,3454.3738,S,13836.4873,E,0.0505,31.472,190717,,*0B
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:02.000Z","ept":0.005,"lat":-34.906229800,"lon":138.608120900,"alt":24.760,"epx":1.146,"epy":1.146,"epv":1.622,"track":31.4718,"speed":0.026,"climb":-0.355,"eps":-0.02,"epc":16.23}
$GPZDA,085003.20,19,07,2017,00,00*61
$GPGGA,085003,3454.3738,S,13836.4873,E,2,00,,24.70,M,,,*0C
$GPRMC,085003,A,3454.3738,S,13836.4873,E,0.0350,31.472,190717,,*0C
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:03.200Z","ept":0.005,"lat":-34.906230200,"lon":138.608120900,"alt":24.695,"epx":1.142,"epy":1.142,"epv":1.616,"track":31.4718,"speed":0.018,"climb":-0.325,"eps":0.01,"epc":16.19}
$GPZDA,085003.40,19,07,2017,00,00*67
$GPGGA,085003,3454.3738,S,13836.4873,E,2,00,,24.62,M,,,*0F
$GPRMC,085003,A,3454.3738,S,13836.4873,E,0.0214,31.472,190717,,*0D
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:03.400Z","ept":0.005,"lat":-34.906230500,"lon":138.608121000,"alt":24.617,"epx":1.138,"epy":1.138,"epv":1.609,"track":31.4718,"speed":0.011,"climb":-0.390,"eps":0.01,"epc":16.12}
$GPZDA,085003.60,19,07,2017,00,00*65
$GPGGA,085003,3454.3739,S,13836.4873,E,2,00,,24.56,M,,,*09
$GPRMC,085003,A,3454.3739,S,13836.4873,E,0.0253,31.472,190717,,*0F
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:03.600Z","ept":0.005,"lat":-34.906230900,"lon":138.608121000,"alt":24.561,"epx":1.119,"epy":1.119,"epv":1.582,"track":31.4718,"speed":0.013,"climb":-0.280,"eps":-0.01,"epc":15.95}
$GPZDA,085003.80,19,07,2017,00,00*6B
$GPGGA,085003,3454.3739,S,13836.4873,E,2,00,,24.49,M,,,*07
$GPRMC,085003,A,3454.3739,S,13836.4873,E,0.0408,31.472,190717,,*07
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:03.800Z","ept":0.005,"lat":-34.906231300,"lon":138.608120900,"alt":24.486,"epx":1.098,"epy":1.098,"epv":1.552,"track":31.4718,"speed":0.021,"climb":-0.375,"eps":0.02,"epc":15.67}
$GPZDA,085004.00,19,07,2017,00,00*64
$GPGGA,085003,3454.3739,S,13836.4872,E,2,00,,24.39,M,,,*01
$GPRMC,085003,A,3454.3739,S,13836.4872,E,0.0233,31.472,190717,,*08
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:03.000Z","ept":0.005,"lat":-34.906231700,"lon":138.608120600,"alt":24.390,"epx":1.090,"epy":1.090,"epv":1.538,"track":31.4718,"speed":0.012,"climb":-0.480,"eps":-0.00,"epc":15.45}
$GPZDA,085004.20,19,07,2017,00,00*66
$GPGGA,085004,3454.3739,S,13836.4872,E,2,00,,24.31,M,,,*0E
$GPRMC,085004,A,3454.3739,S,13836.4872,E,0.0330,31.472,190717,,*0D
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:04.200Z","ept":0.005,"lat":-34.906232200,"lon":138.608120300,"alt":24.309,"epx":1.074,"epy":1.074,"epv":1.516,"track":31.4718,"speed":0.017,"climb":-0.405,"eps":-0.01,"epc":15.27}
$GPZDA,085004.40,19,07,2017,00,00*60
$GPGGA,085004,3454.3740,S,13836.4872,E,2,00,,24.23,M,,,*03
$GPRMC,085004,A,3454.3740,S,13836.4872,E,0.0194,31.472,190717,,*0F
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:04.400Z","ept":0.005,"lat":-34.906232600,"lon":138.608120000,"alt":24.225,"epx":1.054,"epy":1.054,"epv":1.487,"track":31.4718,"speed":0.010,"climb":-0.420,"eps":-0.01,"epc":15.01}
$GPZDA,085004.60,19,07,2017,00,00*62
$GPGGA,085004,3454.3740,S,13836.4872,E,2,00,,24.15,M,,,*06
$GPRMC,085004,A,3454.3740,S,13836.4872,E,0.0505,31.472,190717,,*03
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:04.600Z","ept":0.005,"lat":-34.906233100,"lon":138.608119600,"alt":24.154,"epx":1.031,"epy":1.031,"epv":1.455,"track":31.4718,"speed":0.026,"climb":-0.355,"eps":0.03,"epc":14.71}
$GPZDA,085004.80,19,07,2017,00,00*6C
$GPGGA,085004,3454.3740,S,13836.4872,E,2,00,,24.06,M,,,*04
$GPRMC,085004,A,3454.3740,S,13836.4872,E,0.0700,31.472,190717,,*04
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:04.800Z","ept":0.005,"lat":-34.906233600,"lon":138.608119300,"alt":24.063,"epx":1.012,"epy":1.012,"epv":1.428,"track":31.4718,"speed":0.036,"climb":-0.455,"eps":-0.04,"epc":14.41}
$GPZDA,085005.00,19,07,2017,00,00*65
$GPGGA,085004,3454.3740,S,13836.4871,E,2,00,,23.98,M,,,*07
$GPRMC,085004,A,3454.3740,S,13836.4871,E,0.0408,31.472,190717,,*0C
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:04.000Z","ept":0.005,"lat":-34.906233900,"lon":138.608118900,"alt":23.975,"epx":0.997,"epy":0.997,"epv":1.405,"track":31.4718,"speed":0.021,"climb":-0.440,"eps":0.02,"epc":14.16}
$GPZDA,085005.20,19,07,2017,00,00*67
$GPGGA,085005,3454.3741,S,13836.4871,E,2,00,,23.88,M,,,*06
$GPRMC,085005,A,3454.3741,S,13836.4871,E,0.0156,31.472,190717,,*02
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:05.200Z","ept":0.005,"lat":-34.906234400,"lon":138.608118500,"alt":23.884,"epx":0.986,"epy":0.986,"epv":1.388,"track":31.4718,"speed":0.008,"climb":-0.455,"eps":0.01,"epc":13.96}
$GPZDA,085005.40,19,07,2017,00,00*61
$GPGGA,085005,3454.3741,S,13836.4871,E,2,00,,23.77,M,,,*06
$GPRMC,085005,A,3454.3741,S,13836.4871,E,0.0330,31.472,190717,,*00
$GPGSA,A,3,,,,,,,,,,,,,,,*1C
{"class":"TPV","status":2,"mode":3,"time":"2017-07-19T08:50:05.400Z","ept":0.005,"lat":-34.906234900,"lon":138.608118100,"alt":23.769,"epx":0.981,"epy":0.981,"epv":1.379,"track":31.4718,"speed":0.017,"climb":-0.575,"eps":-0.01,"epc":13.83}

0 comments on commit 71a487d

Please sign in to comment.