diff --git a/src/modules/simulation/sensor_baro_sim/CMakeLists.txt b/src/modules/simulation/sensor_baro_sim/CMakeLists.txt index 0a4a0394e5e..1e13d230c90 100644 --- 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 ) diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index ffdcb59a0b9..6add696dfe9 100644 --- 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); diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index 4e12c2dbfbc..9dfa1a4d01a 100644 --- 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 #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include #include #include @@ -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}; diff --git a/src/modules/simulation/sensor_mag_sim/CMakeLists.txt b/src/modules/simulation/sensor_mag_sim/CMakeLists.txt index 664a3082f71..e9fa21d2902 100644 --- 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 ) diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp index 61ecb6963bc..209759bc2ca 100644 --- 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)); } } diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index f4613f6f0ea..c9dae9284da 100644 --- 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 +#include #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -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{}; diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 1b9d3e56272..1d924c20287 100644 --- 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 ) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index b933ce6d04f..79705f632e6 100644 --- 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) { diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 9c6def293d3..4549a6f1f44 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -74,7 +74,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -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;