Browse Source

AC_Loiter: init_target only inits pos controller if inactive

this reduces a twitch found during the development of zig-zag mode
mission-4.1.18
Randy Mackay 6 years ago
parent
commit
f0181be9c9
  1. 4
      libraries/AC_WPNav/AC_Loiter.cpp

4
libraries/AC_WPNav/AC_Loiter.cpp

@ -106,9 +106,11 @@ void AC_Loiter::init_target(const Vector3f& position)
_pos_control.set_desired_velocity_xy(0.0f,0.0f); _pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.set_desired_accel_xy(0.0f,0.0f); _pos_control.set_desired_accel_xy(0.0f,0.0f);
// initialise position controller // initialise position controller if not already active
if (!_pos_control.is_active_xy()) {
_pos_control.init_xy_controller(); _pos_control.init_xy_controller();
} }
}
/// initialize's position and feed-forward velocity from current pos and velocity /// initialize's position and feed-forward velocity from current pos and velocity
void AC_Loiter::init_target() void AC_Loiter::init_target()

Loading…
Cancel
Save