From 321ac6f994ed3b44b5c8ff163e652dad8523c8d5 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 22 Jan 2025 13:35:26 +0100 Subject: [PATCH] new sensor failures airspeed, baro, and mag off. agp has no failures, and other failure types like stuck etc not implemented at all. doubting this implementation, lots of repetitions and almost-repetitions. maybe we should do it more similar to SimulatorMavlink.cpp? Where simulator_sih handles the sensor failures itself, with one big if mess, but the rest of it nice and lean. failing tests for quadx: - 'Continue on baro stuck during mission (baro height mode)': failed - 'Continue on baro stuck during mission (GPS height mode)': failed - 'Fly straight Multicopter Mission': failed - 'Offboard takeoff and land': failed - 'Offboard position control': failed --- .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 44 ++++++++++++ .../sensor_airspeed_sim/SensorAirspeedSim.hpp | 9 +++ .../sensor_baro_sim/SensorBaroSim.cpp | 49 ++++++++++++- .../sensor_baro_sim/SensorBaroSim.hpp | 8 +++ .../sensor_mag_sim/SensorMagSim.cpp | 68 ++++++++++++++++--- .../sensor_mag_sim/SensorMagSim.hpp | 10 ++- .../simulator_mavlink/SimulatorMavlink.cpp | 2 + 7 files changed, 179 insertions(+), 11 deletions(-) diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 5566efbb1c..634b61a740 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -154,6 +154,50 @@ void SensorAirspeedSim::Run() perf_end(_loop_perf); } +void SensorAirspeedSim::check_failure_injection() +{ + 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; + _arsp_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, Airspeed ok"); + supported = true; + _arsp_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); + } + } +} + + 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 3aafc9ca93..a7b5bf9bf1 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -46,6 +46,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -72,6 +74,8 @@ public: bool init(); + void check_failure_injection(); + private: void Run() override; @@ -85,8 +89,13 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + bool _arsp_blocked{false}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index e7dd2bd991..a695078536 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp @@ -106,9 +106,12 @@ void SensorBaroSim::Run() } if (_vehicle_global_position_sub.updated()) { + + check_failure_injection(); + vehicle_global_position_s gpos; - if (_vehicle_global_position_sub.copy(&gpos)) { + if (_vehicle_global_position_sub.copy(&gpos) && !_baro_blocked) { const float dt = math::constrain((gpos.timestamp - _last_update_time) * 1e-6f, 0.001f, 0.1f); @@ -178,6 +181,50 @@ void SensorBaroSim::Run() perf_end(_loop_perf); } + +void SensorBaroSim::check_failure_injection() +{ + 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; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, BARO ok"); + supported = true; + _baro_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); + } + } +} + int SensorBaroSim::task_spawn(int argc, char *argv[]) { SensorBaroSim *instance = new SensorBaroSim(); diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index d103845378..714a104f34 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -65,6 +67,8 @@ public: bool init(); + void check_failure_injection(); + private: void Run() override; @@ -73,15 +77,19 @@ private: 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)}; + 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}; + bool _baro_blocked{false}; hrt_abstime _last_update_time{0}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; 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..803027dc5c 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp @@ -107,19 +107,24 @@ void SensorMagSim::Run() } if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s gpos; - if (_vehicle_global_position_sub.copy(&gpos)) { - if (gpos.eph < 1000) { + check_failure_injection(); - // magnetic field data returned by the geo library using the current GPS position - const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon)); - const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon)); - const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon); + if (!_mag_blocked) { + vehicle_global_position_s gpos; - _mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0); + if (_vehicle_global_position_sub.copy(&gpos)) { + if (gpos.eph < 1000) { - _mag_earth_available = true; + // magnetic field data returned by the geo library using the current GPS position + const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon)); + const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon)); + const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon); + + _mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0); + + _mag_earth_available = true; + } } } } @@ -142,6 +147,51 @@ void SensorMagSim::Run() perf_end(_loop_perf); } + +void SensorMagSim::check_failure_injection() +{ + 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); + } + } +} + + int SensorMagSim::task_spawn(int argc, char *argv[]) { SensorMagSim *instance = new SensorMagSim(); diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 18c4adbf1d..d4c88b111f 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -65,6 +67,8 @@ public: bool init(); + void check_failure_injection(); + private: void Run() override; @@ -77,10 +81,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)}; + + PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + 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_earth_available{false}; + bool _mag_blocked{false}; matrix::Vector3f _mag_earth_pred{}; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 342cfcd4e7..097fe6eee4 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -1246,6 +1246,8 @@ void SimulatorMavlink::run() void SimulatorMavlink::check_failure_injections() { + PX4_INFO("Entering SimulatorMavlink::check_failure_injections"); + vehicle_command_s vehicle_command; while (_vehicle_command_sub.update(&vehicle_command)) {