diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index a695078536..7e8b050c03 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp @@ -164,12 +164,17 @@ void SensorBaroSim::Run() // calculate temperature in Celsius float temperature = temperature_local - 273.0f + _sim_baro_off_t.get(); + if (!_baro_stuck) { + _last_baro_pressure = pressure; + _last_baro_temperature = temperature; + } + // publish sensor_baro_s sensor_baro{}; sensor_baro.timestamp_sample = gpos.timestamp; sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = pressure; - sensor_baro.temperature = temperature; + sensor_baro.pressure = _last_baro_pressure; + sensor_baro.temperature = _last_baro_temperature; sensor_baro.timestamp = hrt_absolute_time(); _sensor_baro_pub.publish(sensor_baro); @@ -203,11 +208,19 @@ void SensorBaroSim::check_failure_injection() if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { PX4_WARN("CMD_INJECT_FAILURE, BARO off"); supported = true; + _baro_stuck = false; _baro_blocked = true; + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); + supported = true; + _baro_stuck = true; + _baro_blocked = false; + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { PX4_INFO("CMD_INJECT_FAILURE, BARO ok"); supported = true; + _baro_stuck = false; _baro_blocked = false; } } diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index 714a104f34..bd68a0d429 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp @@ -85,6 +85,10 @@ private: float _baro_drift_pa_per_sec{0.0}; float _baro_drift_pa{0.0}; bool _baro_blocked{false}; + bool _baro_stuck{false}; + + float _last_baro_pressure{0.0}; + float _last_baro_temperature{0.0}; hrt_abstime _last_update_time{0}; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 097fe6eee4..3e80ed16a8 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -1469,6 +1469,7 @@ void SimulatorMavlink::check_failure_injections() PX4_INFO("CMD_INJECT_FAILURE, baro ok"); supported = true; _baro_blocked = false; + _baro_stuck = false; } } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) {