From 085ec0e8efc07751c78b2bea4f99abfe36c16779 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 11 Jul 2021 20:42:28 -0400 Subject: [PATCH] flight_mode_manager: StickAccelerationXY minimize setpoint oscillations around 0 --- .../tasks/Utility/StickAccelerationXY.cpp | 7 +++++-- 1 file changed, 5 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 5c0d8f9ca7..eceaa6da39 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -99,8 +99,11 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, // 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_setpoint.norm_squared() < FLT_EPSILON - && _velocity_setpoint.norm_squared() < drag.norm_squared() * dt * dt) { + if (((_acceleration_setpoint.norm_squared() < FLT_EPSILON) + || (sign(_acceleration_setpoint_prev(0)) != sign(_acceleration_setpoint(0))) + || (sign(_acceleration_setpoint_prev(1)) != sign(_acceleration_setpoint(1)))) + && (_velocity_setpoint.norm_squared() < (drag.norm_squared() * dt * dt))) { + drag.setZero(); _velocity_setpoint.setZero(); }