From ecb78ca207daa42820e5e259e7d36dc451a2029a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 21 Oct 2023 19:25:45 +0200 Subject: [PATCH] new library for atmosphere calculations Signed-off-by: RomanBapst --- src/lib/CMakeLists.txt | 1 + src/lib/airspeed/airspeed.cpp | 30 +---- src/lib/airspeed/airspeed.h | 1 - src/lib/atmosphere/CMakeLists.txt | 37 +++++ src/lib/atmosphere/atmosphere.cpp | 81 +++++++++++ src/lib/atmosphere/atmosphere.h | 80 +++++++++++ src/lib/atmosphere/test_atmosphere.cpp | 126 ++++++++++++++++++ .../sensors/vehicle_air_data/CMakeLists.txt | 2 + .../vehicle_air_data/VehicleAirData.cpp | 38 ++---- .../vehicle_air_data/VehicleAirData.hpp | 2 - 10 files changed, 341 insertions(+), 57 deletions(-) create mode 100644 src/lib/atmosphere/CMakeLists.txt create mode 100644 src/lib/atmosphere/atmosphere.cpp create mode 100644 src/lib/atmosphere/atmosphere.h create mode 100644 src/lib/atmosphere/test_atmosphere.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8210be828059..1094ff33c10a 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) +add_subdirectory(atmosphere EXCLUDE_FROM_ALL) add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 24f7f018509c..4d29be4a008f 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -44,6 +44,9 @@ #include #include +#include + +using atmosphere::getDensityFromPressureAndTemp; float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) @@ -53,7 +56,7 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ } // air density in kg/m3 - const float rho_air = get_air_density(pressure_ambient, temperature_celsius); + const float rho_air = getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius); const float dp = fabsf(differential_pressure); float dp_tot = dp; @@ -153,18 +156,6 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ break; } - // if (!PX4_ISFINITE(dp_tube)) { - // dp_tube = 0.0f; - // } - - // if (!PX4_ISFINITE(dp_pitot)) { - // dp_pitot = 0.0f; - // } - - // if (!PX4_ISFINITE(dv)) { - // dv = 0.0f; - // } - // computed airspeed without correction for inflow-speed at tip of pitot-tube const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); @@ -192,7 +183,7 @@ float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float te temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius } - return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius)); } @@ -203,7 +194,7 @@ float calc_CAS_from_IAS(float speed_indicated, float scale) float calc_TAS(float total_pressure, float static_pressure, float temperature_celsius) { - float density = get_air_density(static_pressure, temperature_celsius); + float density = getDensityFromPressureAndTemp(static_pressure, temperature_celsius); if (density < 0.0001f || !PX4_ISFINITE(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; @@ -219,15 +210,6 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce } } -float get_air_density(float static_pressure, float temperature_celsius) -{ - if (!PX4_ISFINITE(temperature_celsius)) { - temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius - } - - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} - float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index dba000a4e509..6b5161c5ddde 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -127,7 +127,6 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe * @param static_pressure ambient pressure in millibar * @param temperature_celcius air / ambient temperature in Celsius */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); /** * @brief Calculates calibrated airspeed from true airspeed and air density diff --git a/src/lib/atmosphere/CMakeLists.txt b/src/lib/atmosphere/CMakeLists.txt new file mode 100644 index 000000000000..eb25c8df4b75 --- /dev/null +++ b/src/lib/atmosphere/CMakeLists.txt @@ -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) diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp new file mode 100644 index 000000000000..66b98626f7d5 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.cpp @@ -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 +#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; +} +} \ No newline at end of file diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h new file mode 100644 index 000000000000..849a4303470b --- /dev/null +++ b/src/lib/atmosphere/atmosphere.h @@ -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_ diff --git a/src/lib/atmosphere/test_atmosphere.cpp b/src/lib/atmosphere/test_atmosphere.cpp new file mode 100644 index 000000000000..0a79c89ca11d --- /dev/null +++ b/src/lib/atmosphere/test_atmosphere.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * To run this test only use: make tests TESTFILTER=atmosphere + */ + +#include +#include +using namespace atmosphere; + +TEST(TestAtmosphere, pressureFromAltitude) +{ + // GIVEN pressure at seal level in standard atmosphere and sea level altitude + const float pressureAtSeaLevel = 101325.f; // Pa + float altitude = 0.0f; + + // WHEN we calculate pressure based on altitude + float pressure = getPressureFromAltitude(altitude); + + // THEN expect seal level pressure for standard atmosphere + EXPECT_FLOAT_EQ(pressure, pressureAtSeaLevel); + + // WHEN we are at 3000m altitude + altitude = 3000.0f; + pressure = getPressureFromAltitude(altitude); + + // THEN expect standard atmosphere pressure at 3000m (error of 10Pa corresponds to 1m altitude error in standard atmosphere when altitude is between 1000 and 2000m) + EXPECT_NEAR(pressure, 70120.f, 10.0f); +} + +TEST(TestAtmosphere, altitudeFromPressure) +{ + // GIVEN pressure at seal level in standard atmosphere + const float pressureAtSealevel = 101325.f; + float pressure = pressureAtSealevel; + + // WHEN we calculate altitude from pressure + float altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect resulting altitude to be near sea level + EXPECT_FLOAT_EQ(altitude, 0.0f); + + // GIVEN pressure is standard atmosphere pressure at 3000m + pressure = 70109.f; + + // WHEN we calculate altitude from pressure + altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect altitude to be near 3000m + EXPECT_NEAR(altitude, 3000.0f, 0.5f); +} + +TEST(TestAtmosphere, DensityFromPressure) +{ +// GIVEN standard atmosphere at sea level + const float pressureAtSealevel = 101325.f; + const float tempSeaLevel = 15.f; + + // WHEN we calculate density from pressure and temperature + float density = getDensityFromPressureAndTemp(pressureAtSealevel, tempSeaLevel); + + // THEN expect density at sea level in standard atmosphere + EXPECT_NEAR(density, 1.225f, 0.001f); + + // GIVEN standard atmosphere at 3000m + const float pressure = 70109.f; + const float tempAt3000m = -4.5f; + + // WHEN we calculate density from pressure and temperature + density = getDensityFromPressureAndTemp(pressure, tempAt3000m); + + // THEN expect density at 3000m in standard atmosphere + EXPECT_NEAR(density, 0.9091f, 0.001f); +} + +TEST(TestAtmosphere, StandardTemperature) +{ + // GIVEN standard atmosphere at sea level + float altitude = 0.f; + + // WHEN we calculate standard temperature from altitude + float temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at sea level + EXPECT_NEAR(temperature, 15.f, 0.001f); + + // GIVEN standard atmosphere at 3000m + altitude = 3000.f; + + // WHEN we calculate standard temperature from altitude + temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at 3000m + EXPECT_NEAR(temperature, -4.5f, 0.001f); +} \ No newline at end of file diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt index c8457cc2df24..909f249a7c01 100644 --- a/src/modules/sensors/vehicle_air_data/CMakeLists.txt +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -40,4 +40,6 @@ target_link_libraries(vehicle_air_data data_validator px4_work_queue sensor_calibration + PUBLIC + atmosphere ) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 472d228c580c..4c15c74a2a0a 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -36,11 +36,14 @@ #include #include #include +#include + namespace sensors { using namespace matrix; +using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; @@ -189,8 +192,9 @@ void VehicleAirData::Run() // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; - float data_array[3] {pressure_corrected, report.temperature, PressureToAltitude(pressure_corrected)}; + float data_array[3] {pressure_corrected, report.temperature, getAltitudeFromPressure(pressure_corrected, pressure_sealevel_pa)}; _voter.put(uorb_index, report.timestamp, data_array, report.error_count, _priority[uorb_index]); _timestamp_sample_sum[uorb_index] += report.timestamp_sample; @@ -251,11 +255,11 @@ void VehicleAirData::Run() const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; - float altitude = PressureToAltitude(pressure_pa, temperature); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; + const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - float air_density = pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius - - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; @@ -295,32 +299,6 @@ void VehicleAirData::Run() perf_end(_cycle_perf); } -float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const -{ - // calculate altitude using the hypsometric equation - static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin - static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre - - // current pressure at MSL in kPa (QNH in hPa) - const float p1 = _param_sens_baro_qnh.get() * 0.1f; - - // measured pressure in kPa - const float p = pressure_pa * 0.001f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; - - return altitude; -} - void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us) { // check failover and report (save failover report for a cycle where parameters didn't update) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 390f25767f87..fd28de34f8c6 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -79,8 +79,6 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem bool ParametersUpdate(bool force = false); void UpdateStatus(); - float PressureToAltitude(float pressure_pa, float temperature = 15.f) const; - static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _sensors_status_baro_pub{ORB_ID(sensors_status_baro)};