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); } }