From 1fefa76e5d2f5e2bbffa848222934f26f161d9bd Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 7 Mar 2019 12:04:32 +0100 Subject: [PATCH] ManualSmoothVel - Set jerk to 1.0 during position hold to help the optimizer to converge towards 0 vel and acc --- .../FlightTaskManualPositionSmoothVel.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 4256c065dc..8e182740fa 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -141,6 +141,19 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _position_lock_z_active = false; } + // During position lock, lower jerk to help the optimizer + // to converge to 0 acceleration and velocity + if (_position_lock_xy_active) { + jerk[0] = 1.f; + jerk[1] = 1.f; + + } else { + jerk[0] = _jerk_max.get(); + jerk[1] = _jerk_max.get(); + } + + jerk[2] = _position_lock_z_active ? 1.f : _jerk_max.get(); + for (int i = 0; i < 3; ++i) { _smoothing[i].setMaxJerk(jerk[i]); _smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));