From f0181be9c95e6783f12f675f0c6229538cd537fc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 4 Oct 2018 19:54:32 +0900 Subject: [PATCH] AC_Loiter: init_target only inits pos controller if inactive this reduces a twitch found during the development of zig-zag mode --- libraries/AC_WPNav/AC_Loiter.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 0c405e5759..8433207652 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -106,8 +106,10 @@ void AC_Loiter::init_target(const Vector3f& position) _pos_control.set_desired_velocity_xy(0.0f,0.0f); _pos_control.set_desired_accel_xy(0.0f,0.0f); - // initialise position controller - _pos_control.init_xy_controller(); + // initialise position controller if not already active + if (!_pos_control.is_active_xy()) { + _pos_control.init_xy_controller(); + } } /// initialize's position and feed-forward velocity from current pos and velocity