From a6bfafefb050d356ab9d0eff5e937c3d930cd49d Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 28 Feb 2019 15:16:19 +0900
Subject: [PATCH] Copter: move loiter and wpnav init out of make_safe_shut_down

resolves autotest RTLSpeed test failure
---
 ArduCopter/mode.cpp        | 4 ----
 ArduCopter/mode_auto.cpp   | 4 ++++
 ArduCopter/mode_brake.cpp  | 1 +
 ArduCopter/mode_guided.cpp | 1 +
 ArduCopter/mode_land.cpp   | 1 +
 ArduCopter/mode_rtl.cpp    | 1 +
 6 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index 859b62379c..8d721c7513 100644
--- a/ArduCopter/mode.cpp
+++ b/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
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 5b2e93f514..b7196fa31a 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/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;
     }
 
diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp
index 7f2c69872e..71182879f0 100644
--- a/ArduCopter/mode_brake.cpp
+++ b/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;
     }
 
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 1546451ad4..01a23dad2f 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/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;
     }
 
diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp
index 4f5c8d150b..312774ab79 100644
--- a/ArduCopter/mode_land.cpp
+++ b/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);
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index c4a74296b8..a2d8651468 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/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;
     }