dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

SimulatorMavlink: add airspeed icing failure (ramp down scale) 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-06-26 14:14:16 +02:00
parent a4b04c3982
commit 0d36d54fbe
2 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
View File

@ -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) {

1
src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
View File

@ -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] {};