From d6a770a547102cbd289793fb226528552c35dfda Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 6 Sep 2023 14:02:15 +0300 Subject: [PATCH] FixedWingPositionControl: compensate minimum sink rate for weight and density Signed-off-by: RomanBapst --- .../fw_pos_control/FixedwingPositionControl.cpp | 16 +++++++++++++--- .../fw_pos_control/FixedwingPositionControl.hpp | 17 +++++++++++++++++ 2 files changed, 30 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 6a4291b49c..143e383c71 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/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 diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5d86500147..cbc6e21fec 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -488,7 +488,24 @@ private: */ 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 *