diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 958b8b57c2..4256c065dc 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -180,18 +180,28 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 	if (Vector2f(_vel_sp_smooth).length() < 0.01f &&
 	    Vector2f(_acceleration_setpoint).length() < .2f &&
 	    sticks_expo_xy.length() <= FLT_EPSILON) {
-		_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
-		_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
 		_position_lock_xy_active = true;
 	}
 
 	if (fabsf(_vel_sp_smooth(2)) < 0.01f &&
 	    fabsf(_acceleration_setpoint(2)) < .2f &&
 	    fabsf(_sticks_expo(2)) <= FLT_EPSILON) {
-		_position_setpoint_z_locked = pos_sp_smooth(2);
 		_position_lock_z_active = true;
 	}
 
+	// Set valid position setpoint while in position lock.
+	// When the position lock condition above is false, it does not
+	// mean that the unlock condition is true. This is why
+	// we are checking the lock flag here.
+	if (_position_lock_xy_active) {
+		_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
+		_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
+	}
+
+	if (_position_lock_z_active) {
+		_position_setpoint_z_locked = pos_sp_smooth(2);
+	}
+
 	_position_setpoint(0) = _position_setpoint_xy_locked(0);
 	_position_setpoint(1) = _position_setpoint_xy_locked(1);
 	_position_setpoint(2) = _position_setpoint_z_locked;