px4_platform_common: add bitmask.h for templated type safe bitmask operations

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
+
+#ifdef __cplusplus
+
+#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 */

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:
@@ -247,15 +276,6 @@ private:
 	};
 #endif
 
-	///! Enumeration to use on the bitmask in HIL_SENSOR
-	enum SensorSource {
-		ACCEL		= 0x0007,
-		GYRO		= 0x0038,
-		MAG		= 0x01C0,
-		BARO		= 0x1A00,
-		DIFF_PRESS	= 0x0400
-	};
-
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SIM_BAT_DRAIN>) _param_sim_bat_drain, ///< battery drain interval
 		(ParamFloat<px4::params::SIM_BAT_MIN_PCT>) _battery_min_percentage, //< minimum battery percentage