diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
index 788b7cf989..f73fbe4023 100644
--- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
@@ -74,12 +74,11 @@ bool FlightTaskManualAcceleration::update()
 	static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl
 
 	if (max_hagl_ratio > factor_threshold) {
-		max_hagl_ratio = math::min(max_hagl_ratio, 1.f);
 		const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
-		_stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel));
+		_stick_acceleration_xy.setVelocityConstraint(interpolate(max_hagl_ratio, factor_threshold, 1.f, vxy_max, min_vel));
 
-	} else {
-		_stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max));
+	} else if (PX4_ISFINITE(vehicle_local_pos.vxy_max)) {
+		_stick_acceleration_xy.setVelocityConstraint(vehicle_local_pos.vxy_max);
 	}
 
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
index 052378244b..686db33a88 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
@@ -43,6 +43,8 @@ StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) :
 {
 	_brake_boost_filter.reset(1.f);
 	resetPosition();
+	_velocity_slew_rate_xy.setSlewRate(_param_mpc_acc_hor.get());
+	_velocity_slew_rate_xy.setForcedValue(_param_mpc_vel_manual.get());
 }
 
 void StickAccelerationXY::resetPosition()
@@ -75,23 +77,22 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
 	}
 }
 
+void StickAccelerationXY::setVelocityConstraint(float vel)
+{
+	if ((vel < _velocity_constraint) && (vel >= FLT_EPSILON)) {
+		_velocity_constraint = vel;
+	}
+};
+
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
 		const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
-	// gradually adjust velocity constraint because good tracking is required for the drag estimation
-	if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) {
-		if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) {
-			_current_velocity_constraint = _targeted_velocity_constraint;
-
-		} else {
-			_current_velocity_constraint = math::constrain(_targeted_velocity_constraint,
-						       _current_velocity_constraint - dt * _param_mpc_acc_hor.get(),
-						       _current_velocity_constraint + dt * _param_mpc_acc_hor.get());
-		}
-	}
+	// avoid setpoint steps from limit changes to improve velocity tracking and hence drag estimation
+	const float velocity_constraint = _velocity_slew_rate_xy.update(_velocity_constraint, dt);
+	_velocity_constraint = _param_mpc_vel_manual.get(); // reset, reduced to strictest limit in next loop
 
 	// maximum commanded velocity can be constrained dynamically
-	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint);
+	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), velocity_constraint);
 	Vector2f velocity_scale(velocity_sc, velocity_sc);
 	// maximum commanded acceleration is scaled down with velocity
 	const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());
diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
index ad6f4afec1..feb4070643 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
@@ -64,8 +64,10 @@ public:
 	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
 	float getMaxAcceleration() { return _param_mpc_acc_hor.get(); };
 	float getMaxJerk() { return _param_mpc_jerk_max.get(); };
-	void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); };
-	float getVelocityConstraint() { return _current_velocity_constraint; };
+
+	// Assuming the velocity constraint resets in every loop and update constraint if new value is lower
+	void setVelocityConstraint(float vel);
+	float getVelocityConstraint() { return _velocity_slew_rate_xy.getState(); };
 
 private:
 	CollisionPrevention _collision_prevention{this};
@@ -80,6 +82,7 @@ private:
 
 	SlewRate<float> _acceleration_slew_rate_x;
 	SlewRate<float> _acceleration_slew_rate_y;
+	SlewRate<float> _velocity_slew_rate_xy;
 	AlphaFilter<float> _brake_boost_filter;
 
 	matrix::Vector2f _position_setpoint;
@@ -87,8 +90,7 @@ private:
 	matrix::Vector2f _acceleration_setpoint;
 	matrix::Vector2f _acceleration_setpoint_prev;
 
-	float _targeted_velocity_constraint{INFINITY};
-	float _current_velocity_constraint{INFINITY};
+	float _velocity_constraint{INFINITY};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,