diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 7cb37936e8..5ef13a84eb 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -330,6 +330,11 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler) // initialise ekf position reset check check_for_ekf_position_reset(); + // initialise pos controller speed and acceleration + _pos_control.set_speed_xy(_loiter_speed_cms); + _pos_control.set_accel_xy(_loiter_accel_cmss); + _pos_control.set_jerk_xy(_loiter_jerk_max_cmsss); + calc_loiter_desired_velocity(dt,ekfGndSpdLimit); _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, true); }