From 36267f657376c0c831060b7fe38484edd1a994a6 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 30 Oct 2023 14:07:10 +0100 Subject: [PATCH] use atmosphere lib instead of geo lib Signed-off-by: RomanBapst --- src/lib/airspeed/airspeed.cpp | 14 ++++---- src/lib/atmosphere/atmosphere.cpp | 2 +- src/lib/atmosphere/atmosphere.h | 4 +-- src/modules/commander/baro_calibration.cpp | 33 +++---------------- src/modules/ekf2/EKF/drag_fusion.cpp | 3 +- src/modules/ekf2/EKF/estimator_interface.h | 4 +-- .../ekf2/test/test_EKF_drag_fusion.cpp | 7 ++-- .../FixedwingPositionControl.hpp | 5 +-- src/modules/simulation/simulator_sih/sih.hpp | 4 +-- .../vtol_att_control/vtol_att_control_main.h | 4 +-- src/modules/vtol_att_control/vtol_type.cpp | 3 +- 11 files changed, 31 insertions(+), 52 deletions(-) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 4d29be4a008f..b563ecd3b5d0 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -43,10 +43,10 @@ #include "airspeed.h" #include -#include #include using atmosphere::getDensityFromPressureAndTemp; +using atmosphere::kAirDensitySeaLevelStandardAtmos; 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) @@ -157,7 +157,7 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ } // 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); + const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / kAirDensitySeaLevelStandardAtmos); // corrected airspeed const float airspeed_corrected = airspeed_uncorrected + dv; @@ -169,10 +169,10 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ float calc_IAS(float differential_pressure) { if (differential_pressure > 0.0f) { - return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return sqrtf((2.0f * differential_pressure) / kAirDensitySeaLevelStandardAtmos); } else { - return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return -sqrtf((2.0f * fabsf(differential_pressure)) / kAirDensitySeaLevelStandardAtmos); } } @@ -183,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 / getDensityFromPressureAndTemp(pressure_ambient, + return speed_calibrated * sqrtf(kAirDensitySeaLevelStandardAtmos / getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius)); } @@ -197,7 +197,7 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce float density = getDensityFromPressureAndTemp(static_pressure, temperature_celsius); if (density < 0.0001f || !PX4_ISFINITE(density)) { - density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + density = kAirDensitySeaLevelStandardAtmos; } float pressure_difference = total_pressure - static_pressure; @@ -212,5 +212,5 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { - return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return speed_true * sqrtf(air_density / kAirDensitySeaLevelStandardAtmos); } diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp index 5500568a2b39..5fc22ddf7c5d 100644 --- a/src/lib/atmosphere/atmosphere.cpp +++ b/src/lib/atmosphere/atmosphere.cpp @@ -44,7 +44,7 @@ namespace atmosphere float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius) { - return (pressure_pa / (kAirGasConstant * (temperature_celsius - kAbsoluteNullCelcius))); + return (pressure_pa / (kAirGasConstant * (temperature_celsius - kAbsoluteNullCelsius))); } float getPressureFromAltitude(const float altitude_m) { diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h index 72e85b8f3508..eeca58422d78 100644 --- a/src/lib/atmosphere/atmosphere.h +++ b/src/lib/atmosphere/atmosphere.h @@ -51,8 +51,8 @@ static constexpr float kAirDensitySeaLevelStandardAtmos = 1.225f; // kg/m^3 static constexpr float kAirDensityStandardAtmos11000Amsl = 0.3639; static constexpr float kAirGasConstant = 287.1f; // J/(kg * K) -static constexpr float kAbsoluteNullCelcius = -273.15f; // °C -static constexpr float kTempRefKelvin = 15.f - kAbsoluteNullCelcius; // temperature at base height in Kelvin +static constexpr float kAbsoluteNullCelsius = -273.15f; // °C +static constexpr float kTempRefKelvin = 15.f - kAbsoluteNullCelsius; // 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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 2a027920a2c1..ab5267d2d6b4 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -60,37 +60,12 @@ using namespace matrix; using namespace time_literals; +using namespace atmosphere; static constexpr char sensor_name[] {"baro"}; static constexpr int MAX_SENSOR_COUNT = 4; -static float PressureToAltitude(float pressure_pa, float temperature) -{ - // 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 = 1013.25f * 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; -} - int do_baro_calibration(orb_advert_t *mavlink_log_pub) { calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); @@ -172,7 +147,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub) const float pressure_pa = data_sum[instance] / data_sum_count[instance]; const float temperature = temperature_sum[instance] / data_sum_count[instance]; - float pressure_altitude = PressureToAltitude(pressure_pa, temperature); + float pressure_altitude = getAltitudeFromPressure(pressure_pa, temperature); // Use GPS altitude as a reference to compute the baro bias measurement const float baro_bias = pressure_altitude - gps_altitude; @@ -189,7 +164,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub) // perform a binary search while (front <= last) { middle = front + (last - front) / 2; - float altitude_calibrated = PressureToAltitude(pressure_pa - middle, temperature); + float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, temperature); if (altitude_calibrated > altitude + 0.1f) { last = middle; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index ca858cd06b68..a7987ab0a244 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -41,6 +41,7 @@ #include #include +#include void Ekf::controlDragFusion(const imuSample &imu_delayed) { @@ -67,7 +68,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // correct rotor momentum drag for increase in required rotor mass flow with altitude // obtained from momentum disc theory - const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C), 0.f); + const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos), 0.f); // drag model parameters const bool using_bcoef_x = _params.bcoef_x > 1.0f; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5143a10c2d59..4d4cae818be7 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -71,7 +71,7 @@ # include "sensor_range_finder.hpp" #endif // CONFIG_EKF2_RANGE_FINDER -#include +#include #include #include #include @@ -374,7 +374,7 @@ class EstimatorInterface float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m) #endif // CONFIG_EKF2_OPTICAL_FLOW - float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) + float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // air density (kg/m**3) bool _imu_updated{false}; // true if the ekf should update (completed downsampling process) bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized diff --git a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp index 7e4bc4eda2ce..ca03a0f5e78f 100644 --- a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp @@ -39,6 +39,7 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" +#include class EkfDragFusionTest : public ::testing::Test { @@ -179,7 +180,7 @@ TEST_F(EkfDragFusionTest, testForwardBluffBodyDrag) Vector2f predicted_accel(CONSTANTS_ONE_G * sinf(pitch), 0.0f); const float airspeed = sqrtf((2.0f * bcoef_x * predicted_accel.length()) / - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + atmosphere::kAirDensitySeaLevelStandardAtmos); Vector2f wind_speed(-airspeed, 0.0f); // The magnitude of error perpendicular to wind is equivalent to the error in the direction of wind @@ -219,7 +220,7 @@ TEST_F(EkfDragFusionTest, testLateralBluffBodyDrag) Vector2f predicted_accel(0.0f, - CONSTANTS_ONE_G * sinf(roll)); const float airspeed = sqrtf((2.0f * bcoef_y * predicted_accel.length()) / - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + atmosphere::kAirDensitySeaLevelStandardAtmos); Vector2f wind_speed(0.0f, -airspeed); // The magnitude of error perpendicular to wind is equivalent to the error in the of wind @@ -259,7 +260,7 @@ TEST_F(EkfDragFusionTest, testDiagonalBluffBodyDrag) Vector2f predicted_accel = quat_sim.rotateVectorInverse(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G)).xy(); const float airspeed = sqrtf((2.0f * bcoef_y * predicted_accel.norm()) / - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + atmosphere::kAirDensitySeaLevelStandardAtmos); Vector2f wind_speed(airspeed * predicted_accel / predicted_accel.norm()); // The magnitude of error perpendicular to wind is equivalent to the error in the of wind diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index ff5444cde603..a1ad6866b4b4 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -49,12 +49,13 @@ #include "launchdetection/LaunchDetector.h" #include "runway_takeoff/RunwayTakeoff.h" -#include +#include #include #include #include +#include #include #include #include @@ -362,7 +363,7 @@ class FixedwingPositionControl final : public ModuleBase // matrix, vectors, dcm, quaterions #include // math::radians, -#include // to get the physical constants +#include // to get the physical constants #include // to get the real time #include #include @@ -134,7 +134,7 @@ class Sih : public ModuleBase, public ModuleParams // hard constants static constexpr uint16_t NB_MOTORS = 6; static constexpr float T1_C = 15.0f; // ground temperature in Celsius - static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin + static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre // Aerodynamic coefficients static constexpr float RHO = 1.225f; // air density at sea level [kg/m^3] diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 94044b338458..132448177f33 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -50,7 +50,7 @@ #pragma once #include -#include +#include #include #include #include @@ -209,7 +209,7 @@ class VtolAttitudeControl : public ModuleBase, public Modul vtol_vehicle_status_s _vtol_vehicle_status{}; float _home_position_z{NAN}; - float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [kg/m^3] + float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3] hrt_abstime _last_run_timestamp{0}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index ea66ea53489e..bf1ca82a9a1f 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -45,6 +45,7 @@ #include #include #include +#include using namespace matrix; @@ -565,7 +566,7 @@ float VtolType::getFrontTransitionTimeFactor() const const float rho = math::constrain(_attc->getAirDensity(), 0.7f, 1.5f); if (PX4_ISFINITE(rho)) { - float rho0_over_rho = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / rho; + float rho0_over_rho = atmosphere::kAirDensitySeaLevelStandardAtmos / rho; return sqrtf(rho0_over_rho) * rho0_over_rho; }