From c59dc21c8b48aaa6df715d4850e121c36cc22981 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 16 Nov 2017 16:13:53 +0900 Subject: [PATCH] AC_WPNav: sanity check wp accel max vs lean angle max --- libraries/AC_WPNav/AC_WPNav.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index fe0c80842d..d017b7b67c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -144,6 +144,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC // sanity check some parameters _loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN); + _wp_accel_cms = MIN(_wp_accel_cms, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); _wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN); } @@ -389,14 +390,6 @@ 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(); _pos_control.clear_desired_velocity_ff_z();