4 files changed, 27 insertions, 14 deletions

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 5342ccf78..dd505abdb 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -280,8 +280,8 @@ GPS::task_main()
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 0.9f;
-			_report.epv_m = 1.8f;
+			_report.eph = 0.9f;
+			_report.epv = 1.8f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;
@@ -357,7 +357,7 @@ GPS::task_main()
 					}
 
 					if (!_healthy) {
-						char *mode_str = "unknown";
+						const char *mode_str = "unknown";
 
 						switch (_mode) {
 						case GPS_DRIVER_MODE_UBX:
@@ -449,9 +449,9 @@ GPS::print_info()
 
 	if (_report.timestamp_position != 0) {
 		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
-				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
+				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.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());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -578,7 +578,7 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char *device_name = GPS_DEFAULT_UART_PORT;
+	const char *device_name = GPS_DEFAULT_UART_PORT;
 	bool fake_gps = false;
 
 	/*
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 680f00d97..41716cd97 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -251,16 +251,16 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		_gps_position->lon = 0;
 
 		// Indicate this data is not usable and bail out
-		_gps_position->eph_m = 1000.0f;
-		_gps_position->epv_m = 1000.0f;
+		_gps_position->eph = 1000.0f;
+		_gps_position->epv = 1000.0f;
 		_gps_position->fix_type = 0;
 		return;
 	}
 
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
-	_gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
+	_gps_position->eph = packet.hdop / 100.0f; // from cm to m
+	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 7502809bd..404607571 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 	_gps_position(gps_position),
 	_configured(false),
 	_waiting_for_ack(false),
+	_got_posllh(false),
+	_got_velned(false),
+	_got_timeutc(false),
 	_disable_cmd_last(0)
 {
 	decode_init();
@@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
 	bool handled = false;
 
 	while (true) {
+		bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
 
 		/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
@@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			if (handled) {
+			if (ready_to_return) {
+				_got_posllh = false;
+				_got_velned = false;
+				_got_timeutc = false;
 				return 1;
 
 			} else {
@@ -432,12 +439,13 @@ UBX::handle_message()
 					_gps_position->lat = packet->lat;
 					_gps_position->lon = packet->lon;
 					_gps_position->alt = packet->height_msl;
-					_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
-					_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+					_gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m
+					_gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
 					_gps_position->timestamp_position = hrt_absolute_time();
 
 					_rate_count_lat_lon++;
 
+					_got_posllh = true;
 					ret = 1;
 					break;
 				}
@@ -482,6 +490,7 @@ UBX::handle_message()
 					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 					_gps_position->timestamp_time = hrt_absolute_time();
 
+					_got_timeutc = true;
 					ret = 1;
 					break;
 				}
@@ -557,6 +566,7 @@ UBX::handle_message()
 
 					_rate_count_vel++;
 
+					_got_velned = true;
 					ret = 1;
 					break;
 				}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 5cf47b60b..43d688893 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -397,6 +397,9 @@ private:
 	struct vehicle_gps_position_s *_gps_position;
 	bool			_configured;
 	bool			_waiting_for_ack;
+	bool			_got_posllh;
+	bool			_got_velned;
+	bool			_got_timeutc;
 	uint8_t			_message_class_needed;
 	uint8_t			_message_id_needed;
 	ubx_decode_state_t	_decode_state;