Copter: move loiter and wpnav init out of make_safe_shut_down

resolves autotest RTLSpeed test failure

ArduCopter/mode.cpp

@@ -425,10 +425,6 @@ void Copter::Mode::make_safe_shut_down()
         break;
     }
 
-    // we may need to move this out
-    wp_nav->wp_and_spline_init();
-    // we may need to move this out
-    loiter_nav->init_target();
     pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
     pos_control->update_z_controller();
     // we may need to move this out

ArduCopter/mode_auto.cpp

@@ -742,6 +742,7 @@ void Copter::ModeAuto::wp_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        wp_nav->wp_and_spline_init();
         return;
     }
 
@@ -771,6 +772,7 @@ void Copter::ModeAuto::spline_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        wp_nav->wp_and_spline_init();
         return;
     }
 
@@ -815,6 +817,7 @@ void Copter::ModeAuto::land_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        loiter_nav->init_target();
         return;
     }
 
@@ -869,6 +872,7 @@ void Copter::ModeAuto::loiter_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        wp_nav->wp_and_spline_init();
         return;
     }
 

ArduCopter/mode_brake.cpp

@@ -34,6 +34,7 @@ void Copter::ModeBrake::run()
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
         return;
     }
 

ArduCopter/mode_guided.cpp

@@ -380,6 +380,7 @@ void Copter::Mode::auto_takeoff_run()
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed) {
         make_safe_shut_down();
+        wp_nav->shift_wp_origin_to_current_pos();
         return;
     }
 

ArduCopter/mode_land.cpp

@@ -61,6 +61,7 @@ void Copter::ModeLand::gps_run()
     // Land State Machine Determination
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        loiter_nav->init_target();
     } else {
         // set motors to full range
         motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

ArduCopter/mode_rtl.cpp

@@ -364,6 +364,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
         make_safe_shut_down();
+        loiter_nav->init_target();
         return;
     }