sensors: add 'sensors start -hil' parameter

This does the following if given:
- don't initialize the sensors (the sensor drivers are not started)
- publish sensor_combined even in hil mode
- do not apply or publish thermal corrections

src/modules/sensors/sensors.cpp

@@ -140,7 +140,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	Sensors();
+	Sensors(bool hil_enabled);
 
 	/**
 	 * Destructor, also kills the sensors task.
@@ -165,8 +165,7 @@ private:
 	bool 		_task_should_exit;		/**< if true, sensor task should exit */
 	int 		_sensors_task;			/**< task handle for sensor task */
 
-	bool		_hil_enabled;			/**< if true, HIL is active */
-	bool		_publishing;			/**< if true, we are publishing sensor data (in HIL mode, we don't) */
+	const bool	_hil_enabled;			/**< if true, HIL is active */
 	bool		_armed;				/**< arming status of the vehicle */
 
 	int		_actuator_ctrl_0_sub;		/**< attitude controls sub */
@@ -250,14 +249,13 @@ namespace sensors
 Sensors	*g_sensors = nullptr;
 }
 
-Sensors::Sensors() :
+Sensors::Sensors(bool hil_enabled) :
 	_h_adc(),
 	_last_adc(0),
 
 	_task_should_exit(true),
 	_sensors_task(-1),
-	_hil_enabled(false),
-	_publishing(true),
+	_hil_enabled(hil_enabled),
 	_armed(false),
 
 	_actuator_ctrl_0_sub(-1),
@@ -277,7 +275,7 @@ Sensors::Sensors() :
 	_airspeed_validator(),
 
 	_rc_update(_parameters),
-	_voted_sensors_update(_parameters)
+	_voted_sensors_update(_parameters, hil_enabled)
 {
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
 	memset(&_parameters, 0, sizeof(_parameters));
@@ -412,25 +410,12 @@ Sensors::vehicle_control_mode_poll()
 	struct vehicle_control_mode_s vcontrol_mode;
 	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle control mode has changed */
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
 
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 		_armed = vcontrol_mode.flag_armed;
-
-		/* switching from non-HIL to HIL mode */
-		if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
-			_hil_enabled = true;
-			_publishing = false;
-
-			/* switching from HIL to non-HIL mode */
-
-		} else if (!_publishing && !_hil_enabled) {
-			_hil_enabled = false;
-			_publishing = true;
-		}
 	}
 }
 
@@ -473,8 +458,8 @@ Sensors::parameter_update_poll(bool forced)
 void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
-	/* only read if publishing */
-	if (!_publishing) {
+	/* only read if not in HIL mode */
+	if (_hil_enabled) {
 		return;
 	}
 
@@ -569,13 +554,15 @@ Sensors::task_main()
 	/* start individual sensors */
 	int ret = 0;
 
-	/* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */
-	ret = sensors_init();
+	if (!_hil_enabled) {
+		/* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */
+		ret = sensors_init();
 
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
-	// TODO: move adc_init into the sensors_init call.
-	ret = ret || adc_init();
+		// TODO: move adc_init into the sensors_init call.
+		ret = ret || adc_init();
 #endif
+	}
 
 	if (ret) {
 		PX4_ERR("sensor initialization failed");
@@ -670,7 +657,7 @@ Sensors::task_main()
 
 		diff_pres_poll(raw);
 
-		if (_publishing && raw.timestamp > 0) {
+		if (raw.timestamp > 0) {
 
 			_voted_sensors_update.set_relative_timestamps(raw);
 
@@ -771,7 +758,13 @@ int sensors_main(int argc, char *argv[])
 			return 0;
 		}
 
-		sensors::g_sensors = new Sensors;
+		bool hil_enabled = false;
+
+		if (argc > 2 && !strcmp(argv[2], "-hil")) {
+			hil_enabled = true;
+		}
+
+		sensors::g_sensors = new Sensors(hil_enabled);
 
 		if (sensors::g_sensors == nullptr) {
 			PX4_ERR("alloc failed");

src/modules/sensors/voted_sensors_update.cpp

@@ -53,8 +53,8 @@ using namespace DriverFramework;
 
 const double VotedSensorsUpdate::_msl_pressure = 101.325f;
 
-VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters)
-	: _parameters(parameters)
+VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
+	: _parameters(parameters), _hil_enabled(hil_enabled)
 {
 	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
 	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
@@ -579,9 +579,11 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 			}
 
 			// handle temperature compensation
-			if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
-					offsets[uorb_index], scales[uorb_index]) == 2) {
-				_corrections_changed = true;
+			if (!_hil_enabled) {
+				if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
+						offsets[uorb_index], scales[uorb_index]) == 2) {
+					_corrections_changed = true;
+				}
 			}
 
 			// rotate corrected measurements from sensor to body frame
@@ -677,9 +679,11 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 			}
 
 			// handle temperature compensation
-			if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
-					offsets[uorb_index], scales[uorb_index]) == 2) {
-				_corrections_changed = true;
+			if (!_hil_enabled) {
+				if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
+						offsets[uorb_index], scales[uorb_index]) == 2) {
+					_corrections_changed = true;
+				}
 			}
 
 			// rotate corrected measurements from sensor to body frame
@@ -785,9 +789,11 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 			float corrected_pressure = 100.0f * baro_report.pressure;
 
 			// handle temperature compensation
-			if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
-					offsets[uorb_index], scales[uorb_index]) == 2) {
-				_corrections_changed = true;
+			if (!_hil_enabled) {
+				if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
+						offsets[uorb_index], scales[uorb_index]) == 2) {
+					_corrections_changed = true;
+				}
 			}
 
 			// First publication with data
@@ -1013,7 +1019,7 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw)
 	baro_poll(raw);
 
 	// publish sensor corrections if necessary
-	if (_corrections_changed) {
+	if (!_hil_enabled && _corrections_changed) {
 		_corrections.timestamp = hrt_absolute_time();
 
 		if (_sensor_correction_pub == nullptr) {

src/modules/sensors/voted_sensors_update.h

@@ -76,7 +76,7 @@ public:
 	 * @param parameters parameter values. These do not have to be initialized when constructing this object.
 	 * Only when calling init(), they have to be initialized.
 	 */
-	VotedSensorsUpdate(const Parameters &parameters);
+	VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled);
 
 	/**
 	 * initialize subscriptions etc.
@@ -259,6 +259,7 @@ private:
 	math::Matrix<3, 3>	_mag_rotation[SENSOR_COUNT_MAX] = {};	/**< rotation matrix for the orientation that the external mag0 is mounted */
 
 	const Parameters &_parameters;
+	const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */
 
 	float _accel_diff[3][2];	/**< filtered accel differences between IMU units (m/s/s) */
 	float _gyro_diff[3][2];		/**< filtered gyro differences between IMU uinits (rad/s) */