From 5767aa47d9f21a9d00575bb352779fd22f1daab7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 13 Dec 2013 21:59:23 +0900 Subject: [PATCH] AC_WPNav: reduce leash length for stopping We now limit the target stopping point to 1x the xy leash length while previously it was 2x. This is justified because this limit is only used when the copter is travelling at higher speeds but at higher speeds air drag tends to make the copter stop more quickly naturally. --- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 43344eb336..67ff2fc69d 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -139,7 +139,7 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo linear_distance = _wp_accel_cms/(2.0f*kP*kP); target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms); } - target_dist = constrain_float(target_dist, 0, _wp_leash_xy*2.0f); + target_dist = constrain_float(target_dist, 0, _wp_leash_xy); target.x = position.x + (target_dist * velocity.x / vel_total); target.y = position.y + (target_dist * velocity.y / vel_total);