diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 45c4bc0ef6..12e058d55f 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -639,6 +639,10 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) const Vector3f specific_force_B = R_E2B * _specific_force_E; const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); + // Fault injection: which IMU index (0-based), -1 means none + const int fault_imu = _sih_fault_imu.get() - 1; // param is 1-indexed, -1 means off + const float fault_vibe = _sih_fault_vibe.get(); + // Publish to all simulated IMUs with independent noise for (uint8_t i = 0; i < IMU_COUNT; i++) { Vector3f accel_noise; @@ -653,7 +657,13 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f); } - const Vector3f accel = specific_force_B + accel_noise; + Vector3f accel = specific_force_B + accel_noise; + + // Inject high-amplitude Z-axis vibration on the selected IMU + if ((int)i == fault_imu && fault_vibe > FLT_EPSILON) { + accel(2) += fault_vibe * generate_wgn(); + } + const Vector3f gyro = _w_B + earth_spin_rate_B + gyro_noise; _px4_accel[i].update(time_now_us, accel(0), accel(1), accel(2)); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 780bb4a75f..bcd3007f44 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -353,6 +353,9 @@ private: (ParamInt) _sih_vtype, (ParamFloat) _sih_wind_n, (ParamFloat) _sih_wind_e, - (ParamFloat) _sih_ranging_beacon_noise + (ParamFloat) _sih_ranging_beacon_noise, + // fault injection + (ParamInt) _sih_fault_imu, + (ParamFloat) _sih_fault_vibe ) }; diff --git a/src/modules/simulation/simulator_sih/sih_params.yaml b/src/modules/simulation/simulator_sih/sih_params.yaml index c405274fb2..3c6a65e814 100644 --- a/src/modules/simulation/simulator_sih/sih_params.yaml +++ b/src/modules/simulation/simulator_sih/sih_params.yaml @@ -369,3 +369,28 @@ parameters: default: 6000.0 min: 0.1 decimal: 1 + SIH_FAULT_IMU: + description: + short: IMU index for fault injection + long: |- + Selects which simulated IMU receives injected faults. + 0 disables fault injection. 1 selects IMU 0, 2 selects IMU 1. + Can be changed at runtime to start/stop faults mid-flight. + type: int32 + default: 0 + min: 0 + max: 2 + SIH_FAULT_VIBE: + description: + short: Accel vibration amplitude for fault injection + long: |- + Adds random high-amplitude noise to the Z-axis of the selected IMU. + Values above ~155 m/s^2 will trigger accelerometer clipping detection, + which in turn triggers the EKF bad_acc_clipping fault flag. + Set to 0 to disable vibration injection even when SIH_FAULT_IMU is set. + type: float + default: 0.0 + unit: m/s^2 + min: 0.0 + max: 500.0 + decimal: 1