mirror of https://github.com/ArduPilot/ardupilot
359 lines
14 KiB
C++
359 lines
14 KiB
C++
#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.
|
||
*/
|
||
|
||
/*
|
||
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
|
||
}
|
||
|
||
#if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
|
||
// 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;
|
||
}
|
||
#endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
|
||
|
||
/*
|
||
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;
|
||
}
|