AC_Autotune: Add ABORT state for consistency between tests

lthall · Jul 26, 2024 · 04a0bd0 · 04a0bd0
1 parent 9f307c5
commit 04a0bd0
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp
@@ -134,6 +134,9 @@ void AC_AutoTune::send_step_string()
     case UPDATE_GAINS:
         gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
         return;
+    case ABORT:
+        gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test");
+        return;
     case TESTING:
         gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing");
         return;
@@ -527,7 +530,9 @@ void AC_AutoTune::control_attitude()
                 }
             }
         }
+        FALLTHROUGH;
 
+    case ABORT:
         if (axis == YAW || axis == YAW_D) {
             // todo: check to make sure we need this
             attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false);
@@ -585,6 +590,7 @@ void AC_AutoTune::backup_gains_and_initialise()
  */
 void AC_AutoTune::load_gains(enum GainType gain_type)
 {
+    // todo: add previous setting so gains are not loaded on each loop.
     switch (gain_type) {
     case GAIN_ORIGINAL:
         load_orig_gains();

diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h
@@ -198,6 +198,7 @@ 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
     };
 
     // mini steps performed while in Tuning mode, Testing step

diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
@@ -573,7 +573,6 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_
 // update min and max and test for end conditions
 void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min)
 {
-    const uint32_t now = AP_HAL::millis();
     if (angle >= angle_max) {
         if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) {
             // we have reached the angle limit before completing the measurement of maximum and minimum
@@ -587,10 +586,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl
                 LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
             }
             // ignore result and start test again
-            step = WAITING_FOR_LEVEL;
-            positive_direction = twitch_reverse_direction();
-            step_start_time_ms = now;
-            level_start_time_ms = now;
+            step = ABORT;
         } else {
             step = UPDATE_GAINS;
         }