mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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
This commit is contained in:
parent
bc95c25cc9
commit
037469db30
@ -147,6 +147,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
|
||||
|
Loading…
Reference in New Issue
Block a user