Added PerformanceModel for fixed wing (#22091)

* created a Performance Model for fixed wing vehicle
- added compensation for maximum climbrate, minimum sinkrate, minimum airspeed and trim airspeed based on weight ratio and air density
- added atmosphere lib to standard atmosphere calculations


---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>

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 @@ protected:
 	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