Skip to content

Commit

Permalink
AC_WPNav: prevent I term buildup during landing
Browse files Browse the repository at this point in the history
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
  • Loading branch information
tridge authored and rmackay9 committed Feb 27, 2019
1 parent 6668a2e commit a9b5a0f
Showing 1 changed file with 4 additions and 0 deletions.
4 changes: 4 additions & 0 deletions libraries/AC_WPNav/AC_Loiter.cpp
Expand Up @@ -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
Expand Down

0 comments on commit a9b5a0f

Please sign in to comment.