Performance model: replace minimum density with service ceiling

Signed-off-by: RomanBapst <[email protected]>
PX4 · Oct 23, 2023 · 17900ff · 17900ff
1 parent d01dc17
commit 17900ff
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 16 deletions.
diff --git a/src/modules/fw_pos_control/performance_model/PerformanceModel.cpp b/src/modules/fw_pos_control/performance_model/PerformanceModel.cpp
@@ -40,6 +40,7 @@
 #include <geo/geo.h>
 #include <px4_platform_common/events.h>
 #include "PerformanceModel.h"
+#include <lib/atmosphere/atmosphere.h>
 
 // air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
 static constexpr float kAirDensityStandardAtmos5000Amsl = 0.7363f;
@@ -73,18 +74,19 @@ float PerformanceModel::getWeightRatio() const
 }
 float PerformanceModel::getMaximumClimbRate(float air_density) const
 {
-
 	float climbrate_max = _param_fw_t_clmb_max.get();
 
-	const float density_min = _param_density_min.get();
+	const float service_ceiling = _param_service_ceiling.get();
 
-	if (density_min < kAirDensityStandardAtmos1000Amsl
-	    && density_min > kAirDensityStandardAtmos5000Amsl) {
+	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_min), 0.0f);
+						density), 0.0f);
 		const float delta_rho = air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-		climbrate_max = math::max(_param_fw_t_clmb_max.get() + density_gradient * delta_rho, kClimbrateMin);
+		climbrate_max = math::constrain(_param_fw_t_clmb_max.get() + density_gradient * delta_rho, kClimbrateMin,
+						_param_fw_t_clmb_max.get());
 	}
 
 	return climbrate_max / getWeightRatio();

diff --git a/src/modules/fw_pos_control/performance_model/PerformanceModel.h b/src/modules/fw_pos_control/performance_model/PerformanceModel.h
@@ -122,7 +122,7 @@ class PerformanceModel : public ModuleParams
 		(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
 		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
 		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
-		(ParamFloat<px4::params::FW_DENSITY_MIN>) _param_density_min,
+		(ParamFloat<px4::params::FW_S_CEILING>) _param_service_ceiling,
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,

diff --git a/src/modules/fw_pos_control/performance_model/performance_model_params.c b/src/modules/fw_pos_control/performance_model/performance_model_params.c
@@ -77,20 +77,19 @@ PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);
 
 
 /**
- * Service ceiling density
+ * Service ceiling
  *
- * Air density at which the vehicle in normal configuration is able to achieve a maximum climb rate of
+ * Altitude in standard atmosphere at which the vehicle in normal configuration (WEIGHT_BASE) is still able to achieve a maximum climb rate of
  * 0.5m/s at maximum throttle (FW_THR_MAX). Used to compensate for air density in FW_T_CLMB_MAX.
- * Will only have an effect if value is between 0.7363 (5000m) and 1.112 (1000m).
+ * Set negative to disable.
  *
- * @min 0.7363
- * @max 1.225
- * @unit kg/m^3
- * @decimal 2
- * @increment 0.01
+ * @min -1.0
+ * @unit m
+ * @decimal 0
+ * @increment 1.0
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_DENSITY_MIN, 1.225);
+PARAM_DEFINE_FLOAT(FW_S_CEILING, -1.0f);
 
 /**
  * Vehicle base weight.