diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index b28d06df67..97d146c4d4 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -26,6 +26,10 @@ static bool auto_init(bool ignore_checks) if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } + + // initialise waypoint and spline controller + wp_nav.wp_and_spline_init(); + // start the mission mission.start(); return true; diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 72721f1f8a..adf331f4e0 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -14,6 +14,10 @@ static bool guided_pilot_yaw_override_yaw = false; static bool guided_init(bool ignore_checks) { if (GPS_ok() || ignore_checks) { + + // initialise waypoint and spline controller + wp_nav.wp_and_spline_init(); + // initialise wpnav to stopping point at current altitude // To-Do: set to current location if disarmed? // To-Do: set to stopping point altitude?