From 1b381b56752c83e6348b914abb700e5d7501a75b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 29 Apr 2015 14:12:41 +0900 Subject: [PATCH] Baro: get_air_density_ratio gets div-by-zero check --- libraries/AP_Baro/AP_Baro.cpp | 9 +++++++-- libraries/AP_Baro/AP_Baro.h | 2 +- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index f412fe4f5b..1f5aa1384a 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -208,10 +208,15 @@ float AP_Baro::get_EAS2TAS(void) return _EAS2TAS; } -// return air density / sea level density - decreases in altitude +// return air density / sea level density - decreases as altitude climbs float AP_Baro::get_air_density_ratio(void) { - return 1.0f/(sq(get_EAS2TAS())); + float eas2tas = get_EAS2TAS(); + if (eas2tas > 0.0f) { + return 1.0f/(sq(get_EAS2TAS())); + } else { + return 1.0f; + } } // return current climb_rate estimeate relative to time that calibrate() diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 6a0eed037b..00d035ad6f 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -79,7 +79,7 @@ public: // get scale factor required to convert equivalent to true airspeed float get_EAS2TAS(void); - // get air density / sea level density - decreases in altitude + // get air density / sea level density - decreases as altitude climbs float get_air_density_ratio(void); // get current climb rate in meters/s. A positive number means