mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: added atmospheric tables for high altitude flight
this gets altitude and EAS2TAS much more accurately up to around 150k feet AMSL. Enabled on boards using EKF double
This commit is contained in:
Andrew Tridgell
Peter Barker
parent
128d9b0e41
commit
f8ce6a8623
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,362 @@
|
|||
#include "AP_Baro.h"
|
||||
#include <AP_InternalError/AP_InternalError.h>
|
||||
|
||||
/*
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,130 @@
|
|||
/*
|
||||
test atmospheric model
|
||||
to build, use:
|
||||
./waf configure --board sitl --debug
|
||||
./waf --target tests/test_baro_atmosphere
|
||||
*/
|
||||
#include <AP_gtest.h>
|
||||
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
|
||||
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()
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
def build(bld):
|
||||
bld.ap_find_tests(
|
||||
use='ap',
|
||||
)
|
||||
Loading…
Reference in New Issue