simulator: set sensor update rate according to HIL_SENSOR bitmask

platforms/common/include/px4_platform_common/bitmask.h

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bitmask.h
+ * @brief Provides SFINAE for type safe templated bitmask operations
+ * @author Nuno Marques <nuno.marques@dronesolutions.io>
+ *
+ */
+
+#pragma once
+
+#if defined(__cplusplus) && (defined(__PX4_POSIX) || defined(__PX4_LINUX))
+
+#include<type_traits>
+
+template<typename E>
+struct enable_bitmask_operators {
+	static const bool enable = false;
+};
+
+namespace px4
+{
+
+#define ENABLE_BIT_OPERATORS(E) \
+	template<> \
+	struct enable_bitmask_operators<E> \
+	{ \
+		static const bool enable = true; \
+	};
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
+operator==(E lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	return static_cast<E>(
+		       static_cast<underlying>(lhs) ==
+		       static_cast<underlying>(rhs)
+	       );
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
+operator~(E lhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	return static_cast<E>(
+		       ~static_cast<underlying>(lhs)
+	       );
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
+operator|(E lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	return static_cast<E>(
+		       static_cast<underlying>(lhs) |
+		       static_cast<underlying>(rhs)
+	       );
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E &>::type
+operator|=(E &lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	lhs = static_cast<E>(
+		      static_cast<underlying>(lhs) |
+		      static_cast<underlying>(rhs)
+	      );
+	return lhs;
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
+operator&(E lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	return static_cast<E>(
+		       static_cast<underlying>(lhs) &
+		       static_cast<underlying>(rhs)
+	       );
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E &>::type
+operator&=(E &lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	lhs = static_cast<E>(
+		      static_cast<underlying>(lhs) &
+		      static_cast<underlying>(rhs)
+	      );
+	return lhs;
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
+operator^(E lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	return static_cast<E>(
+		       static_cast<underlying>(lhs) ^
+		       static_cast<underlying>(rhs)
+	       );
+}
+
+template<typename E>
+typename std::enable_if<enable_bitmask_operators<E>::enable, E &>::type
+operator^=(E &lhs, E rhs)
+{
+	typedef typename std::underlying_type<E>::type underlying;
+	lhs = static_cast<E>(
+		      static_cast<underlying>(lhs) ^
+		      static_cast<underlying>(rhs)
+	      );
+	return lhs;
+}
+
+} /* namespace px4 */
+
+#endif /* __cplusplus && (__PX4_POSIX || __PX4_LINUX) */

src/modules/simulator/simulator.h

@@ -53,6 +53,7 @@
 #include <lib/ecl/geo/geo.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/atomic.h>
+#include <px4_platform_common/bitmask.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <uORB/Publication.hpp>
@@ -80,6 +81,34 @@
 #include <v2.0/mavlink_types.h>
 #include <lib/battery/battery.h>
 
+//! Enumeration to use on the bitmask in HIL_SENSOR
+enum class SensorSource {
+	ACCEL		= 0b111,
+	GYRO		= 0b111000,
+	MAG		= 0b111000000,
+	BARO		= 0b1101000000000,
+	DIFF_PRESS	= 0b10000000000
+};
+ENABLE_BIT_OPERATORS(SensorSource)
+
+//! AND operation for the enumeration and unsigned types that returns the bitmask
+template<typename A, typename B>
+static inline SensorSource operator &(A lhs, B rhs)
+{
+	// make it type safe
+	static_assert((std::is_same<A, uint32_t>::value || std::is_same<A, SensorSource>::value),
+		      "first argument is not uint32_t or SensorSource enum type");
+	static_assert((std::is_same<B, uint32_t>::value || std::is_same<B, SensorSource>::value),
+		      "second argument is not uint32_t or SensorSource enum type");
+
+	typedef typename std::underlying_type<SensorSource>::type underlying;
+
+	return static_cast<SensorSource>(
+		       static_cast<underlying>(lhs) &
+		       static_cast<underlying>(rhs)
+	       );
+}
+
 class Simulator : public ModuleParams
 {
 public:
@@ -198,7 +227,7 @@ class Simulator : public ModuleParams
 	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 *);
 

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);
 	}