Failure injection: refactor ok/off/stuck status and add msg timestamp…

… during stuck

src/modules/sensors/failure_detector_HITL/failure_detector_HITL.cpp

@@ -155,9 +155,14 @@ bool FailureDetectorHITL::update()
 	return update;
 }
 #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
-bool FailureDetectorHITL::isGpsBlocked() const
+bool FailureDetectorHITL::isGpsOk() const
 {
-	return FailureStatus::ok != _gps;
+	return FailureStatus::ok == _gps;
+}
+
+bool FailureDetectorHITL::isGpsOff() const
+{
+	return FailureStatus::off == _gps;
 }
 
 bool FailureDetectorHITL::isGpsStuck() const
@@ -167,9 +172,14 @@ bool FailureDetectorHITL::isGpsStuck() const
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
-bool FailureDetectorHITL::isBaroBlocked() const
+bool FailureDetectorHITL::isBaroOk() const
+{
+	return FailureStatus::ok == _baro;
+}
+
+bool FailureDetectorHITL::isBaroOff() const
 {
-	return FailureStatus::ok != _baro;
+	return FailureStatus::off == _baro;
 }
 
 bool FailureDetectorHITL::isBaroStuck() const
@@ -179,9 +189,14 @@ bool FailureDetectorHITL::isBaroStuck() const
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-bool FailureDetectorHITL::isMagBlocked() const
+bool FailureDetectorHITL::isMagOk() const
+{
+	return FailureStatus::ok == _mag;
+}
+
+bool FailureDetectorHITL::isMagOff() const
 {
-	return FailureStatus::ok != _mag;
+	return FailureStatus::off == _mag;
 }
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
@@ -210,13 +225,3 @@ void FakeSensors::update(const FailureDetectorHITL &detector)
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 }
 
-void FakeSensors::turnOffAll()
-{
-#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
-	_fake_gps_publisher.setEnabled(false);
-#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
-
-#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
-	_fake_baro_publisher.setEnabled(false);
-#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
-}

src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp

@@ -76,17 +76,20 @@ class FailureDetectorHITL final
 	bool update();
 
 #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
-	bool isGpsBlocked() const;
+	bool isGpsOk() const;
+	bool isGpsOff() const;
 	bool isGpsStuck() const;
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
-	bool isBaroBlocked() const;
+	bool isBaroOk() const;
+	bool isBaroOff() const;
 	bool isBaroStuck() const;
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-	bool isMagBlocked() const;
+	bool isMagOk() const;
+	bool isMagOff() const;
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
 private:
@@ -161,22 +164,19 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
 		Deinit();
 	}
 
-	void setEnabled(bool enable)
+	void setEnabled(bool enabled)
 	{
 		pthread_mutex_lock(&_mutex);
 
-		if (_enable && !enable) {
+		if (_enabled && !enabled) {
 			delete _sensor_pub;
 
-		} else if (!_enable && enable) {
+		} else if (!_enabled && enabled) {
 			_sensor_pub = new uORB::Publication<sensorsData>(meta_);
-			_sensor_pub->advertise();
 		}
 
-		if (_enable != enable) {
-			PX4_INFO("Fake stuck sensor %s was enabled with status %d ", meta_->o_name, enable);
-			_enable = enable;
-		}
+		PX4_INFO("Fake stuck sensor %s was enabled with status %d ", meta_->o_name, enabled);
+		_enabled = enabled;
 
 		pthread_mutex_unlock(&_mutex);
 	}
@@ -191,7 +191,8 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
 
 		pthread_mutex_lock(&_mutex);
 
-		if (_enable && _first_init) {
+		if (_enabled && _first_init) {
+			_last_output.timestamp = hrt_absolute_time();
 			_sensor_pub->publish(_last_output);
 		}
 
@@ -206,7 +207,7 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
 	uORB::Subscription _sensor_sub {};
 
 	const orb_metadata *meta_;
-	bool _enable{};
+	bool _enabled{};
 	bool _first_init{}; /**< Flag indicating whether the sensor has been initialized for the first time. */
 	sensorsData _last_output{};
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
@@ -230,12 +231,6 @@ class FakeSensors final
 	 * @brief Updates states of the fake sensors with data from the failure detector.
 	 */
 	void update(const FailureDetectorHITL &detector);
-
-	/**
-	 * @brief Turn off all sensors.
-	 * @note It is required doing befor run original sensors.
-	 */
-	void turnOffAll();
 private:
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)

src/modules/sensors/sensors.cpp

@@ -161,26 +161,12 @@ bool Sensors::init()
 
 int Sensors::parameters_update()
 {
-	if (_hil_enabled) {
-		_fakeSensors.turnOffAll();
-
-#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
-		InitializeVehicleGPSPosition();
-#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
-
-#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
-		InitializeVehicleAirData();
-#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
-
-#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-		InitializeVehicleMagnetometer();
-#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
-
-		_fakeSensors.update(_failureDetector);
+	if (_armed && !_failure_detector_updated) {
+		return 0;
 	}
 
-	if (_armed) {
-		return 0;
+	if (_failure_detector_updated) {
+		_fakeSensors.update(_failureDetector);
 	}
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
@@ -441,19 +427,19 @@ void Sensors::adc_poll()
 void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
-		if (_vehicle_air_data == nullptr && !_failureDetector.isBaroBlocked()) {
+		if (_vehicle_air_data == nullptr && _failureDetector.isBaroOk()) {
 			_vehicle_air_data = new VehicleAirData();
 
 			if (_vehicle_air_data) {
 				_vehicle_air_data->Start();
 			}
 		}
-	}
 
-	if (_vehicle_air_data && _failureDetector.isBaroBlocked()) {
-		_vehicle_air_data->Stop();
-		delete _vehicle_air_data;
-		_vehicle_air_data = nullptr;
+		if (_vehicle_air_data && !_failureDetector.isBaroOk()) {
+			_vehicle_air_data->Stop();
+			delete _vehicle_air_data;
+			_vehicle_air_data = nullptr;
+		}
 	}
 }
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
@@ -462,19 +448,19 @@ void Sensors::InitializeVehicleAirData()
 void Sensors::InitializeVehicleGPSPosition()
 {
 	if (_param_sys_has_gps.get()) {
-		if (_vehicle_gps_position == nullptr && !_failureDetector.isGpsBlocked()) {
+		if (_vehicle_gps_position == nullptr && _failureDetector.isGpsOk()) {
 			_vehicle_gps_position = new VehicleGPSPosition();
 
 			if (_vehicle_gps_position) {
 				_vehicle_gps_position->Start();
 			}
 		}
-	}
 
-	if (_vehicle_gps_position && _failureDetector.isGpsBlocked()) {
-		_vehicle_gps_position->Stop();
-		delete _vehicle_gps_position;
-		_vehicle_gps_position = nullptr;
+		if (_vehicle_gps_position && !_failureDetector.isGpsOk()) {
+			_vehicle_gps_position->Stop();
+			delete _vehicle_gps_position;
+			_vehicle_gps_position = nullptr;
+		}
 	}
 }
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
@@ -518,19 +504,19 @@ void Sensors::InitializeVehicleIMU()
 void Sensors::InitializeVehicleMagnetometer()
 {
 	if (_param_sys_has_mag.get()) {
-		if (_vehicle_magnetometer == nullptr && !_failureDetector.isMagBlocked()) {
+		if (_vehicle_magnetometer == nullptr && _failureDetector.isMagOk()) {
 			_vehicle_magnetometer = new VehicleMagnetometer();
 
 			if (_vehicle_magnetometer) {
 				_vehicle_magnetometer->Start();
 			}
 		}
-	}
 
-	if (_vehicle_magnetometer && _failureDetector.isMagBlocked()) {
-		_vehicle_magnetometer->Stop();
-		delete _vehicle_magnetometer;
-		_vehicle_magnetometer = nullptr;
+		if (_vehicle_magnetometer && !_failureDetector.isMagOk()) {
+			_vehicle_magnetometer->Stop();
+			delete _vehicle_magnetometer;
+			_vehicle_magnetometer = nullptr;
+		}
 	}
 }
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
@@ -651,8 +637,13 @@ void Sensors::Run()
 		}
 	}
 
-	if (_hil_enabled && _failureDetector.update()) {
-		parameters_update();
+	if (_hil_enabled) {
+		_failure_detector_updated = _failureDetector.update();
+
+		if (_failure_detector_updated) {
+			parameters_update();
+			_failure_detector_updated = false;
+		}
 	}
 
 	_voted_sensors_update.sensorsPoll(_sensor_combined);

src/modules/sensors/sensors.hpp

@@ -135,6 +135,8 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 
 	const bool _hil_enabled;	/**< if true, HIL is active */
 
+	bool _failure_detector_updated{};	/**< true if the failure detector has been updated */
+
 	perf_counter_t	_loop_perf;	/**< loop performance counter */
 
 	VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};