Baro: improved atmospheric model for high altitude flight

ArduPlane/tailsitter.cpp

@@ -707,7 +707,7 @@ void Tailsitter::speed_scaling(void)
             // (mass / A) is disk loading DL so:
             // Ue^2 = (((t / t_h) * DL * 9.81)/(0.5 * rho)) + U0^2
 
-            const float rho = SSL_AIR_DENSITY * plane.barometer.get_air_density_ratio();
+            const float rho = SSL_AIR_DENSITY * quadplane.ahrs.get_air_density_ratio();
             float hover_rho = rho;
             if ((gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
                 // if applying altitude correction use sea level density for hover case
@@ -752,7 +752,7 @@ void Tailsitter::speed_scaling(void)
 
     if ((gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
         // air density correction
-        spd_scaler /= plane.barometer.get_air_density_ratio();
+        spd_scaler /= quadplane.ahrs.get_air_density_ratio();
     }
 
     const SRV_Channel::Aux_servo_function_t functions[] = {

Tools/autotest/arducopter.py

@@ -3780,7 +3780,7 @@ def WPNAV_SPEED_DN(self):
 
         minimum_duration = 5
 
-        self.takeoff(500, timeout=60)
+        self.takeoff(500, timeout=70)
         self.change_mode('AUTO')
 
         start_speed_ms = self.get_parameter('WPNAV_SPEED_DN') / 100.0

libraries/AP_AHRS/AP_AHRS.cpp

@@ -3570,6 +3570,13 @@ float AP_AHRS::get_EAS2TAS(void) const
     return 1.0;
 }
 
+// get air density / sea level density - decreases as altitude climbs
+float AP_AHRS::get_air_density_ratio(void) const
+{
+    const float eas2tas = get_EAS2TAS();
+    return 1.0 / sq(eas2tas);
+}
+
 // singleton instance
 AP_AHRS *AP_AHRS::_singleton;
 

libraries/AP_AHRS/AP_AHRS.h

@@ -143,6 +143,9 @@ class AP_AHRS {
     // get apparent to true airspeed ratio
     float get_EAS2TAS(void) const;
 
+    // get air density / sea level density - decreases as altitude climbs
+    float get_air_density_ratio(void) const;
+
     // return an airspeed estimate if available. return true
     // if we have an estimate
     bool airspeed_estimate(float &airspeed_ret) const;

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -265,7 +265,7 @@ void AP_AHRS::Log_Write_Home_And_Origin()
 
 // get apparent to true airspeed ratio
 float AP_AHRS_Backend::get_EAS2TAS(void) {
-    return AP::baro().get_EAS2TAS();
+    return AP::baro()._get_EAS2TAS();
 }
 
 // return current vibration vector for primary IMU

libraries/AP_Airspeed/AP_Airspeed_SITL.cpp

@@ -33,11 +33,8 @@ bool AP_Airspeed_SITL::get_temperature(float &temperature)
     // this was mostly swiped from SIM_Airspeed_DLVR:
     const float sim_alt = sitl->state.altitude;
 
-    float sigma, delta, theta;
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
-
     // To Do: Add a sensor board temperature offset parameter
-    temperature = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
+    temperature = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt);
 
     return true;
 }

libraries/AP_Airspeed/Airspeed_Calibration.cpp

@@ -12,7 +12,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS.h>
-#include <AP_Baro/AP_Baro.h>
+#include <AP_AHRS/AP_AHRS.h>
 
 #include "AP_Airspeed.h"
 
@@ -131,7 +131,7 @@ void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t
 
     // calculate true airspeed, assuming a airspeed ratio of 1.0
     float dpress = MAX(get_differential_pressure(i), 0);
-    float true_airspeed = sqrtf(dpress) * AP::baro().get_EAS2TAS();
+    float true_airspeed = sqrtf(dpress) * AP::ahrs().get_EAS2TAS();
 
     float zratio = state[i].calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal);
 
@@ -177,7 +177,7 @@ void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
         vy: vground.y,
         vz: vground.z,
         diff_pressure: get_differential_pressure(primary),
-        EAS2TAS: AP::baro().get_EAS2TAS(),
+        EAS2TAS: AP::ahrs().get_EAS2TAS(),
         ratio: param[primary].ratio.get(),
         state_x: state[primary].calibration.state.x,
         state_y: state[primary].calibration.state.y,

libraries/AP_Baro/AP_Baro.cpp

@@ -351,7 +351,7 @@ void AP_Baro::calibrate(bool save)
             sensors[i].calibrated = false;
         } else {
             if (save) {
-                float p0_sealevel = get_sealevel_pressure(sum_pressure[i] / count[i]);
+                float p0_sealevel = get_sealevel_pressure(sum_pressure[i] / count[i], _field_elevation_active);
                 sensors[i].ground_pressure.set_and_save(p0_sealevel);
             }
         }
@@ -387,7 +387,7 @@ void AP_Baro::update_calibration()
     }
     for (uint8_t i=0; i<_num_sensors; i++) {
         if (healthy(i)) {
-            float corrected_pressure = get_sealevel_pressure(get_pressure(i) + sensors[i].p_correction);
+            float corrected_pressure = get_sealevel_pressure(get_pressure(i) + sensors[i].p_correction, _field_elevation_active);
             sensors[i].ground_pressure.set(corrected_pressure);
         }
 
@@ -401,60 +401,11 @@ void AP_Baro::update_calibration()
     _guessed_ground_temperature = get_external_temperature();
 }
 
-// return altitude difference in meters between current pressure and a
-// given base_pressure in Pascal
-float AP_Baro::get_altitude_difference(float base_pressure, float pressure) const
-{
-    float temp    = C_TO_KELVIN(get_ground_temperature());
-    float scaling = pressure / base_pressure;
-
-    // This is an exact calculation that is within +-2.5m of the standard
-    // atmosphere tables in the troposphere (up to 11,000 m amsl).
-    return 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling)))-_field_elevation_active;
-}
-
-// return sea level pressure where in which the current measured pressure
-// at field elevation returns the same altitude under the
-// 1976 standard atmospheric model
-float AP_Baro::get_sealevel_pressure(float pressure) const
-{
-    float temp    = C_TO_KELVIN(get_ground_temperature());
-    float p0_sealevel;
-    // This is an exact calculation that is within +-2.5m of the standard
-    // atmosphere tables in the troposphere (up to 11,000 m amsl).
-    p0_sealevel = 8.651154799255761e30f*pressure*powF((769231.0f-(5000.0f*_field_elevation_active)/temp),-5.255993146184937f);
-
-    return p0_sealevel;
-}
-
-
-// return current scale factor that converts from equivalent to true airspeed
-// valid for altitudes up to 10km AMSL
-// assumes standard atmosphere lapse rate
-float AP_Baro::get_EAS2TAS(void)
-{
-    float altitude = get_altitude();
-
-    float pressure = get_pressure();
-    if (is_zero(pressure)) {
-        return 1.0f;
-    }
-
-    // only estimate lapse rate for the difference from the ground location
-    // provides a more consistent reading then trying to estimate a complete
-    // ISA model atmosphere
-    float tempK = C_TO_KELVIN(get_ground_temperature()) - ISA_LAPSE_RATE * altitude;
-    const float eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK));
-    if (!is_positive(eas2tas_squared)) {
-        return 1.0f;
-    }
-    return sqrtf(eas2tas_squared);
-}
 
 // return air density / sea level density - decreases as altitude climbs
-float AP_Baro::get_air_density_ratio(void)
+float AP_Baro::_get_air_density_ratio(void)
 {
-    const float eas2tas = get_EAS2TAS();
+    const float eas2tas = _get_EAS2TAS();
     if (eas2tas > 0.0f) {
         return 1.0f/(sq(eas2tas));
     } else {
@@ -923,6 +874,9 @@ void AP_Baro::update(void)
                 corrected_pressure -= wind_pressure_correction(i);
 #endif
                 altitude = get_altitude_difference(sensors[i].ground_pressure, corrected_pressure);
+
+                // the ground pressure is references against the field elevation
+                altitude -= _field_elevation_active;
             } else if (sensors[i].type == BARO_TYPE_WATER) {
                 //101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
                 //No temperature or depth compensation for density of water.
@@ -1117,7 +1071,7 @@ bool AP_Baro::arming_checks(size_t buflen, char *buffer) const
     if (_alt_error_max > 0 && gps.status() >= AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
         const float alt_amsl = gps.location().alt*0.01;
         // note the addition of _field_elevation_active as this is subtracted in get_altitude_difference()
-        const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure()) + _field_elevation_active;
+        const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure());
         const float error = fabsf(alt_amsl - alt_pressure);
         if (error > _alt_error_max) {
             hal.util->snprintf(buffer, buflen, "GPS alt error %.0fm (see BARO_ALTERR_MAX)", error);

libraries/AP_Baro/AP_Baro.h

@@ -101,22 +101,51 @@ class AP_Baro
     float get_altitude(void) const { return get_altitude(_primary); }
     float get_altitude(uint8_t instance) const { return sensors[instance].altitude; }
 
+    // get altitude above mean sea level
+    float get_altitude_AMSL(uint8_t instance) const { return get_altitude(instance) + _field_elevation_active; }
+    float get_altitude_AMSL(void) const { return get_altitude_AMSL(_primary); }
+
     // returns which i2c bus is considered "the" external bus
     uint8_t external_bus() const { return _ext_bus; }
 
+    // Atmospheric Model Functions
+    static float geometric_alt_to_geopotential(float alt);
+    static float geopotential_alt_to_geometric(float alt);
+
+    float get_temperature_from_altitude(float alt) const;
+    float get_altitude_from_pressure(float pressure) const;
+
+    // EAS2TAS for SITL
+    static float get_EAS2TAS_for_alt_amsl(float alt_amsl);
+
+    // lookup expected pressure for a given altitude. Used for SITL backend
+    static void get_pressure_temperature_for_alt_amsl(float alt_amsl, float &pressure, float &temperature_K);
+
+    // lookup expected temperature in degrees C for a given altitude. Used for SITL backend
+    static float get_temperatureC_for_alt_amsl(const float alt_amsl);
+
+    // lookup expected pressure in Pa for a given altitude. Used for SITL backend
+    static float get_pressure_for_alt_amsl(const float alt_amsl);
+
+    // get air density for SITL
+    static float get_air_density_for_alt_amsl(float alt_amsl);
+
     // get altitude difference in meters relative given a base
     // pressure in Pascal
     float get_altitude_difference(float base_pressure, float pressure) const;
 
     // get sea level pressure relative to 1976 standard atmosphere model
     // pressure in Pascal
-    float get_sealevel_pressure(float pressure) const;
+    float get_sealevel_pressure(float pressure, float altitude) const;
 
-    // get scale factor required to convert equivalent to true airspeed
-    float get_EAS2TAS(void);
+    // get scale factor required to convert equivalent to true
+    // airspeed. This should only be used to update the AHRS value
+    // once per loop. Please use AP::ahrs().get_EAS2TAS()
+    float _get_EAS2TAS(void) const;
 
     // get air density / sea level density - decreases as altitude climbs
-    float get_air_density_ratio(void);
+    // please use AP::ahrs()::get_air_density_ratio()
+    float _get_air_density_ratio(void);
 
     // get current climb rate in meters/s. A positive number means
     // going up
@@ -168,9 +197,6 @@ class AP_Baro
     // get baro drift amount
     float get_baro_drift_offset(void) const { return _alt_offset_active; }
 
-    // simple atmospheric model
-    static void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta);
-
     // simple underwater atmospheric model
     static void SimpleUnderWaterAtmosphere(float alt, float &rho, float &delta, float &theta);
 
@@ -321,6 +347,14 @@ class AP_Baro
     void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
 
     void update_field_elevation();
+
+    // atmosphere model functions
+    float get_altitude_difference_extended(float base_pressure, float pressure) const;
+    float get_EAS2TAS_extended(float pressure) const;
+    static float get_temperature_by_altitude_layer(float alt, int8_t idx);
+
+    float get_altitude_difference_simple(float base_pressure, float pressure) const;
+    float get_EAS2TAS_simple(float altitude, float pressure) const;
 };
 
 namespace AP {

libraries/AP_Baro/AP_Baro_HIL.cpp

@@ -8,33 +8,6 @@ extern const AP_HAL::HAL& hal;
 // ==========================================================================
 // based on tables.cpp from http://www.pdas.com/atmosdownload.html
 
-/* 
-   Compute the temperature, density, and pressure in the standard atmosphere
-   Correct to 20 km.  Only approximate thereafter.
-*/
-void AP_Baro::SimpleAtmosphere(
-	const float alt,                           // geometric altitude, km.
-	float& sigma,                   // density/sea-level standard density
-	float& delta,                 // pressure/sea-level standard pressure
-	float& theta)           // temperature/sea-level standard temperature
-{
-    const float REARTH = 6369.0f;        // radius of the Earth (km)
-    const float GMR    = 34.163195f;     // gas constant
-    float h=alt*REARTH/(alt+REARTH);     // geometric to geopotential altitude
-
-    if (h < 11.0f) {
-        // Troposphere
-        theta = (SSL_AIR_TEMPERATURE - 6.5f * h) / SSL_AIR_TEMPERATURE;
-        delta = powf(theta, GMR / 6.5f);
-    } else {
-        // Stratosphere
-        theta = 216.65f / SSL_AIR_TEMPERATURE;
-        delta = 0.2233611f * expf(-GMR * (h - 11.0f) / 216.65f);
-    }
-
-    sigma = delta/theta;
-}
-
 void AP_Baro::SimpleUnderWaterAtmosphere(
 	float alt,            // depth, km.
 	float& rho,           // density/sea-level

libraries/AP_Baro/AP_Baro_Logging.cpp

@@ -13,6 +13,7 @@ void AP_Baro::Write_Baro_instance(uint64_t time_us, uint8_t baro_instance)
         time_us       : time_us,
         instance      : baro_instance,
         altitude      : get_altitude(baro_instance),
+        altitude_AMSL : get_altitude_AMSL(baro_instance),
         pressure      : get_pressure(baro_instance),
         temperature   : (int16_t)(get_temperature(baro_instance) * 100 + 0.5f),
         climbrate     : get_climb_rate(),

libraries/AP_Baro/AP_Baro_SITL.cpp

@@ -118,12 +118,9 @@ void AP_Baro_SITL::_timer()
     }
 
 #if !APM_BUILD_TYPE(APM_BUILD_ArduSub)
-    float sigma, delta, theta;
-
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
-    float p = SSL_AIR_PRESSURE * delta;
-    float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);
-
+    float p, T_K;
+    AP_Baro::get_pressure_temperature_for_alt_amsl(sim_alt, p, T_K);
+    float T = KELVIN_TO_C(T_K);
     temperature_adjustment(p, T);
 #else
     float rho, delta, theta;
@@ -143,7 +140,7 @@ void AP_Baro_SITL::_timer()
 // unhealthy if baro is turned off or beyond supported instances
 bool AP_Baro_SITL::healthy(uint8_t instance) 
 {
-    return !_sitl->baro[instance].disable;
+    return _last_sample_time != 0 && !_sitl->baro[instance].disable;
 }
 
 // Read the sensor
@@ -189,7 +186,7 @@ float AP_Baro_SITL::wind_pressure_correction(uint8_t instance)
         error += bp.wcof_zn * sqz;
     }
 
-    return error * 0.5 * SSL_AIR_DENSITY * AP::baro().get_air_density_ratio();
+    return error * 0.5 * SSL_AIR_DENSITY * AP::baro()._get_air_density_ratio();
 }
 
 #endif  // AP_SIM_BARO_ENABLED

libraries/AP_Baro/AP_Baro_Wind.cpp

@@ -76,15 +76,17 @@ float AP_Baro::wind_pressure_correction(uint8_t instance)
         return 0;
     }
 
+    auto &ahrs = AP::ahrs();
+
     // correct for static pressure position errors
     Vector3f airspeed_vec_bf;
-    if (!AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
+    if (!ahrs.airspeed_vector_true(airspeed_vec_bf)) {
         return 0;
     }
 
     float error = 0.0;
 
-    const float kp = 0.5 * SSL_AIR_DENSITY * get_air_density_ratio();
+    const float kp = 0.5 * SSL_AIR_DENSITY * ahrs.get_air_density_ratio();
     const float sqxp = sq(airspeed_vec_bf.x) * kp;
     const float sqyp = sq(airspeed_vec_bf.y) * kp;
     const float sqzp = sq(airspeed_vec_bf.z) * kp;