feat(simulation): implement required failure hooks

2026-07-24 15:27:41 +08:00 · 2026-01-24 16:52:50 +13:00
parent cf8a65c5dd
commit 69d17b14aa
9 changed files with 123 additions and 5 deletions
--- a/src/modules/simulation/sensor_baro_sim/CMakeLists.txt
+++ b/src/modules/simulation/sensor_baro_sim/CMakeLists.txt
@@ -42,6 +42,7 @@ px4_add_module(
 		parameters.yaml
 	DEPENDS
 		drivers_barometer
+		failure_injection
 		geo
 		px4_work_queue
 	)
--- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp
+++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp
@@ -88,6 +88,16 @@ float SensorBaroSim::generate_wgn()
 	return X;
 }

+void SensorBaroSim::updateFailureConfig()
+{
+	_failure_config.update();
+
+	const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_BARO, 1);
+
+	_baro_blocked = (mode == failure_injection::Mode::Off);
+	_baro_stuck = (mode == failure_injection::Mode::Stuck);
+}
+
 void SensorBaroSim::Run()
 {
 	if (should_exit()) {
@@ -107,6 +117,22 @@ void SensorBaroSim::Run()
 		updateParams();
 	}

+	updateFailureConfig();
+
+	if (_baro_blocked) {
+		perf_end(_loop_perf);
+		return;
+	}
+
+	if (_baro_stuck) {
+		// Publish stuck (last known) values
+		_px4_baro.set_temperature(_last_temperature);
+		_px4_baro.update(hrt_absolute_time(), _last_pressure);
+
+		perf_end(_loop_perf);
+		return;
+	}
+
 	if (_vehicle_global_position_sub.updated()) {
 		vehicle_global_position_s gpos;

@@ -136,6 +162,10 @@ void SensorBaroSim::Run()
 			// calculate temperature in Celsius
 			float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();

+			// Store values for stuck mode
+			_last_pressure = pressure;
+			_last_temperature = temperature;
+
 			// publish
 			_px4_baro.set_temperature(temperature);
 			_px4_baro.update(gpos.timestamp, pressure);
--- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp
+++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp
@@ -33,6 +33,7 @@

 #pragma once

+#include <lib/failure_injection/FailureInjection.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -42,6 +43,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/failure_injection.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>

@@ -68,6 +70,7 @@ public:

 private:
 	void Run() override;
+	void updateFailureConfig();

 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
@@ -75,8 +78,14 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};

+	failure_injection::Config _failure_config;
+
+	bool _baro_blocked{false};
+	bool _baro_stuck{false};
 	float _baro_drift_pa_per_sec{0.0};
 	float _baro_drift_pa{0.0};
+	float _last_pressure{0.0f};
+	float _last_temperature{0.0f};

 	hrt_abstime _last_update_time{0};

--- a/src/modules/simulation/sensor_mag_sim/CMakeLists.txt
+++ b/src/modules/simulation/sensor_mag_sim/CMakeLists.txt
@@ -42,6 +42,7 @@ px4_add_module(
 		parameters.yaml
 	DEPENDS
 		drivers_magnetometer
+		failure_injection
 		geo
 		px4_work_queue
 	)
--- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp
+++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp
@@ -89,6 +89,16 @@ float SensorMagSim::generate_wgn()
 	return X;
 }

+void SensorMagSim::updateFailureConfig()
+{
+	_failure_config.update();
+
+	const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, 1);
+
+	_mag_blocked = (mode == failure_injection::Mode::Off);
+	_mag_stuck = (mode == failure_injection::Mode::Stuck);
+}
+
 void SensorMagSim::Run()
 {
 	if (should_exit()) {
@@ -108,6 +118,13 @@ void SensorMagSim::Run()
 		updateParams();
 	}

+	updateFailureConfig();
+
+	if (_mag_blocked) {
+		perf_end(_loop_perf);
+		return;
+	}
+
 	if (_vehicle_global_position_sub.updated()) {
 		vehicle_global_position_s gpos;

@@ -133,11 +150,23 @@ void SensorMagSim::Run()
 			Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred;

 			expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f);
+			expected_field += Vector3f(_sim_mag_offset_x.get(),
+						   _sim_mag_offset_y.get(),
+						   _sim_mag_offset_z.get());
+
+			if (_mag_stuck && _last_field_valid) {
+				// Replay the last field seen before the failure was injected
+				expected_field = _last_field;
+
+			} else {
+				_last_field = expected_field;
+				_last_field_valid = true;
+			}

 			_px4_mag.update(attitude.timestamp,
-					expected_field(0) + _sim_mag_offset_x.get(),
-					expected_field(1) + _sim_mag_offset_y.get(),
-					expected_field(2) + _sim_mag_offset_z.get());
+					expected_field(0),
+					expected_field(1),
+					expected_field(2));
 		}
 	}

--- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp
+++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp
@@ -34,6 +34,7 @@
 #pragma once

 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <lib/failure_injection/FailureInjection.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -42,6 +43,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/failure_injection.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -69,6 +71,7 @@ public:

 private:
 	void Run() override;
+	void updateFailureConfig();

 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
@@ -79,9 +82,15 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
+	failure_injection::Config _failure_config;

 	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION

+	bool _mag_blocked{false};
+	bool _mag_stuck{false};
+	bool _last_field_valid{false};
+	matrix::Vector3f _last_field{};
+
 	bool _mag_earth_available{false};

 	matrix::Vector3f _mag_earth_pred{};
--- a/src/modules/simulation/simulator_sih/CMakeLists.txt
+++ b/src/modules/simulation/simulator_sih/CMakeLists.txt
@@ -47,6 +47,7 @@ px4_add_module(
 		drivers_accelerometer
 		drivers_gyroscope
 		drivers_rangefinder
+		failure_injection
 		geo
 	)

--- a/src/modules/simulation/simulator_sih/sih.cpp
+++ b/src/modules/simulation/simulator_sih/sih.cpp
@@ -191,6 +191,20 @@ void Sih::realtime_loop()
 	px4_sem_destroy(&_data_semaphore);
 }

+void Sih::updateFailureConfig()
+{
+	_failure_config.update();
+
+	_airspeed_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, 1)
+			     == failure_injection::Mode::Off);
+	_distance_sensor_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR, 1)
+				    == failure_injection::Mode::Off);
+	_accel_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL, 1)
+			  == failure_injection::Mode::Off);
+	_gyro_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GYRO, 1)
+			 == failure_injection::Mode::Off);
+}
+
 void Sih::sensor_step()
 {
 	// check for parameter updates
@@ -204,6 +218,8 @@ void Sih::sensor_step()
 		parameters_updated();
 	}

+	updateFailureConfig();
+
 	perf_begin(_loop_perf);

 	const hrt_abstime now = hrt_absolute_time();
@@ -715,12 +731,21 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us)
 	Vector3f gyro = _w_B + earth_spin_rate_B + gyro_noise;

 	// update IMU every iteration
-	_px4_accel.update(time_now_us, accel(0), accel(1), accel(2));
-	_px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2));
+	if (!_accel_blocked) {
+		_px4_accel.update(time_now_us, accel(0), accel(1), accel(2));
+	}
+
+	if (!_gyro_blocked) {
+		_px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2));
+	}
 }

 void Sih::send_airspeed(const hrt_abstime &time_now_us)
 {
+	if (_airspeed_blocked) {
+		return;
+	}
+
 	// TODO: send differential pressure instead?
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
@@ -736,6 +761,10 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)

 void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
 {
+	if (_distance_sensor_blocked) {
+		return;
+	}
+
 	float current_distance;

 	if (_distance_snsr_override >= 0.f) {
--- a/src/modules/simulation/simulator_sih/sih.hpp
+++ b/src/modules/simulation/simulator_sih/sih.hpp
@@ -74,7 +74,9 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <lib/failure_injection/FailureInjection.hpp>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/failure_injection.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -125,6 +127,7 @@ public:

 private:
 	void parameters_updated();
+	void updateFailureConfig();

 	// simulated sensors
 	PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
@@ -142,6 +145,12 @@ private:

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs_sim)};
+	failure_injection::Config _failure_config;
+
+	bool _airspeed_blocked{false};
+	bool _distance_sensor_blocked{false};
+	bool _accel_blocked{false};
+	bool _gyro_blocked{false};

 	// hard constants
 	static constexpr uint16_t NUM_ACTUATORS_MAX = 9;