From 59f3de192f8c553a45aa8f1c54c1ee840df57d60 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 10 Jan 2018 09:26:33 +0100 Subject: [PATCH] ManualSmoothingXY: velocity as criteria for direction change to prenvent fast acceleration at low spped --- src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index 00417e42a9..8f70e46a70 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -140,10 +140,11 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f; /* In almost all cases we want to use acceleration. - * Only use direction change if not aligned, no yawspeed demand and demand larger than 0.7 of max speed. + * Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s. * Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */ bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp); - bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand; + bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand + && (vel.length() > 2.0f); bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand; if (direction_change) {