GPS driver UBX: Support hdop and vdop

src/drivers/gps/gps.cpp

@@ -553,6 +553,7 @@ GPS::print_info()
 		warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
 		warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
 		      (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
+		warnx("hdop: %.2f, vdop: %.2f", (double)_report_gps_pos.hdop, (double)_report_gps_pos.vdop);
 		warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());

src/drivers/gps/ubx.cpp

@@ -237,6 +237,12 @@ UBX::configure(unsigned &baudrate)
 			return 1;
 		}
 
+		configure_message_rate(UBX_MSG_NAV_DOP, 1);
+
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
+
 		configure_message_rate(UBX_MSG_NAV_VELNED, 1);
 
 		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
@@ -808,9 +814,6 @@ UBX::payload_rx_done(void)
 		_gps_position->epv		= (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
 		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
 
-		_gps_position->hdop		= (float)_buf.payload_rx_nav_pvt.pDOP * 0.01f;
-		_gps_position->vdop		= _gps_position->hdop;
-
 		_gps_position->vel_m_s		= (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
 
 		_gps_position->vel_n_m_s	= (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
@@ -894,10 +897,19 @@ UBX::payload_rx_done(void)
 		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f;	// from cm to m
 		_gps_position->satellites_used	= _buf.payload_rx_nav_sol.numSV;
 
-		_gps_position->hdop		= (float)_buf.payload_rx_nav_sol.pDOP * 0.01f;
-		_gps_position->vdop		= _gps_position->hdop;
+		_gps_position->timestamp_variance = hrt_absolute_time();
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_NAV_DOP:
+		UBX_TRACE_RXMSG("Rx NAV-DOP\n");
+
+		_gps_position->hdop		= _buf.payload_rx_nav_dop.hDOP * 0.01f;	// from cm to m
+		_gps_position->vdop		= _buf.payload_rx_nav_dop.vDOP * 0.01f;	// from cm to m
 
 		_gps_position->timestamp_variance = hrt_absolute_time();
+		warnx("DOOOP!");
 
 		ret = 1;
 		break;

src/drivers/gps/ubx.h

@@ -62,6 +62,7 @@
 
 /* Message IDs */
 #define UBX_ID_NAV_POSLLH	0x02
+#define UBX_ID_NAV_DOP		0x04
 #define UBX_ID_NAV_SOL		0x06
 #define UBX_ID_NAV_PVT		0x07
 #define UBX_ID_NAV_VELNED	0x12
@@ -80,6 +81,7 @@
 /* Message Classes & IDs */
 #define UBX_MSG_NAV_POSLLH	((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
 #define UBX_MSG_NAV_SOL		((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
+#define UBX_MSG_NAV_DOP		((UBX_CLASS_NAV) | UBX_ID_NAV_DOP << 8)
 #define UBX_MSG_NAV_PVT		((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
 #define UBX_MSG_NAV_VELNED	((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
 #define UBX_MSG_NAV_TIMEUTC	((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
@@ -169,6 +171,18 @@ typedef struct {
 	uint32_t	vAcc;  		/**< Vertical accuracy estimate [mm] */
 } ubx_payload_rx_nav_posllh_t;
 
+/* Rx NAV-DOP */
+typedef struct {
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint16_t	gDOP;		/**< Geometric DOP [0.01] */
+	uint16_t	pDOP;		/**< Position DOP [0.01] */
+	uint16_t	tDOP;		/**< Time DOP [0.01] */
+	uint16_t	vDOP;		/**< Vertical DOP [0.01] */
+	uint16_t	hDOP;		/**< Horizontal DOP [0.01] */
+	uint16_t	nDOP;		/**< Northing DOP [0.01] */
+	uint16_t	eDOP;		/**< Easting DOP [0.01] */
+} ubx_payload_rx_nav_dop_t;
+
 /* Rx NAV-SOL */
 typedef struct {
 	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
@@ -184,7 +198,7 @@ typedef struct {
 	int32_t		ecefVY;
 	int32_t		ecefVZ;
 	uint32_t	sAcc;
-	uint16_t	pDOP;
+	uint16_t	pDOP;		/**< Position DOP [0.01] */
 	uint8_t		reserved1;
 	uint8_t		numSV;		/**< Number of SVs used in Nav Solution */
 	uint32_t	reserved2;
@@ -416,6 +430,7 @@ typedef union {
 	ubx_payload_rx_nav_pvt_t		payload_rx_nav_pvt;
 	ubx_payload_rx_nav_posllh_t		payload_rx_nav_posllh;
 	ubx_payload_rx_nav_sol_t		payload_rx_nav_sol;
+	ubx_payload_rx_nav_dop_t		payload_rx_nav_dop;
 	ubx_payload_rx_nav_timeutc_t		payload_rx_nav_timeutc;
 	ubx_payload_rx_nav_svinfo_part1_t	payload_rx_nav_svinfo_part1;
 	ubx_payload_rx_nav_svinfo_part2_t	payload_rx_nav_svinfo_part2;