diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index deb905873a..98ae564450 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -9,12 +9,16 @@ static bool circle_init(bool ignore_checks) { if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) { circle_pilot_yaw_override = false; - circle_nav.init(); - // initialize vertical speeds and leash lengths + // initialize speeds and accelerations + pos_control.set_speed_xy(wp_nav.get_speed_xy()); + pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); + // initialise circle controller including setting the circle center based on vehicle speed + circle_nav.init(); + return true; }else{ return false;