From 6f3868b5ba75de38a409c2ad72251dfaaee99a3e Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 17 Sep 2019 13:58:41 +0200
Subject: [PATCH] ManualVelocitySmoothing - Fix unlock initialization

---
 .../FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp  | 5 ++---
 .../FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp   | 4 ++--
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp
index b747dd9ab828..0018df50a69c 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 4b9f8c8f1e10..ab5e041c081c 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);
 	}
 }