Copter: RTL calls wp_and_spline_init

ArduCopter/control_rtl.pde

@@ -78,6 +78,9 @@ static void rtl_climb_start()
     rtl_state = InitialClimb;
     rtl_state_complete = false;
 
+    // initialise waypoint and spline controller
+    wp_nav.wp_and_spline_init();
+
     // get horizontal stopping point
     Vector3f destination;
     wp_nav.get_wp_stopping_point_xy(destination);
@@ -90,7 +93,9 @@ static void rtl_climb_start()
     destination.z = get_RTL_alt();
 #endif
 
+    // set the destination
     wp_nav.set_wp_destination(destination);
+    wp_nav.set_fast_waypoint(true);
 
     // hold current yaw during initial climb
     set_auto_yaw_mode(AUTO_YAW_HOLD);