FixedWingPositionControl: support compensation of maximum fixed wing climbrate

based on vehicle weight and air density

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -94,6 +94,29 @@ FixedwingPositionControl::init()
 	return true;
 }
 
+float FixedwingPositionControl::getMaximumClimbRate()
+{
+
+	float weight_factor = 1.0f;
+
+	if (_param_weight_base.get() > 0.0f && _param_weight_gross.get() > 0.0f) {
+		weight_factor = math::constrain(_param_weight_base.get() / _param_weight_gross.get(), MIN_WEIGHT_RATIO,
+						MAX_WEIGHT_RATIO);
+	}
+
+	float climbrate_max = _param_fw_t_clmb_max.get();
+
+	if (_param_density_min.get() > 0.0f) {
+		const float min_density = math::max(_param_density_min.get(), AIR_DENSITY_STANDARD_ATMOS_5000_AMSL);
+		const float density_gradient = (_param_fw_t_clmb_max.get() - CLIMBRATE_MIN) / (CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C -
+					       min_density);
+		const float delta_rho = _air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
+		climbrate_max = _param_fw_t_clmb_max.get() + density_gradient * delta_rho;
+	}
+
+	return climbrate_max * weight_factor;
+}
+
 int
 FixedwingPositionControl::parameters_update()
 {
@@ -121,7 +144,7 @@ FixedwingPositionControl::parameters_update()
 	_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
 
 	// TECS parameters
-	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
+	_tecs.set_max_climb_rate(getMaximumClimbRate());
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
@@ -2266,6 +2289,7 @@ FixedwingPositionControl::Run()
 
 		if (_vehicle_air_data_sub.update(&air_data)) {
 			_air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density;
+			_tecs.set_max_climb_rate(getMaximumClimbRate());
 		}
 
 		if (_vehicle_land_detected_sub.updated()) {

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -150,6 +150,9 @@ static constexpr float MAX_WEIGHT_RATIO = 2.0f;
 // air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
 static constexpr float AIR_DENSITY_STANDARD_ATMOS_5000_AMSL = 0.7363f;
 
+// climbrate defining the service ceiling, used to compensate max climbrate based on air density
+static constexpr float CLIMBRATE_MIN = 0.5f; // [m/s]
+
 // [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
 static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
 
@@ -482,6 +485,7 @@ private:
 	 */
 	float get_terrain_altitude_takeoff(float takeoff_alt);
 
+	float getMaximumClimbRate();
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *
@@ -938,6 +942,8 @@ private:
 
 		(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_WING_SPAN>) _param_fw_wing_span,
 		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -1093,3 +1093,18 @@ PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f);
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);
+
+
+/**
+ * Service ceiling density
+ *
+ * Air density at which the vehicle in normal configuration is 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_CLMB_MAX.
+ * Set < 0 to disable compensation of (FW_T_CLMB_MAX) based on air density.
+ *
+ * @max 1.225
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_DENSITY_MIN, -1.0f);