From e1e1122af5bd46e43a143a225608b6793e2c98fc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Math=C3=A9o=20Taillandier?=
<123255318+Matheo-13@users.noreply.github.com>
Date: Thu, 19 Nov 2026 12:58:23 +0100
Subject: [PATCH] feat(simulation): enable airspeed sensor failure injection on
gazebo too (#29210)
* feat(simulation): enable failure injection on gazebo sensor too
* docs(failure_injection): remove added dot
---
docs/en/debug/failure_injection.md | 2 +-
.../failure_injection/FailureInjection.cpp | 36 ++++++
.../failure_injection/FailureInjection.hpp | 24 ++++
.../FailureInjectionConfigTest.cpp | 107 ++++++++++++++++++
src/modules/simulation/gz_bridge/GZBridge.cpp | 6 +-
src/modules/simulation/gz_bridge/GZBridge.hpp | 1 +
.../simulation/gz_bridge/GZBridgeTest.cpp | 59 ++++++++++
.../sensor_airspeed_sim/SensorAirspeedSim.cpp | 97 +++++-----------
.../sensor_airspeed_sim/SensorAirspeedSim.hpp | 7 +-
9 files changed, 265 insertions(+), 74 deletions(-)
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")};
};