From 25c113a5273ab315c511773b23ef81e2a93f0026 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 2 Dec 2020 15:43:43 +0100 Subject: [PATCH] StickAccelerationXY: boost low speed brake --- src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp b/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp index 285cbc79ca..7a975a0c97 100644 --- a/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp +++ b/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp @@ -121,7 +121,9 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo drag_coefficient *= _brake_boost_filter.getState(); - return drag_coefficient.emult(vel_sp); + // increase drag with sqareroot function when velocity is lower than 1m/s + const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm()); + return drag_coefficient.emult(velocity_with_sqrt_boost); } void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration) @@ -142,8 +144,6 @@ void StickAccelerationXY::lockPosition(const Vector2f &vel_sp, const Vector3f &p pos_sp = Vector2f(pos); } - pos_sp += vel_sp * dt; - } else { pos_sp.setNaN();