diff --git a/driver_ubx.c b/driver_ubx.c index e0055af5..05edc594 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; + 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; } @@ -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"); diff --git a/driver_ubx.h b/driver_ubx.h index c4e28fed..99ef9b98 100644 --- a/driver_ubx.h +++ b/driver_ubx.h @@ -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_ */ diff --git a/test/daemon/ublox-neo-m8n.log b/test/daemon/ublox-neo-m8n.log new file mode 100644 index 00000000..28f7c121 Binary files /dev/null and b/test/daemon/ublox-neo-m8n.log differ diff --git a/test/daemon/ublox-neo-m8n.log.chk b/test/daemon/ublox-neo-m8n.log.chk new file mode 100644 index 00000000..d823a0c0 --- /dev/null +++ b/test/daemon/ublox-neo-m8n.log.chk @@ -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}