From 3f67917c2688b91d0ef6f645c8261e51790be11f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Aug 2021 07:50:28 +1000 Subject: [PATCH] AP_Airspeed: check for baro baro data in SDP3X this matters for SDP3X as AP_Periph sensor with no baro --- libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp index 58e08620d5..5c79c00082 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp @@ -223,16 +223,25 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press) AP_Baro *baro = AP_Baro::get_singleton(); - if (baro == nullptr) { - return press; + float baro_pressure; + if (baro == nullptr || baro->num_instances() == 0) { + // with no baro assume sea level + baro_pressure = SSL_AIR_PRESSURE; + } else { + baro_pressure = baro->get_pressure(); } float temperature; if (!get_temperature(temperature)) { - return press; + // assume 25C if no temperature + temperature = 25; } - float rho_air = baro->get_pressure() / (ISA_GAS_CONSTANT * (temperature + C_TO_KELVIN)); + float rho_air = baro_pressure / (ISA_GAS_CONSTANT * (temperature + C_TO_KELVIN)); + if (!is_positive(rho_air)) { + // bad pressure + return press; + } /* the constants in the code below come from a calibrated test of