From 078eef91cbfe5a9a39cf10238fc3c49c44225112 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 24 Feb 2019 15:57:37 +1100 Subject: [PATCH] AC_WPNav: prevent I term buildup during landing this prevents I term buildup in the XY velocity controller during landing. This to account for the EKF giving a non-zero horizontal velocity when we have touched down. The I term buildup in the XY velocity controller can lead to the attitude error going above the level for disabling the relax function as the throttle mix is changed. That results in large motor outputs which can tip over the vehicle after touchdown. Thanks to Leonard for the suggestion --- libraries/AC_WPNav/AC_Loiter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index cd1babc35c..7c01c59ed3 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -149,6 +149,10 @@ void AC_Loiter::soften_for_landing() // set target position to current position _pos_control.set_xy_target(curr_pos.x, curr_pos.y); + + // also prevent I term build up in xy velocity controller. Note + // that this flag is reset on each loop, in run_xy_controller() + _pos_control.set_limit_accel_xy(); } /// set pilot desired acceleration in centi-degrees