diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
index b747dd9ab8..0018df50a6 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
@@ -111,11 +111,10 @@ void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target)
 			// Start the trajectory at the current velocity setpoint
 			_trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
 			_trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
-			_position_setpoint_locked(0) = NAN;
-			_position_setpoint_locked(1) = NAN;
+			_state.v = _velocity_setpoint_feedback;
+			resetPositionLock();
 		}
 
-		_position_lock_active = false;
 		_trajectory[0].setCurrentPosition(_position_estimate(0));
 		_trajectory[1].setCurrentPosition(_position_estimate(1));
 	}
diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
index 4b9f8c8f1e..ab5e041c08 100644
--- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp
@@ -110,10 +110,10 @@ void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target)
 		if (_position_lock_active) {
 			// Start the trajectory at the current velocity setpoint
 			_trajectory.setCurrentVelocity(_velocity_setpoint_feedback);
-			_position_setpoint_locked = NAN;
+			_state.v = _velocity_setpoint_feedback;
+			resetPositionLock();
 		}
 
-		_position_lock_active = false;
 		_trajectory.setCurrentPosition(_position_estimate);
 	}
 }