addressed more review comments

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

src/lib/atmosphere/atmosphere.cpp

@@ -41,19 +41,16 @@
 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 kPa
 
 float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius)
 {
-	return (pressure_pa / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)));
+	return (pressure_pa / (kAirGasConstant * (temperature_celsius - kAbsoluteNullCelcius)));
 }
 float getPressureFromAltitude(const float altitude_m)
 {
 
 	return kPressRefSeaLevelPa * powf((altitude_m * kTempGradient + kTempRefKelvin) / kTempRefKelvin,
-					  -CONSTANTS_ONE_G / (kTempGradient * CONSTANTS_AIR_GAS_CONST));
+					  -CONSTANTS_ONE_G / (kTempGradient * kAirGasConstant));
 }
 float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa)
 {
@@ -70,7 +67,7 @@ float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa)
 	 * h = -------------------------------  + h1
 	 *                   a
 	 */
-	return (((powf(pressure_ratio, (-(kTempGradient * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * kTempRefKelvin) -
+	return (((powf(pressure_ratio, (-(kTempGradient * kAirGasConstant) / CONSTANTS_ONE_G))) * kTempRefKelvin) -
 		kTempRefKelvin) / kTempGradient;
 
 }

src/lib/atmosphere/atmosphere.h

@@ -44,6 +44,17 @@ 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.
+static constexpr float kAirDensitySeaLevelStandardAtmos = 1.225f;				// kg/m^3
+
+// [kg/m^3] air density of standard atmosphere at 11000m above mean sea level (this is the upper limit for the standard
+// atmosphere model we are using, see atmosphere lib used)
+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 kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter
+static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa
 
 /**
 * Calculate air density given air pressure and temperature.

src/lib/fw_performance_model/PerformanceModel.cpp

@@ -39,25 +39,20 @@
 
 #include <geo/geo.h>
 #include <px4_platform_common/events.h>
-#include "PerformanceModel.h"
+#include "PerformanceModel.hpp"
 #include <lib/atmosphere/atmosphere.h>
 
 using namespace atmosphere;
 
-// air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
-static constexpr float kAirDensityStandardAtmos5000Amsl = 0.7363f;
-
-// air density of standard athmosphere at 1000m above mean sea level [kg/m^3]
-static constexpr float kAirDensityStandardAtmos1000Amsl = 1.112f;
 
 // [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
 static constexpr float kMinWeightRatio = 0.5f;
 
 // [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
 static constexpr float kMaxWeightRatio = 2.0f;
 
-// climbrate defining the service ceiling, used to compensate max climbrate based on air density
-static constexpr float kClimbrateMin = 0.5f; // [m/s]
+// [m/s] climbrate defining the service ceiling, used to compensate max climbrate based on air density
+static constexpr float kClimbRateAtServiceCeiling = 0.5f;
 
 PerformanceModel::PerformanceModel(): ModuleParams(nullptr)
 {
@@ -76,18 +71,20 @@ float PerformanceModel::getWeightRatio() const
 }
 float PerformanceModel::getMaximumClimbRate(float air_density) const
 {
+	air_density = sanitiseAirDensity(air_density);
 	float climbrate_max = _param_fw_t_clmb_max.get();
 
 	const float service_ceiling = _param_service_ceiling.get();
 
 	if (service_ceiling > 0.0f) {
-		const float pressure = getPressureFromAltitude(service_ceiling);
-		const float density = getDensityFromPressureAndTemp(pressure, getStandardTemperatureAtAltitude(service_ceiling));
-		const float density_gradient = math::max((_param_fw_t_clmb_max.get() - kClimbrateMin) /
-					       (CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C -
-						density), 0.0f);
-		const float delta_rho = air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-		climbrate_max = math::constrain(_param_fw_t_clmb_max.get() + density_gradient * delta_rho, kClimbrateMin,
+		const float ceiling_pressure = getPressureFromAltitude(service_ceiling);
+		const float ceiling_density = getDensityFromPressureAndTemp(ceiling_pressure,
+					      getStandardTemperatureAtAltitude(service_ceiling));
+		const float climbrate_gradient = math::max((_param_fw_t_clmb_max.get() - kClimbRateAtServiceCeiling) /
+						 (kAirDensitySeaLevelStandardAtmos -
+						  ceiling_density), 0.0f);
+		const float delta_rho = air_density - kAirDensitySeaLevelStandardAtmos;
+		climbrate_max = math::constrain(_param_fw_t_clmb_max.get() + climbrate_gradient * delta_rho, kClimbRateAtServiceCeiling,
 						_param_fw_t_clmb_max.get());
 	}
 
@@ -107,14 +104,13 @@ float PerformanceModel::getWeightThrottleScale() const
 
 float PerformanceModel::getAirDensityThrottleScale(float air_density) const
 {
+	air_density = sanitiseAirDensity(air_density);
 	float air_density_throttle_scale = 1.0f;
 
-	if (PX4_ISFINITE(air_density)) {
-		// scale throttle as a function of sqrt(rho0/rho)
-		const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density);
-		const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / kAirDensityStandardAtmos5000Amsl);
-		air_density_throttle_scale = math::constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
-	}
+	// scale throttle as a function of sqrt(rho0/rho)
+	const float eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / air_density);
+	const float eas2tas_at_11km_amsl = sqrtf(kAirDensitySeaLevelStandardAtmos / kAirDensityStandardAtmos11000Amsl);
+	air_density_throttle_scale = math::constrain(eas2tas, 1.f, eas2tas_at_11km_amsl);
 
 	return air_density_throttle_scale;
 }
@@ -143,7 +139,8 @@ float PerformanceModel::getTrimThrottleForAirspeed(float airspeed_sp) const
 }
 float PerformanceModel::getMinimumSinkRate(float air_density) const
 {
-	return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density);
+	air_density = sanitiseAirDensity(air_density);
+	return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * kAirDensitySeaLevelStandardAtmos / air_density);
 }
 float PerformanceModel::getCalibratedTrimAirspeed() const
 {
@@ -152,11 +149,14 @@ float PerformanceModel::getCalibratedTrimAirspeed() const
 }
 float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor) const
 {
+
+	load_factor = math::max(load_factor, FLT_EPSILON);
 	return _param_fw_airspd_min.get() * sqrtf(getWeightRatio() * load_factor);
 }
 
 float PerformanceModel::getCalibratedStallAirspeed(float load_factor) const
 {
+	load_factor = math::max(load_factor, FLT_EPSILON);
 	return _param_fw_airspd_stall.get() * sqrtf(getWeightRatio() * load_factor);
 }
 
@@ -227,3 +227,12 @@ void PerformanceModel::	updateParameters()
 {
 	updateParams();
 }
+
+float PerformanceModel::sanitiseAirDensity(float air_density)
+{
+	if (!PX4_ISFINITE(air_density)) {
+		air_density = kAirDensitySeaLevelStandardAtmos;
+	}
+
+	return math::max(air_density, kAirDensityStandardAtmos11000Amsl);
+}

src/lib/fw_performance_model/PerformanceModel.h → src/lib/fw_performance_model/PerformanceModel.hpp

@@ -51,15 +51,15 @@ class PerformanceModel : public ModuleParams
 	void updateParameters();
 
 	/**
-	 * Get the maximum climb rate expected for current air density and weight.
+	 * Get the maximum climb rate (true airspeed) expected for current air density and weight.
 	 * @param air_density in kg/m^3
 	 * @return maximum climb rate in m/s
 	 */
 	float getMaximumClimbRate(float air_density) const;
 
 	/**
-	 * Get the minimum sink rate expected for current air density and weight.
-	 * @param air_density
+	 * Get the minimum sink rate (true airspeed) expected for current air density and weight.
+	 * @param air_density in kg/m^3
 	 * @return minimum sink rate in m/s
 	 */
 	float getMinimumSinkRate(float air_density) const;
@@ -71,12 +71,12 @@ class PerformanceModel : public ModuleParams
 	float getWeightRatio() const;
 
 	/**
-	 * Get the trim throttle for the current airspeed setpoint as well as air densty and weight.
-	 * @param throttle_min minimum throttle
-	 * @param throttle_max maximum throttle
+	 * Get the trim throttle for the current airspeed setpoint as well as air density and weight.
+	 * @param throttle_min minimum throttle in range [0,1]
+	 * @param throttle_max maximum throttle in range [0,1]
 	 * @param airspeed_sp calibrated airspeed setpoint in m/s
-	 * @param air_density air density
-	 * @return trim throttle
+	 * @param air_density air density in kg/m^3
+	 * @return trim throttle in range [0,1]
 	 */
 	float getTrimThrottle(float throttle_min, float throttle_max, float airspeed_sp, float air_density) const;
 
@@ -149,6 +149,8 @@ class PerformanceModel : public ModuleParams
 	 * @return throttle scale factor for weight
 	 */
 	float getWeightThrottleScale() const;
+
+	static float sanitiseAirDensity(float air_density);
 };
 
 #endif //PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_

src/lib/geo/geo.h

@@ -54,19 +54,7 @@
 #include <lib/matrix/matrix/math.hpp>
 
 static constexpr float CONSTANTS_ONE_G = 9.80665f;						// m/s^2
-
-static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f;					// pascals (Pa)
-static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f;	// kilopascals (kPa)
-static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA /
-		100.0f;	// Millibar (mbar) (1 mbar = 100 Pa)
-
-static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f;				// kg/m^3
-static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f;					// J/(kg * K)
-static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f;				// °C
-
 static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000;					// meters (m)
-static constexpr float  CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH;		// meters (m)
-
 static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f;				// radians/second (rad/s)