From 23787cf6954c796e95c7c1ba24659b5b526fbb5b Mon Sep 17 00:00:00 2001 From: ziltoid2 Date: Sat, 16 May 2015 21:49:36 +0300 Subject: [PATCH] AP_Baro: use ground_temperature instead of calibration_temperature for alt calculation --- libraries/AP_Baro/AP_Baro.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 15253df81a..31843ae793 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -173,16 +173,15 @@ void AP_Baro::update_calibration() float AP_Baro::get_altitude_difference(float base_pressure, float pressure) const { float ret; + float temp = get_ground_temperature() + 273.15f; #if HAL_CPU_CLASS <= HAL_CPU_CLASS_16 // on slower CPUs use a less exact, but faster, calculation float scaling = base_pressure / pressure; - float temp = get_calibration_temperature() + 273.15f; ret = logf(scaling) * temp * 29.271267f; #else // on faster CPUs use a more exact calculation float scaling = pressure / base_pressure; - float temp = get_calibration_temperature() + 273.15f; - + // This is an exact calculation that is within +-2.5m of the standard atmosphere tables // in the troposphere (up to 11,000 m amsl). ret = 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling)));