FixedWingPositionControl: compensate minimum sink rate for weight and…

… density

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

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -94,16 +94,21 @@ FixedwingPositionControl::init()
 	return true;
 }
 
-float FixedwingPositionControl::getMaximumClimbRate()
+float FixedwingPositionControl::getWeightRatio()
 {
 
 	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,
+		weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
 						MAX_WEIGHT_RATIO);
 	}
 
+	return weight_factor;
+}
+
+float FixedwingPositionControl::getMaximumClimbRate()
+{
 	float climbrate_max = _param_fw_t_clmb_max.get();
 
 	const float density_min = _param_density_min.get();
@@ -116,7 +121,12 @@ float FixedwingPositionControl::getMaximumClimbRate()
 		climbrate_max = _param_fw_t_clmb_max.get() + density_gradient * delta_rho;
 	}
 
-	return climbrate_max * weight_factor;
+	return climbrate_max / getWeightRatio();
+}
+
+float FixedwingPositionControl::getMinimumSinkRate()
+{
+	return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
 }
 
 int

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -488,7 +488,24 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	 */
 	float get_terrain_altitude_takeoff(float takeoff_alt);
 
+	/**
+	 * @brief Return the maximum climb rate achievable under the current conditions.
+	 * @return Maximum climbrate.
+	 */
 	float getMaximumClimbRate();
+
+	/**
+	 * @brief Return the minimum sink rate achievable under the current conditions.
+	 * @return Minimum sink rate.
+	 */
+	float getMinimumSinkRate();
+
+	/**
+	 * @brief Return the ratio of actual vehicle weight to vehicle base weight.
+	 * @return Weight ratio.
+	 */
+	float getWeightRatio();
+
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *