use atmosphere lib instead of geo lib

Signed-off-by: RomanBapst <[email protected]>

src/lib/airspeed/airspeed.cpp

@@ -43,10 +43,10 @@
 #include "airspeed.h"
 
 #include <px4_platform_common/defines.h>
-#include <lib/geo/geo.h>
 #include <lib/atmosphere/atmosphere.h>
 
 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);
 }

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)
 {

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
 

src/modules/commander/baro_calibration.cpp

@@ -48,7 +48,7 @@
 #include <px4_platform_common/time.h>
 #include <drivers/drv_hrt.h>
 #include <matrix/math.hpp>
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <lib/sensor_calibration/Barometer.hpp>
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
@@ -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;

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -41,6 +41,7 @@
 #include <ekf_derivation/generated/compute_drag_y_innov_var_and_k.h>
 
 #include <mathlib/mathlib.h>
+#include <lib/atmosphere/atmosphere.h>
 
 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;

src/modules/ekf2/EKF/estimator_interface.h

@@ -71,7 +71,7 @@
 # include "sensor_range_finder.hpp"
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
@@ -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

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 <lib/atmosphere/atmosphere.h>
 
 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

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -49,12 +49,13 @@
 
 #include "launchdetection/LaunchDetector.h"
 #include "runway_takeoff/RunwayTakeoff.h"
-#include <lib/fw_performance_model/PerformanceModel.h>
+#include <lib/fw_performance_model/PerformanceModel.hpp>
 
 #include <float.h>
 
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <lib/npfg/npfg.hpp>
 #include <lib/tecs/TECS.hpp>
 #include <lib/mathlib/mathlib.h>
@@ -362,7 +363,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};
 	bool _airspeed_valid{false};
-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};
+	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};
 
 	// [us] last time airspeed was received. used to detect timeouts.
 	hrt_abstime _time_airspeed_last_valid{0};

src/modules/simulation/simulator_sih/sih.hpp

@@ -61,7 +61,7 @@
 
 #include <matrix/matrix/math.hpp>   // matrix, vectors, dcm, quaterions
 #include <conversion/rotation.h>    // math::radians,
-#include <lib/geo/geo.h>        // to get the physical constants
+#include <lib/atmosphere/atmosphere.h>        // to get the physical constants
 #include <drivers/drv_hrt.h>        // to get the real time
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@@ -134,7 +134,7 @@ class Sih : public ModuleBase<Sih>, 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]

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -50,7 +50,7 @@
 #pragma once
 
 #include <drivers/drv_hrt.h>
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <matrix/math.hpp>
@@ -209,7 +209,7 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, 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};
 

src/modules/vtol_att_control/vtol_type.cpp

@@ -45,6 +45,7 @@
 #include <float.h>
 #include <px4_platform_common/defines.h>
 #include <matrix/math.hpp>
+#include <lib/atmosphere/atmosphere.h>
 
 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;
 	}