From 4908350ccb1d0ae0dd20663cc11d20cc6481d9f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Apr 2016 17:47:50 +1000 Subject: [PATCH] AC_WPNav: limit WPNAV_ACCEL to that implied by ANGLE_MAX this prevents an overshoot and backtracking in the navigation code when WPNAV_ACCEL is unachievable due to an angle limit --- libraries/AC_WPNav/AC_WPNav.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b0504161bd..787b571ae5 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -393,6 +393,14 @@ void AC_WPNav::wp_and_spline_init() _wp_accel_cms.set_and_save(WPNAV_ACCELERATION); } + // also limit the accel using the maximum lean angle. This + // prevents the navigation controller from trying to move the + // target point at an unachievable rate + float accel_limit_cms = GRAVITY_MSS * 100 * tanf(radians(_attitude_control.lean_angle_max()*0.01f)); + if (_wp_accel_cms > accel_limit_cms) { + _wp_accel_cms.set(accel_limit_cms); + } + // initialise position controller _pos_control.init_xy_controller();