AC_WPNav: init_loiter sets speed, accel before calcing stopping distance

The stopping distance depends upon the speed and acceleration so these
must be updated first

libraries/AC_WPNav/AC_WPNav.cpp

@@ -134,21 +134,19 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
 /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
 void AC_WPNav::init_loiter_target()
 {
-	Vector3f curr_vel = _inav->get_velocity();
+    Vector3f curr_vel = _inav->get_velocity();
 
-	// set target position
-    _pos_control.set_pos_target(_inav->get_position());
-
-    // initialise feed forward velocities to zero
-    _pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
-
-    // initialise pos controller speed
+    // initialise pos controller speed and acceleration
     _pos_control.set_speed_xy(_loiter_speed_cms);
-
-    // initialise pos controller acceleration
     _loiter_accel_cms = _loiter_speed_cms/2.0f;
     _pos_control.set_accel_xy(_loiter_accel_cms);
 
+    // set target position
+    _pos_control.set_target_to_stopping_point_xy();
+
+    // initialise feed forward velocities to zero
+    _pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
+
     // initialise pilot input
     _pilot_accel_fwd_cms = 0;
     _pilot_accel_rgt_cms = 0;