fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 09:24:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

implement baro stuck failure

Browse Source
This commit is contained in:
Balduin 2025-01-22 14:05:02 +01:00 committed by Ramon Roche
parent 321ac6f994
commit 2e89edc5e5
No known key found for this signature in database
GPG Key ID: 275988FAE5821713
3 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp
View File

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

4
src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp
View File

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

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

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