diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 63fc7eddf8..2ddbf97f74 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1500,8 +1500,9 @@ void MulticopterPositionControl::control_auto(float dt)
 	}
 
 	/* set velocity limit if close to current setpoint and no next setpoint available */
+	math::Vector<3> dist = _curr_pos_sp - _pos;
 	_limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
-			&& ((_curr_pos_sp - _pos).length() <= _target_threshold_xy.get());
+			&& (sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) <= _target_threshold_xy.get());
 
 	if (current_setpoint_valid &&
 	    (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {