From bbc5186e2a3b4d4ef0e66f2eb0654f7b4528886d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Fri, 17 Feb 2017 22:28:55 +0100
Subject: [PATCH] GPS driver: Improve GPS "fake" mode init and code style

---
 src/drivers/gps/gps.cpp | 27 ++++++++++++++-------------
 1 file changed, 14 insertions(+), 13 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index c6c4550143..17e32e47cc 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -253,7 +253,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 	_interface(interface),
 	_helper(nullptr),
 	_sat_info(nullptr),
-	_report_gps_pos_pub{nullptr},
+	_report_gps_pos{},
+	_report_gps_pos_pub(nullptr),
 	_gps_orb_instance(-1),
 	_p_report_sat_info(nullptr),
 	_report_sat_info_pub(nullptr),
@@ -272,8 +273,6 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 	/* enforce null termination */
 	_port[sizeof(_port) - 1] = '\0';
 
-	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
-
 	/* create satellite info data object if requested */
 	if (enable_sat_info) {
 		_sat_info = new GPS_Sat_Info();
@@ -657,15 +656,17 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 void
 GPS::task_main()
 {
-	/* open the serial port */
-	_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+	if (!_fake_gps) {
+		/* open the serial port */
+		_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
-	if (_serial_fd < 0) {
-		PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
+		if (_serial_fd < 0) {
+			PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
 
-		/* tell the dtor that we are exiting, set error code */
-		_task = -1;
-		px4_task_exit(1);
+			/* tell the dtor that we are exiting, set error code */
+			_task = -1;
+			px4_task_exit(1);
+		}
 	}
 
 	_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
@@ -679,6 +680,7 @@ GPS::task_main()
 	while (!_task_should_exit) {
 
 		if (_fake_gps) {
+			_report_gps_pos = {};
 			_report_gps_pos.timestamp = hrt_absolute_time();
 			_report_gps_pos.lat = (int32_t)47.378301e7f;
 			_report_gps_pos.lon = (int32_t)8.538777e7f;
@@ -694,8 +696,7 @@ GPS::task_main()
 			_report_gps_pos.vel_n_m_s = 0.0f;
 			_report_gps_pos.vel_e_m_s = 0.0f;
 			_report_gps_pos.vel_d_m_s = 0.0f;
-			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
-							_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+			_report_gps_pos.vel_m_s = 0.0f;
 			_report_gps_pos.cog_rad = 0.0f;
 			_report_gps_pos.vel_ned_valid = true;
 			_report_gps_pos.satellites_used = 10;
@@ -705,7 +706,7 @@ GPS::task_main()
 
 			publish();
 
-			usleep(2e5);
+			usleep(200000);
 
 		} else {