From c712e926d73fb2fa8ef1a20712b2128db79e1473 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 5 Nov 2018 16:14:21 -0700 Subject: [PATCH] AP_Baro: Avoid returning 0.0f for EAS2TAS --- libraries/AP_Baro/AP_Baro.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index edc1da5c2f..804b5572b5 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -319,7 +319,11 @@ float AP_Baro::get_EAS2TAS(void) // provides a more consistent reading then trying to estimate a complete // ISA model atmosphere float tempK = get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude; - _EAS2TAS = safe_sqrt(SSL_AIR_DENSITY / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK))); + const float eas2tas_squared = SSL_AIR_DENSITY / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)); + if (!is_positive(eas2tas_squared)) { + return 1.0; + } + _EAS2TAS = sqrtf(eas2tas_squared); _last_altitude_EAS2TAS = altitude; return _EAS2TAS; }