Copter: Autotune: Feedback and Fixes

lthall · Feb 17, 2024 · bce671c · bce671c
1 parent 272f8f8
commit bce671c
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 2 deletions.
diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp
@@ -270,7 +270,7 @@ void AC_AutoTune::run()
 // return true if vehicle is close to level
 bool AC_AutoTune::currently_level()
 {
-    uint32_t now_ms = AP_HAL::millis();
+    const uint32_t now_ms = AP_HAL::millis();
 
     // slew threshold to ensure settling time
     // relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS

diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h
@@ -272,6 +272,7 @@ class AC_AutoTune
     float    test_angle_max;                        // the maximum angle achieved during TESTING_ANGLE step-multi only
     uint32_t step_start_time_ms;                    // start time of current tuning step (used for timeout checks)
     uint32_t step_time_limit_ms;                    // time limit of current autotune process
+    uint32_t level_start_time_ms;                   // start time of waiting for level
     int8_t   counter;                               // counter for tuning gains
     float    target_rate;                           // target rate-multi only
     float    target_angle;                          // target angle-multi only
@@ -329,7 +330,6 @@ class AC_AutoTune
 
     // variables
     uint32_t override_time;                         // the last time the pilot overrode the controls
-    uint32_t level_start_time_ms;                   // start time of waiting for level
 
     // time in ms of last pilot override warning
     uint32_t last_pilot_override_warning;

diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
@@ -543,6 +543,7 @@ void AC_AutoTune_Multi::twitching_test_rate(float rate, float rate_target_max, f
 // 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)
 {
+    const uint32_t now = AP_HAL::millis();
     if (angle >= angle_max) {
         if (is_equal(rate, meas_rate_min) && step_scaler > 0.5f) {
             // we have reached the angle limit before completing the measurement of maximum and minimum
@@ -551,6 +552,8 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl
             // 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;
         } else {
             step = UPDATE_GAINS;
         }