-
Notifications
You must be signed in to change notification settings - Fork 13.6k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
new library for atmosphere calculations
Signed-off-by: RomanBapst <[email protected]>
- Loading branch information
1 parent
f120ebc
commit ecb78ca
Showing
10 changed files
with
341 additions
and
57 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,37 @@ | ||
############################################################################ | ||
# | ||
# Copyright (c) 2023 PX4 Development Team. All rights reserved. | ||
# | ||
# Redistribution and use in source and binary forms, with or without | ||
# modification, are permitted provided that the following conditions | ||
# are met: | ||
# | ||
# 1. Redistributions of source code must retain the above copyright | ||
# notice, this list of conditions and the following disclaimer. | ||
# 2. Redistributions in binary form must reproduce the above copyright | ||
# notice, this list of conditions and the following disclaimer in | ||
# the documentation and/or other materials provided with the | ||
# distribution. | ||
# 3. Neither the name PX4 nor the names of its contributors may be | ||
# used to endorse or promote products derived from this software | ||
# without specific prior written permission. | ||
# | ||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS | ||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED | ||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
# POSSIBILITY OF SUCH DAMAGE. | ||
# | ||
############################################################################ | ||
|
||
px4_add_library(atmosphere | ||
atmosphere.cpp | ||
) | ||
px4_add_unit_gtest(SRC test_atmosphere.cpp LINKLIBS atmosphere) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,81 @@ | ||
/**************************************************************************** | ||
* | ||
* Copyright (C) 2023 PX4 Development Team. All rights reserved. | ||
* | ||
* Redistribution and use in source and binary forms, with or without | ||
* modification, are permitted provided that the following conditions | ||
* are met: | ||
* | ||
* 1. Redistributions of source code must retain the above copyright | ||
* notice, this list of conditions and the following disclaimer. | ||
* 2. Redistributions in binary form must reproduce the above copyright | ||
* notice, this list of conditions and the following disclaimer in | ||
* the documentation and/or other materials provided with the | ||
* distribution. | ||
* 3. Neither the name PX4 nor the names of its contributors may be | ||
* used to endorse or promote products derived from this software | ||
* without specific prior written permission. | ||
* | ||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS | ||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED | ||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
* POSSIBILITY OF SUCH DAMAGE. | ||
* | ||
****************************************************************************/ | ||
|
||
/** | ||
* @file atmosphere.cpp | ||
* | ||
*/ | ||
|
||
#include <geo/geo.h> | ||
#include "atmosphere.h" | ||
namespace atmosphere | ||
{ | ||
|
||
static constexpr float kTempRefKelvin = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin | ||
static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter | ||
static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa | ||
|
||
float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius) | ||
{ | ||
return (pressure_pa / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS))); | ||
} | ||
float getPressureFromAltitude(const float altitude_m) | ||
{ | ||
|
||
return kPressRefSeaLevelPa * powf((altitude_m * kTempGradient + kTempRefKelvin) / kTempRefKelvin, | ||
-CONSTANTS_ONE_G / (kTempGradient * CONSTANTS_AIR_GAS_CONST)); | ||
} | ||
float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa) | ||
{ | ||
// calculate altitude using the hypsometric equation | ||
|
||
const float pressure_ratio = pressure_pa / pressure_sealevel_pa; | ||
|
||
/* | ||
* Solve: | ||
* | ||
* / -(aR / g) \ | ||
* | (p / p1) . T1 | - T1 | ||
* \ / | ||
* h = ------------------------------- + h1 | ||
* a | ||
*/ | ||
return (((powf(pressure_ratio, (-(kTempGradient * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * kTempRefKelvin) - | ||
kTempRefKelvin) / kTempGradient; | ||
|
||
} | ||
float getStandardTemperatureAtAltitude(float altitude_m) | ||
{ | ||
return 15.0f + kTempGradient * altitude_m; | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,80 @@ | ||
/**************************************************************************** | ||
* | ||
* Copyright (c) 2023 PX4 Development Team. All rights reserved. | ||
* | ||
* Redistribution and use in source and binary forms, with or without | ||
* modification, are permitted provided that the following conditions | ||
* are met: | ||
* | ||
* 1. Redistributions of source code must retain the above copyright | ||
* notice, this list of conditions and the following disclaimer. | ||
* 2. Redistributions in binary form must reproduce the above copyright | ||
* notice, this list of conditions and the following disclaimer in | ||
* the documentation and/or other materials provided with the | ||
* distribution. | ||
* 3. Neither the name PX4 nor the names of its contributors may be | ||
* used to endorse or promote products derived from this software | ||
* without specific prior written permission. | ||
* | ||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS | ||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED | ||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
* POSSIBILITY OF SUCH DAMAGE. | ||
* | ||
****************************************************************************/ | ||
|
||
/** | ||
* @file atmosphere.h | ||
* | ||
*/ | ||
|
||
#ifndef PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ | ||
#define PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ | ||
|
||
namespace atmosphere | ||
{ | ||
|
||
// NOTE: We are currently only modelling the first layer of the US Standard Atmosphere 1976. | ||
// This means that the functions are only valid up to an altitude of 11km. | ||
|
||
/** | ||
* Calculate air density given air pressure and temperature. | ||
* | ||
* @param pressure_pa ambient pressure in Pa | ||
* @param temperature_celsius ambient temperature in degrees Celsius | ||
*/ | ||
float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius); | ||
|
||
/** | ||
* Calculate standard airpressure given altitude. | ||
* | ||
* @param altitude_m altitude above MSL in meters in the standard atmosphere | ||
*/ | ||
float getPressureFromAltitude(const float altitude_m); | ||
|
||
/** | ||
* Calculate altitude from air pressure and temperature. | ||
* | ||
* @param pressure_pa ambient pressure in Pa | ||
* @param pressure_sealevel_pa sea level pressure in Pa | ||
*/ | ||
float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa); | ||
|
||
/** | ||
* Get standard temperature at altitude. | ||
* | ||
* @param altitude_m Altitude msl in meters | ||
* @return Standard temperature in degrees Celsius | ||
*/ | ||
float getStandardTemperatureAtAltitude(float altitude_m); | ||
} | ||
|
||
#endif //PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ |
Oops, something went wrong.