From 1840f9645a1069de755cadfe2f979d3104a911fe Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 12 Sep 2023 08:52:44 +0300 Subject: [PATCH] compensate airspeed setpoint based on weight ratio Signed-off-by: RomanBapst --- .../fw_pos_control/FixedwingPositionControl.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index def0bf8eef..f7617f7f02 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -417,13 +417,15 @@ FixedwingPositionControl::vehicle_attitude_poll() float FixedwingPositionControl::get_manual_airspeed_setpoint() { - float altctrl_airspeed = _param_fw_airspd_trim.get(); + float altctrl_airspeed = math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), + _param_fw_airspd_min.get(), + _param_fw_airspd_max.get()); if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) { // neutral throttle corresponds to trim airspeed return math::interpolateNXY(_manual_control_setpoint_for_airspeed, {-1.f, 0.f, 1.f}, - {_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()}); + {_param_fw_airspd_min.get(), altctrl_airspeed, _param_fw_airspd_max.get()}); } else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) { altctrl_airspeed = _commanded_manual_airspeed_setpoint; @@ -437,7 +439,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_min_airspeed, const Vector2f &ground_speed) { if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { - calibrated_airspeed_setpoint = _param_fw_airspd_trim.get(); + // increase trim airspeed based on weight ratio in order to keep constant alpha. + calibrated_airspeed_setpoint = _param_fw_airspd_trim.get() * sqrtf(getWeightRatio()); } // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained @@ -956,7 +959,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i tecs_update_pitch_throttle(control_interval, _current_altitude, - _param_fw_airspd_trim.get(), + _param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), @@ -987,7 +990,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) tecs_update_pitch_throttle(control_interval, _current_altitude, - _param_fw_airspd_trim.get(), + _param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(),