From 3b98bbd159dbb7475cd143d03f8aeb3e8ab8df77 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 26 Jun 2013 18:30:33 +1000 Subject: [PATCH] AP_Baro: more precise altitude calculation on PX4 if not using an AVR CPU then use a more computationally expensive altitude calculation, which is more precise at higher altitudes --- libraries/AP_Baro/AP_Baro.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index faad924cad..c804f93a56 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -106,12 +106,21 @@ float AP_Baro::get_altitude(void) return _altitude; } - // this has no filtering of the pressure values, use a separate - // filter if you want a smoothed value. The AHRS driver wants - // unsmoothed values + +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on AVR use a less exact, but faster, calculation scaling = (float)_ground_pressure / (float)get_pressure(); temp = ((float)_ground_temperature) + 273.15f; _altitude = logf(scaling) * temp * 29.271267f; +#else + // on faster CPUs use a more exact calculation + scaling = (float)get_pressure() / (float)_ground_pressure; + temp = ((float)_ground_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). + _altitude = 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling))); +#endif _last_altitude_t = _last_update;