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
This commit is contained in:
Mathéo Taillandier authored and GitHub committed 2026-11-19 12:58:23 +01:00
1 parent 5729cf1e3d
commit e1e1122af5
9 files changed
+265 -74

No files matched your search

+1 -1
View File
@@ -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` <Badge type="tip" text="main (PX4 v2.0)" /> 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.
@@ -37,6 +37,8 @@
#include <cstdlib>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <parameters/param.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_gnss.h>
@@ -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;
@@ -52,6 +52,7 @@
#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_injection.h>
@@ -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<sensor_gnss_s> &stuck);
/** Per-consumer airspeed failure state: the last good sample for Stuck and the blockage start for Wrong. */
struct AirspeedState {
Stuck<differential_pressure_s> 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<typename MsgT>
struct Stuck {
};
struct AirspeedState {
};
template<typename MsgT>
bool process(Mode, MsgT &, Stuck<MsgT> &) { 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<sensor_gnss_s> &) { 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 {}; }
@@ -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
// ===========================================================================
@@ -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<float>(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)
@@ -188,6 +188,7 @@ private:
failure_injection::Config _failure_config;
failure_injection::Stuck<sensor_gnss_s> _gnss_stuck[_MAX_GNSS_SENSORS];
failure_injection::Stuck<vehicle_odometry_s> _visual_odometry_stuck{};
failure_injection::AirspeedState _airspeed_failure;
GZMixingInterfaceESC _mixing_interface_esc{_node};
GZMixingInterfaceServo _mixing_interface_servo{_node};
@@ -37,6 +37,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/failure_injection.h>
#include <uORB/topics/sensor_gnss.h>
#include <uORB/topics/vehicle_odometry.h>
@@ -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_injection_s> 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);
}
@@ -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();
@@ -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")};
};