From e78c6804a0ddc5a7d53a21a92a68e061b1d11131 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 30 Apr 2021 11:29:34 +0900
Subject: [PATCH] AC_AutoTune: remove start method and always init position
 controller

---
 libraries/AC_AutoTune/AC_AutoTune.cpp | 51 ++++++++++++---------------
 libraries/AC_AutoTune/AC_AutoTune.h   |  6 ++--
 2 files changed, 26 insertions(+), 31 deletions(-)

diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp
index 596df13e88..7eeb9dac0a 100644
--- a/libraries/AC_AutoTune/AC_AutoTune.cpp
+++ b/libraries/AC_AutoTune/AC_AutoTune.cpp
@@ -140,8 +140,6 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
                                  AP_AHRS_View *_ahrs_view,
                                  AP_InertialNav *_inertial_nav)
 {
-    bool success = true;
-
     use_poshold = _use_poshold;
     attitude_control = _attitude_control;
     pos_control = _pos_control;
@@ -149,6 +147,14 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
     inertial_nav = _inertial_nav;
     motors = AP_Motors::get_singleton();
 
+    // exit immediately if motor are not armed
+    if ((motors == nullptr) || !motors->armed()) {
+        return false;
+    }
+
+    // initialise position controller
+    init_position_controller();
+
     switch (mode) {
     case FAILED:
         // autotune has been run but failed so reset state to uninitialized
@@ -158,31 +164,24 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
 
     case UNINITIALISED:
         // autotune has never been run
-        success = start();
-        if (success) {
-            // so store current gains as original gains
-            backup_gains_and_initialise();
-            // advance mode to tuning
-            mode = TUNING;
-            // send message to ground station that we've started tuning
-            update_gcs(AUTOTUNE_MESSAGE_STARTED);
-        }
+        // so store current gains as original gains
+        backup_gains_and_initialise();
+        // advance mode to tuning
+        mode = TUNING;
+        // send message to ground station that we've started tuning
+        update_gcs(AUTOTUNE_MESSAGE_STARTED);
         break;
 
     case TUNING:
-        // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off
-        success = start();
-        if (success) {
-            // reset gains to tuning-start gains (i.e. low I term)
-            load_gains(GAIN_INTRA_TEST);
-            AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART);
-            update_gcs(AUTOTUNE_MESSAGE_STARTED);
-        }
+        // we are restarting tuning so restart where we left off
+        // reset gains to tuning-start gains (i.e. low I term)
+        load_gains(GAIN_INTRA_TEST);
+        AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART);
+        update_gcs(AUTOTUNE_MESSAGE_STARTED);
         break;
 
     case SUCCESS:
-        // we have completed a tune and the pilot wishes to test the new gains in the current flight mode
-        // so simply apply tuning gains (i.e. do not change flight mode)
+        // we have completed a tune and the pilot wishes to test the new gains
         load_gains(GAIN_TUNED);
         update_gcs(AUTOTUNE_MESSAGE_TESTING);
         AP::logger().Write_Event(LogEvent::AUTOTUNE_PILOT_TESTING);
@@ -191,7 +190,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
 
     have_position = false;
 
-    return success;
+    return true;
 }
 
 // stop - should be called when the ch7/ch8 switch is switched OFF
@@ -210,13 +209,9 @@ void AC_AutoTune::stop()
     // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch
 }
 
-// start - Initialize autotune flight mode
-bool AC_AutoTune::start(void)
+// initialise position controller
+bool AC_AutoTune::init_position_controller(void)
 {
-    if (!motors->armed()) {
-        return false;
-    }
-
     // initialize vertical speeds and leash lengths
     init_z_limits();
 
diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h
index a279f434ba..67d69ee1a3 100644
--- a/libraries/AC_AutoTune/AC_AutoTune.h
+++ b/libraries/AC_AutoTune/AC_AutoTune.h
@@ -61,9 +61,6 @@ protected:
 
     // log PIDs at full rate for during twitch
     virtual void log_pids() = 0;
-    
-    // start tune - virtual so that vehicle code can add additional pre-conditions
-    virtual bool start(void);
 
     // return true if we have a good position estimate
     virtual bool position_ok();
@@ -75,6 +72,9 @@ protected:
                         AP_AHRS_View *ahrs_view,
                         AP_InertialNav *inertial_nav);
 
+    // initialise position controller
+    bool init_position_controller();
+
 private:
     void control_attitude();
     void backup_gains_and_initialise();