AP_Airspeed: check for baro baro data in SDP3X

this matters for SDP3X as AP_Periph sensor with no baro
ArduPilot · Aug 5, 2021 · f896439 · f896439
1 parent 3c0238c
commit f896439
Showing 1 changed file with 13 additions and 4 deletions.
diff --git 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