From f7d7a7123c302f94ae4be470af7144a09a4ed730 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 11 Sep 2023 09:51:46 +0300 Subject: [PATCH] addressed review comments Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 24 +++++-------------- .../FixedwingPositionControl.hpp | 8 +++---- 2 files changed, 10 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 143e383c71..def0bf8eef 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -99,7 +99,7 @@ float FixedwingPositionControl::getWeightRatio() float weight_factor = 1.0f; - if (_param_weight_base.get() > 0.0f && _param_weight_gross.get() > 0.0f) { + if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO, MAX_WEIGHT_RATIO); } @@ -157,7 +157,7 @@ FixedwingPositionControl::parameters_update() // TECS parameters _tecs.set_max_climb_rate(getMaximumClimbRate()); - _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); + _tecs.set_max_sink_rate(getMinimumSinkRate()); _tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get()); @@ -461,19 +461,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, load_factor_from_bank_angle = 1.0f / cosf(_att_sp.roll_body); } - float weight_ratio = 1.0f; - - if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { - weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO, - MAX_WEIGHT_RATIO); - } - // Here we make sure that the set minimum airspeed is automatically adapted to the current load factor. // The minimum airspeed is the controller limit (FW_AIRSPD_MIN, unless either in takeoff or landing) that should // resemble the vehicles stall speed (CAS) with a 1g load plus some safety margin (as no controller tracks perfectly). // Stall speed increases with the square root of the load factor: V_stall ~ sqrt(load_factor). // The load_factor is composed of a term from the bank angle and a term from the weight ratio. - calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio); + calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * getWeightRatio()); // Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds. if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { @@ -2302,6 +2295,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()); + _tecs.set_min_sink_rate(getMinimumSinkRate()); } if (_vehicle_land_detected_sub.updated()) { @@ -2518,13 +2512,6 @@ float FixedwingPositionControl::calculateTrimThrottle(float throttle_min, throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (airspeed_sp - _param_fw_airspd_trim.get()); } - float weight_ratio = 1.0f; - - if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { - weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO, - MAX_WEIGHT_RATIO); - } - float air_density_throttle_scale = 1.0f; if (PX4_ISFINITE(_air_density)) { @@ -2535,7 +2522,8 @@ float FixedwingPositionControl::calculateTrimThrottle(float throttle_min, } // compensate trim throttle for both weight and air density - return math::constrain(throttle_trim * sqrtf(weight_ratio) * air_density_throttle_scale, throttle_min, throttle_max); + return math::constrain(throttle_trim * sqrtf(getWeightRatio()) * air_density_throttle_scale, throttle_min, + throttle_max); } void diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index cbc6e21fec..f5b127fda0 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -489,14 +489,14 @@ private: float get_terrain_altitude_takeoff(float takeoff_alt); /** - * @brief Return the maximum climb rate achievable under the current conditions. - * @return Maximum climbrate. + * @brief Return the maximum climb rate achievable given the estimated air density and the vehicle weight. + * @return Maximum climbrate [m/s]. */ float getMaximumClimbRate(); /** - * @brief Return the minimum sink rate achievable under the current conditions. - * @return Minimum sink rate. + * @brief Return the minimum sink rate achievable given the estimated air density and the vehicle weight. + * @return Minimum sink rate [m/s]. */ float getMinimumSinkRate();