From a7cf981c8c8b0c09676bad7f43fe72f6ef1a1dd1 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 9 May 2019 14:09:15 +0200 Subject: [PATCH] AutoLineSmoothVel - Reduce jerk if acceleration, velocity and desired velocity are small to help the optimizer to converge towards zero --- .../AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index cd03313fe7..4d5beb7dca 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -254,6 +254,15 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() _updateTrajConstraints(); + // If the acceleration and velocities are small and that we want to stop, reduce the amplitude of the jerk signal + // to help the optimizer to converge towards zero + if (Vector2f(_velocity_setpoint).length() < (0.01f * _param_mpc_xy_traj_p.get()) + && Vector2f(accel_sp_smooth).length() < 0.2f + && Vector2f(vel_sp_smooth).length() < 0.1f) { + _trajectory[0].setMaxJerk(1.f); + _trajectory[1].setMaxJerk(1.f); + } + for (int i = 0; i < 3; ++i) { _trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i)); }