Copter: rtl initialises wp controller earlier

We need to initialise wp controller or the build_path could use the wrong speeds and accelerations when calculating the origin (aka stopping point)

ArduCopter/control_rtl.cpp

@@ -11,6 +11,8 @@
 bool Copter::rtl_init(bool ignore_checks)
 {
     if (position_ok() || ignore_checks) {
+        // initialise waypoint and spline controller
+        wp_nav->wp_and_spline_init();
         rtl_build_path(!failsafe.terrain);
         rtl_climb_start();
         return true;
@@ -91,9 +93,6 @@ void Copter::rtl_climb_start()
     rtl_state = RTL_InitialClimb;
     rtl_state_complete = false;
 
-    // initialise waypoint and spline controller
-    wp_nav->wp_and_spline_init();
-
     // RTL_SPEED == 0 means use WPNAV_SPEED
     if (g.rtl_speed_cms != 0) {
         wp_nav->set_speed_xy(g.rtl_speed_cms);