From 76a447ed0f8330e2a721a095fb63d3971d52a7a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 8 Feb 2021 13:20:13 +0100 Subject: [PATCH] fix StickAccelerationXY: avoid setpoint oscillations around 0 With a higher responsiveness, after centering the stick, the velocity and acceleration setpoints could oscillate around 0 and never reach 0, due to discretization. This also prevented position lock engagement. --- .../tasks/Utility/StickAccelerationXY.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index 7a975a0c97..e3c1f2ef96 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -81,12 +81,21 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, applyFeasibilityLimit(_acceleration, dt); // Add drag to limit speed and brake again - _acceleration -= calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity); + Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity); + + // Don't allow the drag to change the sign of the velocity, otherwise we might get into oscillations around 0, due + // to discretization + if (_acceleration.norm_squared() < FLT_EPSILON && _velocity.norm_squared() < drag.norm_squared() * dt * dt) { + drag.setZero(); + _velocity.setZero(); + } + + _acceleration -= drag; applyTiltLimit(_acceleration); // Generate velocity setpoint by forward integrating commanded acceleration - _velocity += Vector2f(_acceleration) * dt; + _velocity += _acceleration * dt; lockPosition(_velocity, pos, dt, _position); }