Failure injection: pause / resume real sensor publishers and fake (st…

…uck) sensor publishers instead of stop / restart
tiiuae · Apr 23, 2024 · 5fd2c8c · 5fd2c8c
1 parent e8283d4
commit 5fd2c8c
Show file tree

Hide file tree

Showing 8 changed files with 49 additions and 37 deletions.
diff --git a/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp b/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp
@@ -139,11 +139,10 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
 	FakeStuckSensor(const orb_metadata *meta):
 		ModuleParams(nullptr),
 		ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+		_sensor_pub(meta),
 		_sensor_sub(meta),
 		meta_(meta)
 	{
-		pthread_mutex_init(&_mutex, nullptr);
-		PX4_INFO("Fake stuck sensor %s:id(%d) was created with ", meta->o_name, meta->o_id);
 	}
 
 	~FakeStuckSensor() override
@@ -166,19 +165,7 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
 
 	void setEnabled(bool enabled)
 	{
-		pthread_mutex_lock(&_mutex);
-
-		if (_enabled && !enabled) {
-			delete _sensor_pub;
-
-		} else if (!_enabled && enabled) {
-			_sensor_pub = new uORB::Publication<sensorsData>(meta_);
-		}
-
-		PX4_INFO("Fake stuck sensor %s was %s", meta_->o_name, enabled ? "enabled" : "disabled");
 		_enabled = enabled;
-
-		pthread_mutex_unlock(&_mutex);
 	}
 private:
 	void Run() override
@@ -189,29 +176,24 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
 			_first_init = true;
 		}
 
-		pthread_mutex_lock(&_mutex);
-
 		if (_enabled && _first_init) {
 			_last_output.timestamp = hrt_absolute_time();
-			_sensor_pub->publish(_last_output);
+			_sensor_pub.publish(_last_output);
 		}
 
-		pthread_mutex_unlock(&_mutex);
-
 		ScheduleDelayed(300_ms); // backup schedule
 
 		perf_end(_cycle_perf);
 	}
 
-	uORB::Publication<sensorsData> *_sensor_pub {};
+	uORB::Publication<sensorsData> _sensor_pub {};
 	uORB::Subscription _sensor_sub {};
 
 	const orb_metadata *meta_;
 	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")};
-	pthread_mutex_t	_mutex {};
 };
 
 }

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
@@ -433,12 +433,14 @@ void Sensors::InitializeVehicleAirData()
 			if (_vehicle_air_data) {
 				_vehicle_air_data->Start();
 			}
-		}
 
-		if (_vehicle_air_data && !_failureDetector.isBaroOk()) {
-			_vehicle_air_data->Stop();
-			delete _vehicle_air_data;
-			_vehicle_air_data = nullptr;
+		} else {
+			if (_failureDetector.isBaroOk()) {
+				_vehicle_air_data->Resume();
+
+			} else {
+				_vehicle_air_data->Pause();
+			}
 		}
 	}
 }
@@ -448,18 +450,20 @@ void Sensors::InitializeVehicleAirData()
 void Sensors::InitializeVehicleGPSPosition()
 {
 	if (_param_sys_has_gps.get()) {
-		if (_vehicle_gps_position == nullptr && _failureDetector.isGpsOk()) {
+		if (_vehicle_gps_position == nullptr) {
 			_vehicle_gps_position = new VehicleGPSPosition();
 
 			if (_vehicle_gps_position) {
 				_vehicle_gps_position->Start();
 			}
-		}
 
-		if (_vehicle_gps_position && !_failureDetector.isGpsOk()) {
-			_vehicle_gps_position->Stop();
-			delete _vehicle_gps_position;
-			_vehicle_gps_position = nullptr;
+		} else {
+			if (_failureDetector.isGpsOk()) {
+				_vehicle_gps_position->Resume();
+
+			} else {
+				_vehicle_gps_position->Pause();
+			}
 		}
 	}
 }
@@ -510,12 +514,14 @@ void Sensors::InitializeVehicleMagnetometer()
 			if (_vehicle_magnetometer) {
 				_vehicle_magnetometer->Start();
 			}
-		}
 
-		if (_vehicle_magnetometer && !_failureDetector.isMagOk()) {
-			_vehicle_magnetometer->Stop();
-			delete _vehicle_magnetometer;
-			_vehicle_magnetometer = nullptr;
+		} else {
+			if (_failureDetector.isMagOk()) {
+				_vehicle_magnetometer->Resume();
+
+			} else {
+				_vehicle_magnetometer->Pause();
+			}
 		}
 	}
 }

diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
@@ -131,6 +131,10 @@ bool VehicleAirData::ParametersUpdate(bool force)
 
 void VehicleAirData::Run()
 {
+	if (_paused) {
+		return;
+	}
+
 	perf_begin(_cycle_perf);
 
 	const hrt_abstime time_now_us = hrt_absolute_time();

diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
@@ -68,6 +68,8 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem
 
 	bool Start();
 	void Stop();
+	void Pause() { _paused = true; }
+	void Resume() { _paused = false; }
 
 	void PrintStatus();
 
@@ -125,6 +127,8 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem
 
 	float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature
 
+	bool _paused{false};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
 		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate

diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
@@ -101,6 +101,10 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
 
 void VehicleGPSPosition::Run()
 {
+	if (_paused) {
+		return;
+	}
+
 	perf_begin(_cycle_perf);
 	ParametersUpdate();
 

diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
@@ -61,6 +61,8 @@ class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem
 
 	bool Start();
 	void Stop();
+	void Pause() { _paused = true; }
+	void Resume() { _paused = false; }
 
 	void PrintStatus();
 
@@ -92,6 +94,8 @@ class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem
 
 	GpsBlending _gps_blending;
 
+	bool _paused{false};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
 		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,

diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
@@ -397,6 +397,10 @@ void VehicleMagnetometer::UpdatePowerCompensation()
 
 void VehicleMagnetometer::Run()
 {
+	if (_paused) {
+		return;
+	}
+
 	perf_begin(_cycle_perf);
 
 	const hrt_abstime time_now_us = hrt_absolute_time();

diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
@@ -74,6 +74,8 @@ class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
 
 	bool Start();
 	void Stop();
+	void Pause() { _paused = true; }
+	void Resume() { _paused = false; }
 
 	void PrintStatus();
 
@@ -173,6 +175,8 @@ class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
 
 	bool _armed{false};
 
+	bool _paused{false};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
 		(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,