diff --git a/docs/en/debug/failure_injection.md b/docs/en/debug/failure_injection.md index 9b740f6f8a8..8fdb1e81fcb 100644 --- a/docs/en/debug/failure_injection.md +++ b/docs/en/debug/failure_injection.md @@ -37,7 +37,7 @@ A `—` means the module still accepts the command, but no consumer applies it i - `gps off | stuck | wrong` on Gazebo (Gz): applied to the GNSS data from the simulator's own sensor as well as to the simulated-GPS module ([SIM_GZ_EN_GPS](../advanced_config/parameter_reference.md#SIM_GZ_EN_GPS) `0`). - `gyro wrong | garbage | drift` and `accel wrong | garbage | drift` corrupt the addressed sensor's samples on all three axes: `wrong` adds a constant bias ([SYS_FAIL_G_BIAS](../advanced_config/parameter_reference.md#SYS_FAIL_G_BIAS) / [SYS_FAIL_A_BIAS](../advanced_config/parameter_reference.md#SYS_FAIL_A_BIAS)), `garbage` adds uniform noise within ±[SYS_FAIL_G_NOISE](../advanced_config/parameter_reference.md#SYS_FAIL_G_NOISE) / ±[SYS_FAIL_A_NOISE](../advanced_config/parameter_reference.md#SYS_FAIL_A_NOISE), and `drift` adds a bias that grows by [SYS_FAIL_G_DRIFT](../advanced_config/parameter_reference.md#SYS_FAIL_G_DRIFT) / [SYS_FAIL_A_DRIFT](../advanced_config/parameter_reference.md#SYS_FAIL_A_DRIFT) per second since injection. -- `airspeed off | stuck | wrong` on Gazebo (Gz): only injectable when airspeed is provided by the simulated-airspeed module ([SENS_EN_ARSPDSIM](../advanced_config/parameter_reference.md#SENS_EN_ARSPDSIM)); worlds that model an airspeed sensor directly are not injected. +- `airspeed off | stuck | wrong` on Gazebo (Gz): applied to the differential pressure data from the simulator's own sensor as well as to the simulated-airspeed module ([SIM_GZ_EN_ASPD](../advanced_config/parameter_reference.md#SIM_GZ_EN_ASPD) `0`). - `vio off | stuck` on Gazebo (Gz): applied to the external-vision odometry that the bridge publishes from the model's odometry topic (for example `x500_vision`); `stuck` keeps publishing the last good sample with fresh timestamps. - `battery wrong` reports the remaining charge just below the [SYS_FAIL_BAT_LVL](../advanced_config/parameter_reference.md#SYS_FAIL_BAT_LVL) warning threshold to trigger the battery failsafe; `off` stops publishing the battery status entirely. - `traffic off` suppresses incoming reports and marks the ADS-B/FLARM link unhealthy. diff --git a/src/lib/failure_injection/FailureInjection.cpp b/src/lib/failure_injection/FailureInjection.cpp index 735190105ed..2d435a47222 100644 --- a/src/lib/failure_injection/FailureInjection.cpp +++ b/src/lib/failure_injection/FailureInjection.cpp @@ -37,6 +37,8 @@ #include +#include +#include #include #include #include @@ -167,6 +169,40 @@ bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gnss_s &se return true; } +bool process_airspeed(const Config &config, uint8_t uorb_instance, differential_pressure_s &differential_pressure, + AirspeedState &state) +{ + using namespace time_literals; + + static constexpr float blockage_fraction = 0.7f; // max blockage (fully ramped) + static constexpr float blockage_rampup_time = 30_s; // time to reach max blockage, linear ramp + + const Mode mode = config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, uorb_instance + 1); + + if (mode != Mode::Wrong) { + state.blocked_timestamp = 0; + } + + // Off and Stuck are message-agnostic; run them first so the Stuck cache keeps the + // unblocked sample. + if (!process(mode, differential_pressure, state.stuck)) { + return false; + } + + if (mode == Mode::Wrong) { + // Simulate pitot blockage: start the ramp on the rising edge and keep it running. + if (state.blocked_timestamp == 0) { + state.blocked_timestamp = differential_pressure.timestamp; + } + + const float blockage_scale = math::constrain(1.f - (differential_pressure.timestamp - state.blocked_timestamp) / + blockage_rampup_time, 1.f - blockage_fraction, 1.f); + differential_pressure.differential_pressure_pa *= blockage_scale; + } + + return true; +} + esc_status_s process_esc(const Config &config, const esc_status_s &status) { esc_status_s result = status; diff --git a/src/lib/failure_injection/FailureInjection.hpp b/src/lib/failure_injection/FailureInjection.hpp index 8d73b2a3224..c73ef416e8c 100644 --- a/src/lib/failure_injection/FailureInjection.hpp +++ b/src/lib/failure_injection/FailureInjection.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -222,6 +223,24 @@ bool process_battery(const Config &config, uint8_t instance, battery_status_s &b bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gnss_s &sensor_gnss, Stuck &stuck); +/** Per-consumer airspeed failure state: the last good sample for Stuck and the blockage start for Wrong. */ +struct AirspeedState { + Stuck stuck; + uint64_t blocked_timestamp{0}; +}; + +/** + * Airspeed counterpart to process(): on FAILURE_UNIT_SENSOR_AIRSPEED for the sensor publishing on + * the given 0-based uORB instance, Off and Stuck behave as in the generic process() and Wrong + * simulates a pitot blockage, ramping the differential pressure down linearly from the onset of + * the failure (taken from differential_pressure.timestamp) to a fixed fraction of the true value. + * + * @param uorb_instance 0-based uORB instance of the publisher (not the 1-based failure instance). + * @return false if the differential_pressure publication must be suppressed (Off), true otherwise. + */ +bool process_airspeed(const Config &config, uint8_t uorb_instance, differential_pressure_s &differential_pressure, + AirspeedState &state); + /** * ESC counterpart to process(): apply the active FAILURE_UNIT_SYSTEM_ESC failures to a copy of * status (matched per ESC by actuator_function). Off zeroes the ESC's telemetry (keeping only @@ -327,6 +346,9 @@ template struct Stuck { }; +struct AirspeedState { +}; + template bool process(Mode, MsgT &, Stuck &) { return true; } @@ -341,6 +363,8 @@ inline bool process_battery(const Config &, uint8_t, battery_status_s &) { retur inline bool process_gnss(const Config &, uint8_t, sensor_gnss_s &, Stuck &) { return true; } +inline bool process_airspeed(const Config &, uint8_t, differential_pressure_s &, AirspeedState &) { return true; } + inline esc_status_s process_esc(const Config &, const esc_status_s &status) { return status; } inline MotorFailureMasks process_motor(const Config &) { return {}; } diff --git a/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp b/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp index 32a0f352781..34147a4bb31 100644 --- a/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp +++ b/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp @@ -103,6 +103,7 @@ sensor_gnss_s clean_gnss() constexpr uint8_t GYRO = failure_injection_s::FAILURE_UNIT_SENSOR_GYRO; constexpr uint8_t GPS = failure_injection_s::FAILURE_UNIT_SENSOR_GPS; +constexpr uint8_t AIRSPEED = failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED; constexpr uint8_t MOTOR = failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR; constexpr uint8_t ESC = failure_injection_s::FAILURE_UNIT_SYSTEM_ESC; constexpr uint8_t BATTERY = failure_injection_s::FAILURE_UNIT_SYSTEM_BATTERY; @@ -447,6 +448,112 @@ TEST(FailureInjectionConfig, ProcessGnssRecoveryUsesLiveSample) EXPECT_DOUBLE_EQ(recovered.latitude, 48.0); } +// =========================================================================== +// process_airspeed(): Off / Stuck / Wrong (pitot blockage ramp) +// =========================================================================== + +namespace +{ + +differential_pressure_s make_differential_pressure(uint64_t timestamp, float differential_pressure_pa) +{ + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp = timestamp; + differential_pressure.timestamp_sample = timestamp; + differential_pressure.differential_pressure_pa = differential_pressure_pa; + return differential_pressure; +} + +} // namespace + +TEST(FailureInjectionConfig, ProcessAirspeedWrongRampsDownToBlockageFloor) +{ + Config config; + config.set(make_config(AIRSPEED, 0x1, WRONG)); + + AirspeedState state; + const uint64_t onset = 1'000'000; + + // The ramp starts at the first Wrong sample: no blockage yet. + differential_pressure_s differential_pressure = make_differential_pressure(onset, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_FLOAT_EQ(differential_pressure.differential_pressure_pa, 100.f); + + // Halfway through the 30 s ramp the pressure is linearly scaled. + differential_pressure = make_differential_pressure(onset + 15'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_NEAR(differential_pressure.differential_pressure_pa, 50.f, 1e-3f); + + // Past the ramp it holds at 30 % of the true value (70 % blockage). + differential_pressure = make_differential_pressure(onset + 60'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_NEAR(differential_pressure.differential_pressure_pa, 30.f, 1e-3f); +} + +TEST(FailureInjectionConfig, ProcessAirspeedClearingWrongRestartsRamp) +{ + Config config; + config.set(make_config(AIRSPEED, 0x1, WRONG)); + + AirspeedState state; + differential_pressure_s differential_pressure = make_differential_pressure(1'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + + config.set(failure_injection_s{}); + differential_pressure = make_differential_pressure(61'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_FLOAT_EQ(differential_pressure.differential_pressure_pa, 100.f); + + // A new Wrong starts a fresh ramp from no blockage. + config.set(make_config(AIRSPEED, 0x1, WRONG)); + differential_pressure = make_differential_pressure(62'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_FLOAT_EQ(differential_pressure.differential_pressure_pa, 100.f); +} + +TEST(FailureInjectionConfig, ProcessAirspeedWrongLeavesUnselectedInstanceUntouched) +{ + Config config; + config.set(make_config(AIRSPEED, 0x2, WRONG)); // failure instance 2 -> uORB instance 1 + + AirspeedState state; + differential_pressure_s differential_pressure = make_differential_pressure(1'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + + differential_pressure = make_differential_pressure(61'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_FLOAT_EQ(differential_pressure.differential_pressure_pa, 100.f); +} + +TEST(FailureInjectionConfig, ProcessAirspeedOffSuppressesPublication) +{ + Config config; + config.set(make_config(AIRSPEED, 0x1, OFF)); + + AirspeedState state; + differential_pressure_s differential_pressure = make_differential_pressure(1'000'000, 100.f); + + EXPECT_FALSE(process_airspeed(config, 0, differential_pressure, state)); + EXPECT_FLOAT_EQ(differential_pressure.differential_pressure_pa, 100.f); +} + +TEST(FailureInjectionConfig, ProcessAirspeedStuckReplaysLastGoodSample) +{ + Config config; + AirspeedState state; + + differential_pressure_s good = make_differential_pressure(1'000'000, 100.f); + EXPECT_TRUE(process_airspeed(config, 0, good, state)); + + config.set(make_config(AIRSPEED, 0x1, STUCK)); + + differential_pressure_s moved = make_differential_pressure(2'000'000, 250.f); + EXPECT_TRUE(process_airspeed(config, 0, moved, state)); + EXPECT_FLOAT_EQ(moved.differential_pressure_pa, 100.f); + EXPECT_EQ(moved.timestamp, 2'000'000u); + EXPECT_EQ(moved.timestamp_sample, 2'000'000u); +} + // =========================================================================== // process_esc(): ESC Off / Wrong on the multi-instance esc_status // =========================================================================== diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 9ef222dde8a..f460b658910 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -581,6 +581,7 @@ void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg, uint8_t i void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) { const uint64_t timestamp = hrt_absolute_time(); + _failure_config.update(); device::Device::DeviceId id{}; id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; @@ -596,7 +597,10 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) _temperature = static_cast(msg.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C report.temperature = _temperature; report.pitot_temperature = NAN; - _differential_pressure_pub.publish(report); + + if (failure_injection::process_airspeed(_failure_config, 0, report, _airspeed_failure)) { + _differential_pressure_pub.publish(report); + } } void GZBridge::imuCallback(const gz::msgs::IMU &msg, uint8_t instance_index) diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 9c979dc914d..d45f6061733 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -188,6 +188,7 @@ private: failure_injection::Config _failure_config; failure_injection::Stuck _gnss_stuck[_MAX_GNSS_SENSORS]; failure_injection::Stuck _visual_odometry_stuck{}; + failure_injection::AirspeedState _airspeed_failure; GZMixingInterfaceESC _mixing_interface_esc{_node}; GZMixingInterfaceServo _mixing_interface_servo{_node}; diff --git a/src/modules/simulation/gz_bridge/GZBridgeTest.cpp b/src/modules/simulation/gz_bridge/GZBridgeTest.cpp index dd583eafaa2..cf2168e87d9 100644 --- a/src/modules/simulation/gz_bridge/GZBridgeTest.cpp +++ b/src/modules/simulation/gz_bridge/GZBridgeTest.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,11 @@ public: { bridge.odometryCallback(message); } + + static void airspeedCallback(GZBridge &bridge, const gz::msgs::AirSpeed &message) + { + bridge.airspeedCallback(message); + } }; namespace @@ -88,6 +94,14 @@ gz::msgs::OdometryWithCovariance odometry(double east, double north, double up) return message; } +gz::msgs::AirSpeed airSpeed(double diff_pressure) +{ + gz::msgs::AirSpeed message; + message.set_diff_pressure(diff_pressure); + message.set_temperature(288.15); + return message; +} + failure_injection_s sensorFailure(uint8_t unit, uint8_t failure_type) { failure_injection_s config{}; @@ -113,6 +127,11 @@ failure_injection_s vioFailure(uint8_t failure_type) return sensorFailure(failure_injection_s::FAILURE_UNIT_SENSOR_VIO, failure_type); } +failure_injection_s airspeedFailure(uint8_t failure_type) +{ + return sensorFailure(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, failure_type); +} + } // namespace TEST(GZBridge, AppliesGpsFailureInjectionAtNavSatPublication) @@ -205,3 +224,43 @@ TEST(GZBridge, AppliesVioFailureInjectionAtOdometryPublication) EXPECT_FLOAT_EQ(recovered.position[1], 21.f); EXPECT_FLOAT_EQ(recovered.position[2], -23.f); } + +TEST(GZBridge, AppliesAirspeedFailureInjectionAtAirspeedPublication) +{ + GZBridge bridge{"default", "x500"}; + uORB::Publication failure_pub{ORB_ID(failure_injection)}; + uORB::Subscription differential_pressure_sub{ORB_ID(differential_pressure)}; + + GZBridgeTestPeer::airspeedCallback(bridge, airSpeed(100.0)); + + differential_pressure_s baseline{}; + ASSERT_TRUE(differential_pressure_sub.update(&baseline)); + EXPECT_FLOAT_EQ(baseline.differential_pressure_pa, 100.f); + + ASSERT_TRUE(failure_pub.publish(airspeedFailure(failure_injection_s::FAILURE_TYPE_STUCK))); + GZBridgeTestPeer::airspeedCallback(bridge, airSpeed(200.0)); + + differential_pressure_s stuck{}; + ASSERT_TRUE(differential_pressure_sub.update(&stuck)); + EXPECT_FLOAT_EQ(stuck.differential_pressure_pa, baseline.differential_pressure_pa); + + // Wrong starts the pitot blockage ramp, so the first sample is still unscaled + // (the ramp itself is covered by the process_airspeed() tests). + ASSERT_TRUE(failure_pub.publish(airspeedFailure(failure_injection_s::FAILURE_TYPE_WRONG))); + GZBridgeTestPeer::airspeedCallback(bridge, airSpeed(200.0)); + + differential_pressure_s wrong{}; + ASSERT_TRUE(differential_pressure_sub.update(&wrong)); + EXPECT_FLOAT_EQ(wrong.differential_pressure_pa, 200.f); + + ASSERT_TRUE(failure_pub.publish(airspeedFailure(failure_injection_s::FAILURE_TYPE_OFF))); + GZBridgeTestPeer::airspeedCallback(bridge, airSpeed(200.0)); + EXPECT_FALSE(differential_pressure_sub.updated()); + + ASSERT_TRUE(failure_pub.publish(airspeedFailure(failure_injection_s::FAILURE_TYPE_OK))); + GZBridgeTestPeer::airspeedCallback(bridge, airSpeed(200.0)); + + differential_pressure_s recovered{}; + ASSERT_TRUE(differential_pressure_sub.update(&recovered)); + EXPECT_FLOAT_EQ(recovered.differential_pressure_pa, 200.f); +} diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index be9999f5ff2..65c3f6db349 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -80,61 +80,46 @@ void SensorAirspeedSim::Run() if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated() && _vehicle_attitude_sub.updated()) { - updateFailureConfig(); + _failure_config.update(); - if (!_airspeed_disconnected) { + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); - vehicle_local_position_s lpos{}; - _vehicle_local_position_sub.copy(&lpos); + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); - vehicle_global_position_s gpos{}; - _vehicle_global_position_sub.copy(&gpos); + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); - vehicle_attitude_s attitude{}; - _vehicle_attitude_sub.copy(&attitude); + Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; + Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; - Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; - Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; + // device id + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; - // device id - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + const float alt_amsl = gpos.alt; + const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; + const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); + const float air_density = AIR_DENSITY_MSL / density_ratio; - const float alt_amsl = gpos.alt; - const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; - const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); - const float air_density = AIR_DENSITY_MSL / density_ratio; + // calculate differential pressure + noise in hPa + const float diff_pressure_noise = (float)math::generate_wgn() * 0.01f; + float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( + 0) + diff_pressure_noise; - if (!_airspeed_stuck) { - // calculate differential pressure + noise in hPa - const float diff_pressure_noise = (float)math::generate_wgn() * 0.01f; - float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( - 0) + diff_pressure_noise; + differential_pressure_s differential_pressure{}; + // report.timestamp_sample = time; + differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION + differential_pressure.differential_pressure_pa = diff_pressure * 100.0f; // hPa to Pa + differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C + differential_pressure.pitot_temperature = NAN; + differential_pressure.timestamp = hrt_absolute_time(); - // simulate pitot blockage: ramp down differential pressure over time - const float blockage_fraction = 0.7f; // max blockage (fully ramped) - const float airspeed_blockage_rampup_time = 30_s; - - float airspeed_blockage_scale = 1.f; - - if (_airspeed_blocked_timestamp > 0) { - airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) / - airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f); - } - - _last_differential_pressure_pa = diff_pressure * 100.0f * airspeed_blockage_scale; // hPa to Pa - } - - differential_pressure_s differential_pressure{}; - // report.timestamp_sample = time; - differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - differential_pressure.differential_pressure_pa = _last_differential_pressure_pa; - differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C - differential_pressure.pitot_temperature = NAN; - differential_pressure.timestamp = hrt_absolute_time(); + if (failure_injection::process_airspeed(_failure_config, 0, differential_pressure, _airspeed_failure)) { _differential_pressure_pub.publish(differential_pressure); } } @@ -142,26 +127,6 @@ void SensorAirspeedSim::Run() perf_end(_loop_perf); } -void SensorAirspeedSim::updateFailureConfig() -{ - _failure_config.update(); - - const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, 1); - - _airspeed_disconnected = (mode == failure_injection::Mode::Off); - _airspeed_stuck = (mode == failure_injection::Mode::Stuck); - - if (mode == failure_injection::Mode::Wrong) { - // Simulate pitot blockage: start the ramp on the rising edge and keep it running. - if (_airspeed_blocked_timestamp == 0) { - _airspeed_blocked_timestamp = hrt_absolute_time(); - } - - } else { - _airspeed_blocked_timestamp = 0; - } -} - int SensorAirspeedSim::task_spawn(int argc, char *argv[]) { SensorAirspeedSim *instance = new SensorAirspeedSim(); diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index 448ba42c6e2..daa742c21bf 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -80,8 +80,6 @@ public: bool init(); - void updateFailureConfig(); - private: void Run() override; @@ -94,10 +92,7 @@ private: failure_injection::Config _failure_config; - bool _airspeed_disconnected{false}; - bool _airspeed_stuck{false}; - hrt_abstime _airspeed_blocked_timestamp{0}; - float _last_differential_pressure_pa{0.f}; + failure_injection::AirspeedState _airspeed_failure; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; };