From 0d36d54fbe9a588db7449198c1f3e98c625acd55 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 26 Jun 2023 14:14:16 +0200 Subject: [PATCH] SimulatorMavlink: add airspeed icing failure (ramp down scale) Signed-off-by: Silvan Fuhrer --- .../simulator_mavlink/SimulatorMavlink.cpp | 17 ++++++++++++++++- .../simulator_mavlink/SimulatorMavlink.hpp | 1 + 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 0fb7603f0f..12dc4a9ff4 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -327,10 +327,19 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil // differential pressure if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_disconnected) { + + float airspeed_blockage_scale = 1.f; + const float airspeed_blockage_rampup_time = 60_s; // time it takes to block the airspeed sensor completely, linear ramp + + if (_airspeed_blocked_timestamp > 0) { + airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) / + airspeed_blockage_rampup_time, 0.5f, 1.f); + } + differential_pressure_s report{}; report.timestamp_sample = time; report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - report.differential_pressure_pa = sensors.diff_pressure * 100.f; // hPa to Pa; + report.differential_pressure_pa = sensors.diff_pressure * 100.f * airspeed_blockage_scale; // hPa to Pa; report.temperature = _sensors_temperature; report.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(report); @@ -1433,10 +1442,16 @@ void SimulatorMavlink::check_failure_injections() supported = true; _airspeed_disconnected = true; + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate icing)"); + supported = true; + _airspeed_blocked_timestamp = hrt_absolute_time(); + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); supported = true; _airspeed_disconnected = false; + _airspeed_blocked_timestamp = 0; } } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) { diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 0d4e97b91c..7790622c5e 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -295,6 +295,7 @@ private: bool _gps_blocked{false}; bool _airspeed_disconnected{false}; + hrt_abstime _airspeed_blocked_timestamp{0}; bool _vio_blocked{false}; float _last_magx[MAG_COUNT_MAX] {};