diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 6c0c19acb5d..4343f250548 100644
--- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -263,8 +263,8 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			const float z_speed = _getMaxZSpeed();
 
 			Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
-			math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed);
-			math::trajectory::clampToZNorm(vel_sp_constrained, z_speed);
+			math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
+			math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
 
 			for (int i = 0; i < 3; i++) {
 				// If available, use the existing velocity as a feedforward, otherwise replace it
diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp
index b41e032531f..51530681eff 100644
--- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp
+++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp
@@ -126,30 +126,54 @@ float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDyna
 	return max_speed;
 }
 
-inline bool clampToXYNorm(Vector3f &target, float max_xy_norm)
+/*
+ * Constrain the 3D vector given a maximum XY norm
+ * If the XY norm of the 3D vector is larger than the maximum norm, the whole vector
+ * is scaled down to respect the constraint.
+ * If the maximum norm is small (defined by the "accuracy" parameter),
+ * only the XY components are scaled down to avoid affecting
+ * Z in case of numerical issues
+ */
+inline void clampToXYNorm(Vector3f &target, float max_xy_norm, float accuracy = FLT_EPSILON)
 {
 	const float xynorm = target.xy().norm();
-	const float scale_factor = max_xy_norm / xynorm;
+	const float scale_factor = (xynorm > FLT_EPSILON)
+				   ? max_xy_norm / xynorm
+				   : 1.f;
 
-	if (scale_factor < 1 && PX4_ISFINITE(scale_factor) && xynorm > FLT_EPSILON) {
-		target *= scale_factor;
-		return true;
+	if (scale_factor < 1.f) {
+		if (max_xy_norm < accuracy && xynorm < accuracy) {
+			target.xy() = Vector2f(target) * scale_factor;
+
+		} else {
+			target *= scale_factor;
+		}
 	}
-
-	return false;
 }
 
-inline bool clampToZNorm(Vector3f &target, float max_z_norm)
+/*
+ * Constrain the 3D vector given a maximum Z norm
+ * If the Z component of the 3D vector is larger than the maximum norm, the whole vector
+ * is scaled down to respect the constraint.
+ * If the maximum norm is small (defined by the "accuracy parameter),
+ * only the Z component is scaled down to avoid affecting
+ * XY in case of numerical issues
+ */
+inline void clampToZNorm(Vector3f &target, float max_z_norm, float accuracy = FLT_EPSILON)
 {
-	float znorm = fabs(target(2));
-	const float scale_factor = max_z_norm / znorm;
+	const float znorm = fabs(target(2));
+	const float scale_factor = (znorm > FLT_EPSILON)
+				   ? max_z_norm / znorm
+				   : 1.f;
 
-	if (scale_factor < 1 && PX4_ISFINITE(scale_factor) && znorm > FLT_EPSILON) {
-		target *= scale_factor;
-		return true;
+	if (scale_factor < 1.f) {
+		if (max_z_norm < accuracy && znorm < accuracy) {
+			target(2) *= scale_factor;
+
+		} else {
+			target *= scale_factor;
+		}
 	}
-
-	return false;
 }
 
 }