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(); }