From a41d35b57f7b9574d14168330c5a247d8d2a658c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 24 Jan 2026 16:52:50 +1300 Subject: [PATCH] simulation: implement required failure hooks --- .../sensor_baro_sim/SensorBaroSim.cpp | 76 +++++++++++++ .../sensor_baro_sim/SensorBaroSim.hpp | 10 ++ .../sensor_gps_sim/SensorGpsSim.cpp | 50 +++++++++ .../sensor_gps_sim/SensorGpsSim.hpp | 7 ++ .../sensor_mag_sim/SensorMagSim.cpp | 50 +++++++++ .../sensor_mag_sim/SensorMagSim.hpp | 8 ++ src/modules/simulation/simulator_sih/sih.cpp | 104 +++++++++++++++++- src/modules/simulation/simulator_sih/sih.hpp | 11 ++ 8 files changed, 314 insertions(+), 2 deletions(-) diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index e7dd2bd991..72ff948ad8 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp @@ -86,6 +86,57 @@ float SensorBaroSim::generate_wgn() return X; } +void SensorBaroSim::check_failure_injections() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, baro off"); + supported = true; + _baro_blocked = true; + _baro_stuck = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); + supported = true; + _baro_blocked = false; + _baro_stuck = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, baro ok"); + supported = true; + _baro_blocked = false; + _baro_stuck = false; + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + void SensorBaroSim::Run() { if (should_exit()) { @@ -105,6 +156,27 @@ void SensorBaroSim::Run() updateParams(); } + check_failure_injections(); + + if (_baro_blocked) { + perf_end(_loop_perf); + return; + } + + if (_baro_stuck) { + // Publish stuck (last known) values + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = hrt_absolute_time(); + sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + sensor_baro.pressure = _last_pressure; + sensor_baro.temperature = _last_temperature; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + perf_end(_loop_perf); + return; + } + if (_vehicle_global_position_sub.updated()) { vehicle_global_position_s gpos; @@ -161,6 +233,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 sensor_baro_s sensor_baro{}; sensor_baro.timestamp_sample = gpos.timestamp; diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index d103845378..3f373a23b3 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include using namespace time_literals; @@ -67,17 +69,25 @@ public: private: void Run() override; + void check_failure_injections(); // generate white Gaussian noise sample with std=1 static float generate_wgn(); uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + bool _baro_blocked{false}; + bool _baro_stuck{false}; bool _baro_rnd_use_last{false}; double _baro_rnd_y2{0.0}; 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_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index aa1fe2b944..c5ad438a56 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -87,6 +87,49 @@ float SensorGpsSim::generate_wgn() return X; } +void SensorGpsSim::check_failure_injections() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, GPS off"); + supported = true; + _gps_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); + supported = true; + _gps_blocked = false; + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + void SensorGpsSim::Run() { if (should_exit()) { @@ -106,6 +149,13 @@ void SensorGpsSim::Run() updateParams(); } + check_failure_injections(); + + if (_gps_blocked) { + perf_end(_loop_perf); + return; + } + if (_vehicle_local_position_groundtruth_sub.updated() && _vehicle_global_position_groundtruth_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index 6bfb06b581..574433cada 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -43,6 +43,8 @@ #include #include #include +#include +#include #include #include @@ -67,6 +69,7 @@ public: private: void Run() override; + void check_failure_injections(); // generate white Gaussian noise sample with std=1 static float generate_wgn(); @@ -77,8 +80,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_groundtruth_sub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Subscription _vehicle_local_position_groundtruth_sub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + bool _gps_blocked{false}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp index 97c2626f75..bb3351ef30 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp @@ -87,6 +87,49 @@ float SensorMagSim::generate_wgn() return X; } +void SensorMagSim::check_failure_injections() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, mag off"); + supported = true; + _mag_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, mag ok"); + supported = true; + _mag_blocked = false; + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + void SensorMagSim::Run() { if (should_exit()) { @@ -106,6 +149,13 @@ void SensorMagSim::Run() updateParams(); } + check_failure_injections(); + + if (_mag_blocked) { + perf_end(_loop_perf); + return; + } + if (_vehicle_global_position_sub.updated()) { vehicle_global_position_s gpos; diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 18c4adbf1d..a7b64dab19 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include using namespace time_literals; @@ -67,6 +69,7 @@ public: private: void Run() override; + void check_failure_injections(); // generate white Gaussian noise sample with std=1 static float generate_wgn(); @@ -77,9 +80,14 @@ 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)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION + bool _mag_blocked{false}; + bool _mag_earth_available{false}; matrix::Vector3f _mag_earth_pred{}; diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index e5c989b2f7..465ada5d1f 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -190,6 +190,91 @@ void Sih::realtime_loop() px4_sem_destroy(&_data_semaphore); } +void Sih::check_failure_injections() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); + supported = true; + _airspeed_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); + supported = true; + _airspeed_blocked = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, distance sensor off"); + supported = true; + _distance_sensor_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, distance sensor ok"); + supported = true; + _distance_sensor_blocked = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, accel off"); + supported = true; + _accel_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, accel ok"); + supported = true; + _accel_blocked = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, gyro off"); + supported = true; + _gyro_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, gyro ok"); + supported = true; + _gyro_blocked = false; + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + void Sih::sensor_step() { // check for parameter updates @@ -203,6 +288,8 @@ void Sih::sensor_step() parameters_updated(); } + check_failure_injections(); + perf_begin(_loop_perf); const hrt_abstime now = hrt_absolute_time(); @@ -636,12 +723,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; @@ -656,6 +752,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; + } + device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; device_id.devid_s.bus = 0; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index bda4484884..da0fb6462c 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -77,6 +77,8 @@ #include #include #include +#include +#include #include #include @@ -117,6 +119,7 @@ public: private: void parameters_updated(); + void check_failure_injections(); // simulated sensors PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION @@ -132,6 +135,14 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + 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;