From 81e1dbc56bba96782827bb9ff606f5eb962499b4 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 5 Sep 2023 14:43:02 +0300 Subject: [PATCH] FixedWingPositionControl: support compensation of maximum fixed wing climbrate based on vehicle weight and air density Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 26 ++++++++++++++++++- .../FixedwingPositionControl.hpp | 6 +++++ .../fw_path_navigation_params.c | 15 +++++++++++ 3 files changed, 46 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index da1938ffc6..92b5f97e58 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/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()) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index fe4e6aa580..cae39c7bd6 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; +// 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) _param_weight_base, (ParamFloat) _param_weight_gross, + (ParamFloat) _param_density_min, + (ParamFloat) _param_fw_wing_span, (ParamFloat) _param_fw_wing_height, 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 86d5875b0b..ff778a8ebb 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/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);