ManualVelocitySmoothing - Fix unlock initialization

loud-at-heart · Sep 19, 2019 · 6f3868b · 6f3868b
1 parent 0153e1b
commit 6f3868b
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 5 deletions.
diff --git 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
@@ -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);
 	}
 }