From f6f2973acd64a9aa2c2dde9c6a8be9c8eabd43bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Aug 2015 11:10:35 +1000 Subject: [PATCH] AC_WPNav: make changes in WPNAV_ACCEL take effect immediately this makes it easier to tune the waypoint controller in auto mode --- libraries/AC_WPNav/AC_WPNav.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 991cf33079..a4c21d509c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -639,6 +639,11 @@ void AC_WPNav::update_wpnav() // update at poscontrol update rate if (dt >= _pos_control.get_dt_xy()) { + // allow the accel and speed values to be set without changing + // out of auto mode. This makes it easier to tune auto flight + _pos_control.set_accel_xy(_wp_accel_cms); + _pos_control.set_accel_z(_wp_accel_z_cms); + // sanity check dt if (dt >= 0.2f) { dt = 0.0f;