From bcb6056a12ae88c2bc04425e3fae8992997b5cab Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Tue, 25 Feb 2020 10:58:30 +0000
Subject: [PATCH] simulator: make sensor update rate dependent of the
 HIL_SENSOR bitmask

---
 Tools/sitl_gazebo                           |  2 +-
 src/modules/simulator/simulator.h           | 11 +++++-
 src/modules/simulator/simulator_mavlink.cpp | 38 +++++++++------------
 3 files changed, 28 insertions(+), 23 deletions(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 88ed282562..f78daf7cd2 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 88ed2825628fb439055db1e2a90f11bdb7365a46
+Subproject commit f78daf7cd2646bf62b2fb844bf0ce209cf7b2783
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 5a69ecdf1a..797885e730 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -198,7 +198,7 @@ private:
 	void send_controls();
 	void send_heartbeat();
 	void send_mavlink_message(const mavlink_message_t &aMsg);
-	void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu);
+	void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors);
 
 	static void *sending_trampoline(void *);
 
@@ -245,6 +245,15 @@ private:
 		WaitingForActuatorControls = 1,
 		WaitingForEkf2Timestamp = 2,
 	};
+
+	///! Enumeration to use on the bitmask in HIL_SENSOR
+	enum SensorSource {
+		ACCEL		= 0x0007,
+		GYRO		= 0x0038,
+		MAG		= 0x01C0,
+		BARO		= 0x1A00,
+		DIFF_PRESS	= 0x0400
+	};
 #endif
 
 	DEFINE_PARAMETERS(
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index e0c7c994a9..be3dfbe3f2 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -191,43 +191,39 @@ void Simulator::send_controls()
 	}
 }
 
-void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu)
+void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors)
 {
-	if ((imu.fields_updated & 0x1FFF) != 0x1FFF) {
-		PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x", imu.fields_updated);
-	}
-
 	// gyro
-	if (!_param_sim_gyro_block.get()) {
-		_px4_gyro.set_temperature(imu.temperature);
-		_px4_gyro.update(time, imu.xgyro, imu.ygyro, imu.zgyro);
+	if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
+		_px4_gyro.set_temperature(sensors.temperature);
+		_px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
 	}
 
 	// accel
-	if (!_param_sim_accel_block.get()) {
-		_px4_accel.set_temperature(imu.temperature);
-		_px4_accel.update(time, imu.xacc, imu.yacc, imu.zacc);
+	if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) {
+		_px4_accel.set_temperature(sensors.temperature);
+		_px4_accel.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
 	}
 
 	// magnetometer
-	if (!_param_sim_mag_block.get()) {
-		_px4_mag.set_temperature(imu.temperature);
-		_px4_mag.update(time, imu.xmag, imu.ymag, imu.zmag);
+	if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) {
+		_px4_mag.set_temperature(sensors.temperature);
+		_px4_mag.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
 	}
 
 	// baro
-	if (!_param_sim_baro_block.get()) {
-		_px4_baro.set_temperature(imu.temperature);
-		_px4_baro.update(time, imu.abs_pressure);
+	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_param_sim_baro_block.get()) {
+		_px4_baro.set_temperature(sensors.temperature);
+		_px4_baro.update(time, sensors.abs_pressure);
 	}
 
 	// differential pressure
-	if (!_param_sim_dpres_block.get()) {
+	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_param_sim_dpres_block.get()) {
 		differential_pressure_s report{};
 		report.timestamp = time;
-		report.temperature = imu.temperature;
-		report.differential_pressure_filtered_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar;
-		report.differential_pressure_raw_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.temperature = sensors.temperature;
+		report.differential_pressure_filtered_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.differential_pressure_raw_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar;
 
 		_differential_pressure_pub.publish(report);
 	}