From f847586b1006717bdcaef5612a9017e7a0fc3868 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 6 Sep 2023 13:27:44 +0300 Subject: [PATCH] review comments Signed-off-by: RomanBapst --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 8 +++++--- src/modules/fw_pos_control/FixedwingPositionControl.hpp | 3 +++ src/modules/fw_pos_control/fw_path_navigation_params.c | 8 +++++--- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 92b5f97e58..6a4291b49c 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -106,10 +106,12 @@ float FixedwingPositionControl::getMaximumClimbRate() 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_min = _param_density_min.get(); + + if (density_min < AIR_DENSITY_STANDARD_ATMOS_1000_AMSL + && density_min > 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); + density_min); const float delta_rho = _air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; climbrate_max = _param_fw_t_clmb_max.get() + density_gradient * delta_rho; } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index cae39c7bd6..5d86500147 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/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; +// air density of standard athmosphere at 1000m above mean sea level [kg/m^3] +static constexpr float AIR_DENSITY_STANDARD_ATMOS_1000_AMSL = 1.112f; + // climbrate defining the service ceiling, used to compensate max climbrate based on air density static constexpr float CLIMBRATE_MIN = 0.5f; // [m/s] diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index ff778a8ebb..3319948825 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -1099,12 +1099,14 @@ 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. + * 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). * + * @min 0.7363 * @max 1.225 + * @unit kg/m^3 * @decimal 2 * @increment 0.01 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_DENSITY_MIN, -1.0f); +PARAM_DEFINE_FLOAT(FW_DENSITY_MIN, 1.225);