From 28fe15d82978327ba07a39749f83c8a03023b10e Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Wed, 13 Sep 2023 14:25:26 +0200 Subject: [PATCH] FixedwingPositionControl: initialize the airspeed slew rate controller with trim airspeed in the constructor --- .../FixedwingPositionControl.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 24f0ce0e44..c5e7f67827 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -76,6 +76,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : /* fetch initial parameter values */ parameters_update(); + + _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_trim.get()); } FixedwingPositionControl::~FixedwingPositionControl() @@ -429,14 +431,6 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, _wind_vel.length(), _param_fw_airspd_max.get()); } - if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) { - _airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed); - } - - if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) { - _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get()); - } - // --- airspeed *setpoint adjustments --- if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { @@ -470,6 +464,14 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); } + if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) { + _airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed); + } + + if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) { + _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get()); + } + return _airspeed_slew_rate_controller.getState(); }