diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 0938cad303..4e410907a0 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/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); diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 81d58a967b..5996c673a6 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -101,22 +101,50 @@ public: 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(void) const { return get_altitude() + _field_elevation_active; } + // 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 +196,6 @@ public: // 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 +346,14 @@ private: 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 { diff --git a/libraries/AP_Baro/AP_Baro_HIL.cpp b/libraries/AP_Baro/AP_Baro_HIL.cpp index 985133e0b8..1442635a0c 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.cpp +++ b/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 diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index fc26899485..861674360e 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/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 diff --git a/libraries/AP_Baro/AP_Baro_Wind.cpp b/libraries/AP_Baro/AP_Baro_Wind.cpp index 17ad6b12dd..67b33e6a65 100644 --- a/libraries/AP_Baro/AP_Baro_Wind.cpp +++ b/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; diff --git a/libraries/AP_Baro/AP_Baro_atmosphere.cpp b/libraries/AP_Baro/AP_Baro_atmosphere.cpp new file mode 100644 index 0000000000..63667432e8 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_atmosphere.cpp @@ -0,0 +1,362 @@ +#include "AP_Baro.h" +#include + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* 1976 U.S. Standard Atmosphere: https://ntrs.nasa.gov/api/citations/19770009539/downloads/19770009539.pdf?attachment=true + +The US Standard Atmosphere defines the atmopshere in terms of whether the temperature is an iso-thermal or gradient layer. +Ideal gas laws apply thus P = rho * R_specific * T : P = pressure, rho = density, R_specific = air gas constant, T = temperature + +Note: the 1976 model is the same as the 1962 US Standard Atomsphere up to 51km. +R_universal: the universal gas constant is slightly off in the 1976 model and thus R_specific is different than today's value + +*/ + +/* Model Constants +R_universal = 8.31432e-3; // Universal gas constant (J/(kmol-K)) incorrect to the redefined 2019 value of 8.31446261815324 J⋅K−1⋅mol−1 +M_air = (0.78084 * 28.0134 + 0.209476 * 31.9988 + 9.34e-3 * 39.948 + 3.14e-4 * 44.00995 + 1.818e-5 * 20.183 + 5.24E-6 * 4.0026 + 1.14E-6 * 83.8 + 8.7E-7 * 131.30 + 2E-6 * 16.04303 + 5E-7 * 2.01594) * 1E-3; (kg/mol) +M_air = 28.9644 // Molecular weight of air (kg/kmol) see page 3 +R_specific = 287.053072 // air specifc gas constant (J⋅kg−1⋅K−1), R_universal / M_air; +gama = 1.4; // specific heat ratio of air used to determine the speed of sound + +R0 = 6356.766E3; // Earth's radius (in m) +g0 = 9.80665; // gravity (m/s^2) + +Sea-Level Constants +H_asml = 0 meters +T0 = 288.150 K +P0 = 101325 Pa +rho0 = 1.2250 kg/m^3 +T0_slope = -6.5E-3 (K/m') + +The tables list altitudes -5 km to 0 km using the same equations as 0 km to 11 km. +*/ + +#ifndef AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED +// default to using the extended functions when doing double precision EKF (which implies more flash space and faster MCU) +// this allows for using the simple model with the --ekf-single configure option +#define AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED HAL_WITH_EKF_DOUBLE +#endif + +/* + return altitude difference in meters between current pressure and a + given base_pressure in Pascal. This is a simple atmospheric model + good to about 11km AMSL. +*/ +float AP_Baro::get_altitude_difference_simple(float base_pressure, float pressure) const +{ + float ret; + float temp_K = 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). + ret = 153.8462f * temp_K * (1.0f - expf(0.190259f * logf(scaling))); + + return ret; +} + +#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED + +/* + Note parameters are as defined in the 1976 model. + These are slightly different from the ones in definitions.h +*/ +static const float radius_earth = 6356.766E3; // Earth's radius (in m) +static const float R_specific = 287.053072; // air specifc gas constant (J⋅kg−1⋅K−1) in 1976 model, R_universal / M_air; + +static const struct { + float amsl_m; // geopotential height above mean sea-level (km') + float temp_K; // Temperature (K) + float pressure_Pa; // Pressure (Pa) + float density; // Density (Pa/kg) + float temp_lapse; // Temperature gradients rates (K/m'), see page 3 +} atmospheric_1976_consts[] = { + { -5000, 320.650, 177687, 1.930467, -6.5E-3 }, + { 11000, 216.650, 22632.1, 0.363918, 0 }, + { 20000, 216.650, 5474.89, 8.80349E-2, 1E-3 }, + { 32000, 228.650, 868.019, 1.32250E-2, 2.8E-3 }, + { 47000, 270.650, 110.906, 1.42753E-3, 0 }, + { 51000, 270.650, 66.9389, 8.61606E-4, -2.8E-3 }, + { 71000, 214.650, 3.95642, 6.42110E-5, -2.0E-3 }, + { 84852, 186.946, 0.37338, 6.95788E-6, 0 }, +}; + +/* + find table entry given geopotential altitude in meters. This returns at least 1 + */ +static uint8_t find_atmosphere_layer_by_altitude(float alt_m) +{ + for (uint8_t idx = 1; idx < ARRAY_SIZE(atmospheric_1976_consts); idx++) { + if(alt_m < atmospheric_1976_consts[idx].amsl_m) { + return idx - 1; + } + } + + // Over the largest altitude return the last index + return ARRAY_SIZE(atmospheric_1976_consts) - 1; +} + +/* + find table entry given pressure (Pa). This returns at least 1 + */ +static uint8_t find_atmosphere_layer_by_pressure(float pressure) +{ + for (uint8_t idx = 1; idx < ARRAY_SIZE(atmospheric_1976_consts); idx++) { + if (atmospheric_1976_consts[idx].pressure_Pa < pressure) { + return idx - 1; + } + } + + // pressure is less than the smallest pressure return the last index + return ARRAY_SIZE(atmospheric_1976_consts) - 1; + +} + +// Convert geopotential altitude to geometric altitude +// +float AP_Baro::geopotential_alt_to_geometric(float alt) +{ + return (radius_earth * alt) / (radius_earth - alt); +} + +float AP_Baro::geometric_alt_to_geopotential(float alt) +{ + return (radius_earth * alt) / (radius_earth + alt); +} + +/* + Compute expected temperature for a given geometric altitude and altitude layer. +*/ +float AP_Baro::get_temperature_from_altitude(float alt) const +{ + alt = geometric_alt_to_geopotential(alt); + const uint8_t idx = find_atmosphere_layer_by_altitude(alt); + return get_temperature_by_altitude_layer(alt, idx); +} + +/* + Compute expected temperature for a given geopotential altitude and altitude layer. +*/ +float AP_Baro::get_temperature_by_altitude_layer(float alt, int8_t idx) +{ + if (is_zero(atmospheric_1976_consts[idx].temp_lapse)) { + return atmospheric_1976_consts[idx].temp_K; + } + return atmospheric_1976_consts[idx].temp_K + atmospheric_1976_consts[idx].temp_lapse * (alt - atmospheric_1976_consts[idx].amsl_m); +} + +/* + return geometric altitude (m) given a pressure (Pa) +*/ +float AP_Baro::get_altitude_from_pressure(float pressure) const +{ + const uint8_t idx = find_atmosphere_layer_by_pressure(pressure); + const float pressure_ratio = pressure / atmospheric_1976_consts[idx].pressure_Pa; + + // Pressure ratio is nonsensical return an error?? + if (!is_positive(pressure_ratio)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return get_altitude_AMSL(); + } + + float alt; + const float temp_slope = atmospheric_1976_consts[idx].temp_lapse; + if (is_zero(temp_slope)) { // Iso-thermal layer + const float fac = -(atmospheric_1976_consts[idx].temp_K * R_specific) / GRAVITY_MSS; + alt = atmospheric_1976_consts[idx].amsl_m + fac * logf(pressure_ratio); + } else { // Gradient temperature layer + const float fac = -(temp_slope * R_specific) / GRAVITY_MSS; + alt = atmospheric_1976_consts[idx].amsl_m + (atmospheric_1976_consts[idx].temp_K / atmospheric_1976_consts[idx].temp_lapse) * (powf(pressure_ratio, fac) - 1); + } + + return geopotential_alt_to_geometric(alt); +} + +/* + Compute expected pressure and temperature for a given geometric altitude. Used for SITL +*/ +void AP_Baro::get_pressure_temperature_for_alt_amsl(float alt_amsl, float &pressure, float &temperature_K) +{ + alt_amsl = geometric_alt_to_geopotential(alt_amsl); + + const uint8_t idx = find_atmosphere_layer_by_altitude(alt_amsl); + const float temp_slope = atmospheric_1976_consts[idx].temp_lapse; + temperature_K = get_temperature_by_altitude_layer(alt_amsl, idx); + + // Previous versions used the current baro temperature instead of an estimate we could try to incorporate this??? non-standard atmosphere + // const float temp = get_temperature(); + + if (is_zero(temp_slope)) { // Iso-thermal layer + const float fac = expf(-GRAVITY_MSS / (temperature_K * R_specific) * (alt_amsl - atmospheric_1976_consts[idx].amsl_m)); + pressure = atmospheric_1976_consts[idx].pressure_Pa * fac; + } else { // Gradient temperature layer + const float fac = GRAVITY_MSS / (temp_slope * R_specific); + const float temp_ratio = temperature_K / atmospheric_1976_consts[idx].temp_K; // temperature ratio [unitless] + pressure = atmospheric_1976_consts[idx].pressure_Pa * powf(temp_ratio, -fac); + } +} + +/* + return air density (kg/m^3), given geometric altitude (m) +*/ +float AP_Baro::get_air_density_for_alt_amsl(float alt_amsl) +{ + alt_amsl = geometric_alt_to_geopotential(alt_amsl); + + const uint8_t idx = find_atmosphere_layer_by_altitude(alt_amsl); + const float temp_slope = atmospheric_1976_consts[idx].temp_lapse; + const float temp = get_temperature_by_altitude_layer(alt_amsl, idx); + // const float temp = get_temperature(); + + float rho; + if (is_zero(temp_slope)) { // Iso-thermal layer + const float fac = expf(-GRAVITY_MSS / (temp * R_specific) * (alt_amsl - atmospheric_1976_consts[idx].amsl_m)); + rho = atmospheric_1976_consts[idx].density * fac; + } else { // Gradient temperature layer + const float fac = GRAVITY_MSS / (temp_slope * R_specific); + const float temp_ratio = temp / atmospheric_1976_consts[idx].temp_K; // temperature ratio [unitless] + rho = atmospheric_1976_consts[idx].density * powf(temp_ratio, -(fac + 1)); + } + return rho; +} + +/* + return current scale factor that converts from equivalent to true airspeed +*/ +float AP_Baro::get_EAS2TAS_extended(float altitude) const +{ + float density = get_air_density_for_alt_amsl(altitude); + if (!is_positive(density)) { + // above this height we are getting closer to spacecraft territory... + const uint8_t table_size = ARRAY_SIZE(atmospheric_1976_consts); + density = atmospheric_1976_consts[table_size-1].density; + } + return sqrtf(SSL_AIR_DENSITY / density); +} + +/* + Given the geometric altitude (m) + return scale factor that converts from equivalent to true airspeed + used by SITL only +*/ +float AP_Baro::get_EAS2TAS_for_alt_amsl(float alt_amsl) +{ + const float density = get_air_density_for_alt_amsl(alt_amsl); + return sqrtf(SSL_AIR_DENSITY / MAX(0.00001,density)); +} + +#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED + +/* + return geometric 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 +{ +#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED + const float alt1 = get_altitude_from_pressure(base_pressure); + const float alt2 = get_altitude_from_pressure(pressure); + return alt2 - alt1; +#else + return get_altitude_difference_simple(base_pressure, pressure); +#endif +} + +/* + return current scale factor that converts from equivalent to true airspeed + valid for altitudes up to 11km AMSL + assumes USA 1976 standard atmosphere lapse rate +*/ +float AP_Baro::get_EAS2TAS_simple(float altitude, float pressure) const +{ + 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 current scale factor that converts from equivalent to true airspeed +*/ +float AP_Baro::_get_EAS2TAS(void) const +{ + const float altitude = get_altitude_AMSL(); + +#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED + return get_EAS2TAS_extended(altitude); +#else + // otherwise use function + return get_EAS2TAS_simple(altitude, get_pressure()); +#endif +} + +// lookup expected temperature in degrees C for a given altitude. Used for SITL backend +float AP_Baro::get_temperatureC_for_alt_amsl(const float alt_amsl) +{ + float pressure, temp_K; + get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K); + return KELVIN_TO_C(temp_K); +} + +// lookup expected pressure in Pa for a given altitude. Used for SITL backend +float AP_Baro::get_pressure_for_alt_amsl(const float alt_amsl) +{ + float pressure, temp_K; + get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K); + return pressure; +} + +/* + return sea level pressure given a current altitude and pressure reading + this is the pressure p0 such that + get_altitude_difference(p0, pressure) == altitude + this function is used during calibration +*/ +float AP_Baro::get_sealevel_pressure(float pressure, float altitude) const +{ + const float min_pressure = 0.01; + const float max_pressure = 1e6; + float p0 = pressure; + /* + use a simple numerical gradient descent method so we don't need + the inverse function. This typically converges in about 5 steps, + we limit it to 20 steps to prevent possible high CPU usage + */ + uint16_t count = 20; + while (count--) { + const float delta = 0.1; + const float err1 = get_altitude_difference(p0, pressure) - altitude; + const float err2 = get_altitude_difference(p0+delta, pressure) - altitude; + const float dalt = err2 - err1; + if (fabsf(err1) < 0.01) { + break; + } + p0 -= err1 * delta / dalt; + p0 = constrain_float(p0, min_pressure, max_pressure); + } + return p0; +} diff --git a/libraries/AP_Baro/tests/test_baro_atmosphere.cpp b/libraries/AP_Baro/tests/test_baro_atmosphere.cpp new file mode 100644 index 0000000000..132d27372e --- /dev/null +++ b/libraries/AP_Baro/tests/test_baro_atmosphere.cpp @@ -0,0 +1,130 @@ +/* + test atmospheric model + to build, use: + ./waf configure --board sitl --debug + ./waf --target tests/test_baro_atmosphere + */ +#include + +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +TEST(AP_Baro, get_air_density_for_alt_amsl) +{ + float accuracy = 1E-6; + + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(-4000), 1.7697266, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(6000), 0.66011156, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(17500), 131.5688E-3, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(25000), 40.08393E-3, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(40000), 3.99567824728293E-3, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(49000), 1.16276961471707E-3, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(65000), 163.210100967015E-6, accuracy); + EXPECT_NEAR(AP_Baro::get_air_density_for_alt_amsl(78000), 25.2385188936996E-6, accuracy); +} + +TEST(AP_Baro, get_altitude_from_pressure) +{ + AP_Baro baro; + float accuracy = 1.5E-1; + + EXPECT_NEAR(baro.get_altitude_from_pressure(159598.2), -4000, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(47217.6), 6000, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(8182.3), 17500, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(2549.2), 25000, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(287.1441), 40000, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(90.3365), 49000, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(10.9297), 65000, accuracy); + EXPECT_NEAR(baro.get_altitude_from_pressure(1.4674), 78000, accuracy); +} + +TEST(AP_Baro, get_temperature_from_altitude) +{ + AP_Baro baro; + + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(-4000), 314.166370821781); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(6000), 249.186776458540); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(17500), 216.650000000000); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(25000), 221.552064726284); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(40000), 250.349646102421); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(49000), 270.650000000000); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(65000), 233.292172386848); + EXPECT_FLOAT_EQ(baro.get_temperature_from_altitude(78000), 202.540977853740); +} + +TEST(AP_Baro, geopotential_alt_to_geometric) +{ + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(-4002.51858796625), -4000); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(5994.34208330151), 6000.0); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(17451.9552525734), 17500); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(24902.0647262842), 25000); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(39749.8736080075), 40000); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(48625.1814380981), 49000); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(64342.0812904114), 65000); + EXPECT_FLOAT_EQ(AP_Baro::geopotential_alt_to_geometric(77054.5110731299), 78000); +} + +TEST(AP_Baro, geometric_alt_to_geopotential) +{ + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(-4000), -4002.51858796625); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(6000), 5994.34208330151); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(17500), 17451.9552525734); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(25000), 24902.0647262842); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(40000), 39749.8736080075); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(49000), 48625.1814380981); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(65000), 64342.0812904114); + EXPECT_FLOAT_EQ(AP_Baro::geometric_alt_to_geopotential(78000), 77054.5110731299); +} + +TEST(AP_Baro, get_pressure_temperature_for_alt_amsl) +{ + float accuracy = 1E-6; + + // layer 0 -4000 m + float pressure, temperature_K; + AP_Baro::get_pressure_temperature_for_alt_amsl(-4000, pressure, temperature_K); + // EXPECT_FLOAT_EQ(pressure, 159598.164433755); // Matlab + EXPECT_FLOAT_EQ(pressure, 159598.09375); + EXPECT_FLOAT_EQ(temperature_K, 314.166370821781); + + // layer 0: 6000 m + AP_Baro::get_pressure_temperature_for_alt_amsl(6000, pressure, temperature_K); + EXPECT_FLOAT_EQ(pressure, 47217.6489854302); + EXPECT_FLOAT_EQ(temperature_K, 249.186776458540); + + // layer 1: 17500 m + AP_Baro::get_pressure_temperature_for_alt_amsl(17500, pressure, temperature_K); + EXPECT_FLOAT_EQ(pressure, 8182.27857345022); + EXPECT_FLOAT_EQ(temperature_K, 216.650000000000); + + // layer 2: 25000m + AP_Baro::get_pressure_temperature_for_alt_amsl(25000, pressure, temperature_K); + // EXPECT_FLOAT_EQ(pressure, 2549.22361148236); // Matlab + EXPECT_FLOAT_EQ(pressure, 2549.2251); + EXPECT_FLOAT_EQ(temperature_K, 221.552064726284); + + // layer 3: 40000m + AP_Baro::get_pressure_temperature_for_alt_amsl(40000, pressure, temperature_K); + EXPECT_FLOAT_EQ(pressure, 287.144059695555); + EXPECT_FLOAT_EQ(temperature_K, 250.349646102421); + + // layer 4: 49000m + AP_Baro::get_pressure_temperature_for_alt_amsl(49000, pressure, temperature_K); + EXPECT_FLOAT_EQ(pressure, 90.3365441635632); + EXPECT_FLOAT_EQ(temperature_K, 270.650000000000); + + // layer 5: 65000m + AP_Baro::get_pressure_temperature_for_alt_amsl(65000, pressure, temperature_K); + // EXPECT_FLOAT_EQ(pressure, 10.9297197424037); // Matlab + EXPECT_FLOAT_EQ(pressure, 10.929715); + EXPECT_FLOAT_EQ(temperature_K, 233.292172386848); + + // layer 6: 78000m + AP_Baro::get_pressure_temperature_for_alt_amsl(78000, pressure, temperature_K); + // EXPECT_FLOAT_EQ(pressure, 1.46736727632119); // matlab + EXPECT_NEAR(pressure, 1.4673687, accuracy); + EXPECT_FLOAT_EQ(temperature_K, 202.540977853740); +} + +AP_GTEST_MAIN() diff --git a/libraries/AP_Baro/tests/wscript b/libraries/AP_Baro/tests/wscript new file mode 100644 index 0000000000..cd3e5e3ce7 --- /dev/null +++ b/libraries/AP_Baro/tests/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_find_tests( + use='ap', + )