Line data Source code
1 : #include "AP_Baro.h"
2 : #include <AP_InternalError/AP_InternalError.h>
3 :
4 : /*
5 : This program is free software: you can redistribute it and/or modify
6 : it under the terms of the GNU General Public License as published by
7 : the Free Software Foundation, either version 3 of the License, or
8 : (at your option) any later version.
9 :
10 : This program is distributed in the hope that it will be useful,
11 : but WITHOUT ANY WARRANTY; without even the implied warranty of
12 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 : GNU General Public License for more details.
14 :
15 : You should have received a copy of the GNU General Public License
16 : along with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : /* 1976 U.S. Standard Atmosphere: https://ntrs.nasa.gov/api/citations/19770009539/downloads/19770009539.pdf?attachment=true
20 :
21 : The US Standard Atmosphere defines the atmopshere in terms of whether the temperature is an iso-thermal or gradient layer.
22 : Ideal gas laws apply thus P = rho * R_specific * T : P = pressure, rho = density, R_specific = air gas constant, T = temperature
23 :
24 : Note: the 1976 model is the same as the 1962 US Standard Atomsphere up to 51km.
25 : R_universal: the universal gas constant is slightly off in the 1976 model and thus R_specific is different than today's value
26 :
27 : */
28 :
29 : /* Model Constants
30 : 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
31 : 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)
32 : M_air = 28.9644 // Molecular weight of air (kg/kmol) see page 3
33 : R_specific = 287.053072 // air specifc gas constant (J⋅kg−1⋅K−1), R_universal / M_air;
34 : gama = 1.4; // specific heat ratio of air used to determine the speed of sound
35 :
36 : R0 = 6356.766E3; // Earth's radius (in m)
37 : g0 = 9.80665; // gravity (m/s^2)
38 :
39 : Sea-Level Constants
40 : H_asml = 0 meters
41 : T0 = 288.150 K
42 : P0 = 101325 Pa
43 : rho0 = 1.2250 kg/m^3
44 : T0_slope = -6.5E-3 (K/m')
45 :
46 : The tables list altitudes -5 km to 0 km using the same equations as 0 km to 11 km.
47 : */
48 :
49 : /*
50 : return altitude difference in meters between current pressure and a
51 : given base_pressure in Pascal. This is a simple atmospheric model
52 : good to about 11km AMSL.
53 : */
54 0 : float AP_Baro::get_altitude_difference_simple(float base_pressure, float pressure) const
55 : {
56 : float ret;
57 0 : float temp_K = C_TO_KELVIN(get_ground_temperature());
58 0 : float scaling = pressure / base_pressure;
59 :
60 : // This is an exact calculation that is within +-2.5m of the standard
61 : // atmosphere tables in the troposphere (up to 11,000 m amsl).
62 0 : ret = 153.8462f * temp_K * (1.0f - expf(0.190259f * logf(scaling)));
63 :
64 0 : return ret;
65 : }
66 :
67 : #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
68 :
69 : /*
70 : Note parameters are as defined in the 1976 model.
71 : These are slightly different from the ones in definitions.h
72 : */
73 : static const float radius_earth = 6356.766E3; // Earth's radius (in m)
74 : static const float R_specific = 287.053072; // air specifc gas constant (J⋅kg−1⋅K−1) in 1976 model, R_universal / M_air;
75 :
76 : static const struct {
77 : float amsl_m; // geopotential height above mean sea-level (km')
78 : float temp_K; // Temperature (K)
79 : float pressure_Pa; // Pressure (Pa)
80 : float density; // Density (Pa/kg)
81 : float temp_lapse; // Temperature gradients rates (K/m'), see page 3
82 : } atmospheric_1976_consts[] = {
83 : { -5000, 320.650, 177687, 1.930467, -6.5E-3 },
84 : { 11000, 216.650, 22632.1, 0.363918, 0 },
85 : { 20000, 216.650, 5474.89, 8.80349E-2, 1E-3 },
86 : { 32000, 228.650, 868.019, 1.32250E-2, 2.8E-3 },
87 : { 47000, 270.650, 110.906, 1.42753E-3, 0 },
88 : { 51000, 270.650, 66.9389, 8.61606E-4, -2.8E-3 },
89 : { 71000, 214.650, 3.95642, 6.42110E-5, -2.0E-3 },
90 : { 84852, 186.946, 0.37338, 6.95788E-6, 0 },
91 : };
92 :
93 : /*
94 : find table entry given geopotential altitude in meters. This returns at least 1
95 : */
96 620094386 : static uint8_t find_atmosphere_layer_by_altitude(float alt_m)
97 : {
98 620094512 : for (uint8_t idx = 1; idx < ARRAY_SIZE(atmospheric_1976_consts); idx++) {
99 620094512 : if(alt_m < atmospheric_1976_consts[idx].amsl_m) {
100 620094386 : return idx - 1;
101 : }
102 : }
103 :
104 : // Over the largest altitude return the last index
105 0 : return ARRAY_SIZE(atmospheric_1976_consts) - 1;
106 : }
107 :
108 : /*
109 : find table entry given pressure (Pa). This returns at least 1
110 : */
111 9488736 : static uint8_t find_atmosphere_layer_by_pressure(float pressure)
112 : {
113 9498630 : for (uint8_t idx = 1; idx < ARRAY_SIZE(atmospheric_1976_consts); idx++) {
114 9498630 : if (atmospheric_1976_consts[idx].pressure_Pa < pressure) {
115 9488736 : return idx - 1;
116 : }
117 : }
118 :
119 : // pressure is less than the smallest pressure return the last index
120 0 : return ARRAY_SIZE(atmospheric_1976_consts) - 1;
121 :
122 : }
123 :
124 : // Convert geopotential altitude to geometric altitude
125 : //
126 9488752 : float AP_Baro::geopotential_alt_to_geometric(float alt)
127 : {
128 9488752 : return (radius_earth * alt) / (radius_earth - alt);
129 : }
130 :
131 620094402 : float AP_Baro::geometric_alt_to_geopotential(float alt)
132 : {
133 620094402 : return (radius_earth * alt) / (radius_earth + alt);
134 : }
135 :
136 : /*
137 : Compute expected temperature for a given geometric altitude and altitude layer.
138 : */
139 16 : float AP_Baro::get_temperature_from_altitude(float alt) const
140 : {
141 16 : alt = geometric_alt_to_geopotential(alt);
142 16 : const uint8_t idx = find_atmosphere_layer_by_altitude(alt);
143 16 : return get_temperature_by_altitude_layer(alt, idx);
144 : }
145 :
146 : /*
147 : Compute expected temperature for a given geopotential altitude and altitude layer.
148 : */
149 620094386 : float AP_Baro::get_temperature_by_altitude_layer(float alt, int8_t idx)
150 : {
151 620094386 : if (is_zero(atmospheric_1976_consts[idx].temp_lapse)) {
152 12 : return atmospheric_1976_consts[idx].temp_K;
153 : }
154 620094374 : return atmospheric_1976_consts[idx].temp_K + atmospheric_1976_consts[idx].temp_lapse * (alt - atmospheric_1976_consts[idx].amsl_m);
155 : }
156 :
157 : /*
158 : return geometric altitude (m) given a pressure (Pa)
159 : */
160 9488736 : float AP_Baro::get_altitude_from_pressure(float pressure) const
161 : {
162 9488736 : const uint8_t idx = find_atmosphere_layer_by_pressure(pressure);
163 9488736 : const float pressure_ratio = pressure / atmospheric_1976_consts[idx].pressure_Pa;
164 :
165 : // Pressure ratio is nonsensical return an error??
166 9488736 : if (!is_positive(pressure_ratio)) {
167 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
168 0 : return get_altitude_AMSL();
169 : }
170 :
171 : float alt;
172 9488736 : const float temp_slope = atmospheric_1976_consts[idx].temp_lapse;
173 9488736 : if (is_zero(temp_slope)) { // Iso-thermal layer
174 9856 : const float fac = -(atmospheric_1976_consts[idx].temp_K * R_specific) / GRAVITY_MSS;
175 9856 : alt = atmospheric_1976_consts[idx].amsl_m + fac * logf(pressure_ratio);
176 : } else { // Gradient temperature layer
177 9478880 : const float fac = -(temp_slope * R_specific) / GRAVITY_MSS;
178 9478880 : alt = atmospheric_1976_consts[idx].amsl_m + (atmospheric_1976_consts[idx].temp_K / atmospheric_1976_consts[idx].temp_lapse) * (powf(pressure_ratio, fac) - 1);
179 : }
180 :
181 9488736 : return geopotential_alt_to_geometric(alt);
182 : }
183 :
184 : /*
185 : Compute expected pressure and temperature for a given geometric altitude. Used for SITL
186 : */
187 33381291 : void AP_Baro::get_pressure_temperature_for_alt_amsl(float alt_amsl, float &pressure, float &temperature_K)
188 : {
189 33381291 : alt_amsl = geometric_alt_to_geopotential(alt_amsl);
190 :
191 33381291 : const uint8_t idx = find_atmosphere_layer_by_altitude(alt_amsl);
192 33381291 : const float temp_slope = atmospheric_1976_consts[idx].temp_lapse;
193 33381291 : temperature_K = get_temperature_by_altitude_layer(alt_amsl, idx);
194 :
195 : // Previous versions used the current baro temperature instead of an estimate we could try to incorporate this??? non-standard atmosphere
196 : // const float temp = get_temperature();
197 :
198 33381291 : if (is_zero(temp_slope)) { // Iso-thermal layer
199 4 : const float fac = expf(-GRAVITY_MSS / (temperature_K * R_specific) * (alt_amsl - atmospheric_1976_consts[idx].amsl_m));
200 4 : pressure = atmospheric_1976_consts[idx].pressure_Pa * fac;
201 : } else { // Gradient temperature layer
202 33381287 : const float fac = GRAVITY_MSS / (temp_slope * R_specific);
203 33381287 : const float temp_ratio = temperature_K / atmospheric_1976_consts[idx].temp_K; // temperature ratio [unitless]
204 33381287 : pressure = atmospheric_1976_consts[idx].pressure_Pa * powf(temp_ratio, -fac);
205 : }
206 33381291 : }
207 :
208 : /*
209 : return air density (kg/m^3), given geometric altitude (m)
210 : */
211 586713079 : float AP_Baro::get_air_density_for_alt_amsl(float alt_amsl)
212 : {
213 586713079 : alt_amsl = geometric_alt_to_geopotential(alt_amsl);
214 :
215 586713079 : const uint8_t idx = find_atmosphere_layer_by_altitude(alt_amsl);
216 586713079 : const float temp_slope = atmospheric_1976_consts[idx].temp_lapse;
217 586713079 : const float temp = get_temperature_by_altitude_layer(alt_amsl, idx);
218 : // const float temp = get_temperature();
219 :
220 : float rho;
221 586713079 : if (is_zero(temp_slope)) { // Iso-thermal layer
222 4 : const float fac = expf(-GRAVITY_MSS / (temp * R_specific) * (alt_amsl - atmospheric_1976_consts[idx].amsl_m));
223 4 : rho = atmospheric_1976_consts[idx].density * fac;
224 : } else { // Gradient temperature layer
225 586713075 : const float fac = GRAVITY_MSS / (temp_slope * R_specific);
226 586713075 : const float temp_ratio = temp / atmospheric_1976_consts[idx].temp_K; // temperature ratio [unitless]
227 586713075 : rho = atmospheric_1976_consts[idx].density * powf(temp_ratio, -(fac + 1));
228 : }
229 586713079 : return rho;
230 : }
231 :
232 : /*
233 : return current scale factor that converts from equivalent to true airspeed
234 : */
235 1535898 : float AP_Baro::get_EAS2TAS_extended(float altitude) const
236 : {
237 1535898 : float density = get_air_density_for_alt_amsl(altitude);
238 1535898 : if (!is_positive(density)) {
239 : // above this height we are getting closer to spacecraft territory...
240 0 : const uint8_t table_size = ARRAY_SIZE(atmospheric_1976_consts);
241 0 : density = atmospheric_1976_consts[table_size-1].density;
242 : }
243 1535898 : return sqrtf(SSL_AIR_DENSITY / density);
244 : }
245 :
246 : /*
247 : Given the geometric altitude (m)
248 : return scale factor that converts from equivalent to true airspeed
249 : used by SITL only
250 : */
251 169080479 : float AP_Baro::get_EAS2TAS_for_alt_amsl(float alt_amsl)
252 : {
253 169080479 : const float density = get_air_density_for_alt_amsl(alt_amsl);
254 169080479 : return sqrtf(SSL_AIR_DENSITY / MAX(0.00001,density));
255 : }
256 :
257 : #endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
258 :
259 : /*
260 : return geometric altitude difference in meters between current pressure and a
261 : given base_pressure in Pascal.
262 : */
263 4744360 : float AP_Baro::get_altitude_difference(float base_pressure, float pressure) const
264 : {
265 : #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
266 4744360 : const float alt1 = get_altitude_from_pressure(base_pressure);
267 4744360 : const float alt2 = get_altitude_from_pressure(pressure);
268 4744360 : return alt2 - alt1;
269 : #else
270 : return get_altitude_difference_simple(base_pressure, pressure);
271 : #endif
272 : }
273 :
274 : /*
275 : return current scale factor that converts from equivalent to true airspeed
276 : valid for altitudes up to 11km AMSL
277 : assumes USA 1976 standard atmosphere lapse rate
278 : */
279 0 : float AP_Baro::get_EAS2TAS_simple(float altitude, float pressure) const
280 : {
281 0 : if (is_zero(pressure)) {
282 0 : return 1.0f;
283 : }
284 :
285 : // only estimate lapse rate for the difference from the ground location
286 : // provides a more consistent reading then trying to estimate a complete
287 : // ISA model atmosphere
288 0 : float tempK = C_TO_KELVIN(get_ground_temperature()) - ISA_LAPSE_RATE * altitude;
289 0 : const float eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK));
290 0 : if (!is_positive(eas2tas_squared)) {
291 0 : return 1.0f;
292 : }
293 0 : return sqrtf(eas2tas_squared);
294 : }
295 :
296 : /*
297 : return current scale factor that converts from equivalent to true airspeed
298 : */
299 1535898 : float AP_Baro::_get_EAS2TAS(void) const
300 : {
301 1535898 : const float altitude = get_altitude_AMSL();
302 :
303 : #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
304 1535898 : return get_EAS2TAS_extended(altitude);
305 : #else
306 : // otherwise use function
307 : return get_EAS2TAS_simple(altitude, get_pressure());
308 : #endif
309 : }
310 :
311 : #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED || AP_SIM_ENABLED
312 : // lookup expected temperature in degrees C for a given altitude. Used for SITL backend
313 3174284 : float AP_Baro::get_temperatureC_for_alt_amsl(const float alt_amsl)
314 : {
315 : float pressure, temp_K;
316 3174284 : get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K);
317 3174284 : return KELVIN_TO_C(temp_K);
318 : }
319 :
320 : // lookup expected pressure in Pa for a given altitude. Used for SITL backend
321 99653 : float AP_Baro::get_pressure_for_alt_amsl(const float alt_amsl)
322 : {
323 : float pressure, temp_K;
324 99653 : get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K);
325 99653 : return pressure;
326 : }
327 : #endif // AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
328 :
329 : /*
330 : return sea level pressure given a current altitude and pressure reading
331 : this is the pressure p0 such that
332 : get_altitude_difference(p0, pressure) == altitude
333 : this function is used during calibration
334 : */
335 39916 : float AP_Baro::get_sealevel_pressure(float pressure, float altitude) const
336 : {
337 39916 : const float min_pressure = 0.01;
338 39916 : const float max_pressure = 1e6;
339 39916 : float p0 = pressure;
340 : /*
341 : use a simple numerical gradient descent method so we don't need
342 : the inverse function. This typically converges in about 5 steps,
343 : we limit it to 20 steps to prevent possible high CPU usage
344 : */
345 39916 : uint16_t count = 20;
346 179722 : while (count--) {
347 179722 : const float delta = 0.1;
348 179722 : const float err1 = get_altitude_difference(p0, pressure) - altitude;
349 179722 : const float err2 = get_altitude_difference(p0+delta, pressure) - altitude;
350 179722 : const float dalt = err2 - err1;
351 179722 : if (fabsf(err1) < 0.01) {
352 39916 : break;
353 : }
354 139806 : p0 -= err1 * delta / dalt;
355 139806 : p0 = constrain_float(p0, min_pressure, max_pressure);
356 : }
357 39916 : return p0;
358 : }
|
