From c7696488fead8da8191cbca0f3df23b8401a3ea8 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Sep 2019 11:10:01 +0200 Subject: [PATCH] VelocitySmoothing - Improve computation of the direction of the trajectory by predicting the velocity at zero acceleration instead of the current velocity This helps when the current velocity is smaller than the target but that the acceleration is too large such that the velocity will overshoot. Without this check, the algorithm increases the acceleration which leads to an even larger overshoot. --- .../tasks/Utility/VelocitySmoothing.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index efae62a3b5..5be4af3f9f 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -166,8 +166,22 @@ void VelocitySmoothing::updateDurations(float t_now, float vel_setpoint) _t0 = t_now; _state_init = _state; + // Compute the velocity at which the trajectory will be + // when the acceleration will be zero + float vel_zero_acc = _state.v; + if (fabsf(_state.a) > FLT_EPSILON) { + float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration + float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration + vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc; + } + /* Depending of the direction, start accelerating positively or negatively */ - _direction = math::sign(_vel_sp - _state.v); + _direction = math::sign(_vel_sp - vel_zero_acc); + if (_direction == 0) { + // If by braking immediately the velocity is exactly + // the require one with zero acceleration, then brake + _direction = -math::sign(_vel_sp - _state.v); + } if (_direction != 0) { updateDurations();