AC_Autotune: Fix gain load problem when mode set to FAILED

Author: lthall

libraries/AC_AutoTune/AC_AutoTune.cpp

@@ -185,6 +185,9 @@ void AC_AutoTune::send_step_string()
     case TESTING:
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Testing");
         return;
+    case COMPLETE:
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Complete");
+        break;
     }
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: unknown step");
 }
@@ -387,11 +390,6 @@ void AC_AutoTune::control_attitude()
             step = TESTING;
             step_start_time_ms = now;
             step_time_limit_ms = get_testing_step_timeout_ms();
-            // set gains to their to-be-tested values
-            load_gains(GAIN_TEST);
-        } else {
-            // when waiting for level we use the intra-test gains
-            load_gains(GAIN_INTRA_TEST);
         }
 
         // Initialize test-specific variables
@@ -419,9 +417,6 @@ void AC_AutoTune::control_attitude()
 
     case TESTING: {
         // Run the twitching step
-        load_gains(GAIN_TEST);
-
-        // run the test
         test_run(axis, direction_sign);
 
         // Check for failure causing reverse response
@@ -569,9 +564,7 @@ void AC_AutoTune::control_attitude()
                     update_gcs(AUTOTUNE_MESSAGE_SUCCESS);
                     LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SUCCESS);
                     AP_Notify::events.autotune_complete = true;
-
-                    // Return to original gains for landing
-                    load_gains(GainType::GAIN_ORIGINAL);
+                    step = COMPLETE;
                 } else {
                     AP_Notify::events.autotune_next_axis = true;
                     reset_update_gain_variables();
@@ -586,16 +579,39 @@ void AC_AutoTune::control_attitude()
             attitude_control->input_euler_angle_roll_pitch_yaw_cd(0.0f, 0.0f, ahrs_view->yaw_sensor, false);
         }
 
-        // set gains to their intra-test values (which are very close to the original gains)
-        load_gains(GAIN_INTRA_TEST);
-
         // reset testing step
         step = WAITING_FOR_LEVEL;
         positive_direction = twitch_reverse_direction();
         step_start_time_ms = now;
         level_start_time_ms = now;
         step_time_limit_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS;
         break;
+
+    case COMPLETE:
+        // we don't need to do anything
+        break;
+    }
+
+    // Load the appropriate gains
+    switch (step) {
+    case WAITING_FOR_LEVEL:
+            // when waiting for level we use the intra-test gains
+            load_gains(GAIN_INTRA_TEST);
+        break;
+    case TESTING:
+            // set gains to their to-be-tested values
+            load_gains(GAIN_TEST);
+        break;
+    case UPDATE_GAINS:
+        FALLTHROUGH;
+    case ABORT:
+        // set gains to their intra-test values (which are very close to the original gains)
+        load_gains(GAIN_INTRA_TEST);
+        break;
+    case COMPLETE:
+        // Return to original gains for landing
+        load_gains(GainType::GAIN_ORIGINAL);
+        break;
     }
 }
 

libraries/AC_AutoTune/AC_AutoTune.h

@@ -208,7 +208,8 @@ class AC_AutoTune
         WAITING_FOR_LEVEL = 0,    // autotune is waiting for vehicle to return to level before beginning the next twitch
         TESTING           = 1,    // autotune has begun a test and is watching the resulting vehicle movement
         UPDATE_GAINS      = 2,    // autotune has completed a test and is updating the gains based on the results
-        ABORT             = 3     // load normal gains and return to WAITING_FOR_LEVEL
+        ABORT             = 3,    // load normal gains and return to WAITING_FOR_LEVEL
+        COMPLETE          = 4     // load original gains
     };
 
     // mini steps performed while in Tuning mode, Testing step