From 9d49690f17ee7b401c21953abee9e412a92bff5c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Wed, 2 Aug 2017 12:18:48 +0200
Subject: [PATCH] GPS simulation: Manage delays correctly

The GPS simulation now mimicks the real driver more closely and should provide even GPS delays. The delays themselves are set by the simulator, and default to 120 ms for Gazebo
---
 Tools/sitl_gazebo                             |  2 +-
 src/modules/simulator/simulator.h             |  1 +
 src/modules/simulator/simulator_mavlink.cpp   |  1 +
 src/platforms/posix/drivers/gpssim/gpssim.cpp | 47 ++++++++++++-------
 4 files changed, 33 insertions(+), 18 deletions(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 0686806fd9..db9a70eccb 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 0686806fd94bd429aa66d2500e5e2aad16d2835b
+Subproject commit db9a70eccbb49240b70eeaea867c1389bcbd73aa
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index f0df09845b..d9db8d8139 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -111,6 +111,7 @@ struct RawAirspeedData {
 
 #pragma pack(push, 1)
 struct RawGPSData {
+	int64_t timestamp;
 	int32_t lat;
 	int32_t lon;
 	int32_t alt;
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index c74bc30025..119c28d4d3 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -261,6 +261,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
 void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
 {
 	RawGPSData gps = {};
+	gps.timestamp = gps_sim->time_usec;
 	gps.lat = gps_sim->lat;
 	gps.lon = gps_sim->lon;
 	gps.alt = gps_sim->alt;
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index f6794a36d2..8eef4f1fb1 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -71,6 +71,7 @@ using namespace DriverFramework;
 #define GPSSIM_DEVICE_PATH "/dev/gpssim"
 
 #define TIMEOUT_5HZ 500
+#define TIMEOUT_10MS 10
 #define RATE_MEASUREMENT_PERIOD 5000000
 
 /* class for dynamic allocation of satellite info data */
@@ -274,22 +275,32 @@ GPSSIM::receive(int timeout)
 	simulator::RawGPSData gps;
 	sim->getGPSSample((uint8_t *)&gps, sizeof(gps));
 
-	_report_gps_pos.timestamp = hrt_absolute_time();
-	_report_gps_pos.lat = gps.lat;
-	_report_gps_pos.lon = gps.lon;
-	_report_gps_pos.alt = gps.alt;
-	_report_gps_pos.eph = (float)gps.eph * 1e-2f;
-	_report_gps_pos.epv = (float)gps.epv * 1e-2f;
-	_report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
-	_report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
-	_report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
-	_report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f;
-	_report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f);
-	_report_gps_pos.fix_type = gps.fix_type;
-	_report_gps_pos.satellites_used = gps.satellites_visible;
+	static uint64_t timestamp_last = 0;
 
-	usleep(120000);
-	return 1;
+	if (gps.timestamp != timestamp_last) {
+		_report_gps_pos.timestamp = hrt_absolute_time();
+		_report_gps_pos.lat = gps.lat;
+		_report_gps_pos.lon = gps.lon;
+		_report_gps_pos.alt = gps.alt;
+		_report_gps_pos.eph = (float)gps.eph * 1e-2f;
+		_report_gps_pos.epv = (float)gps.epv * 1e-2f;
+		_report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
+		_report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
+		_report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
+		_report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f;
+		_report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f);
+		_report_gps_pos.fix_type = gps.fix_type;
+		_report_gps_pos.satellites_used = gps.satellites_visible;
+
+		timestamp_last = gps.timestamp;
+
+		return 1;
+
+	} else {
+
+		usleep(timeout);
+		return 0;
+	}
 }
 
 void
@@ -349,10 +360,12 @@ GPSSIM::task_main()
 			// GPS is obviously detected successfully, reset statistics
 			//_Helper->reset_update_rates();
 
-			while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
+			int recv_ret = 0;
+
+			while ((recv_ret = receive(TIMEOUT_10MS)) >= 0 && !_task_should_exit) {
 				/* opportunistic publishing - else invalid data would end up on the bus */
 
-				if (!(m_pub_blocked)) {
+				if (recv_ret && !(m_pub_blocked)) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
 					if (_p_report_sat_info) {