Copter: Autotune: Increase result reliability

lthall · Feb 17, 2024 · d0a3b55 · d0a3b55
1 parent 9986fb9
commit d0a3b55
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 20 deletions.
diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp
@@ -20,9 +20,8 @@
 #endif
 #define AUTOTUNE_LEVEL_RATE_Y_CD            750     // rate which qualifies as level for yaw
 #define AUTOTUNE_REQUIRED_LEVEL_TIME_MS     500     // time we require the aircraft to be level
-#define AUTOTUNE_LEVEL_TIMEOUT_MS          2000     // time out for level
-#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000     // level failure warning messages sent at this interval to users
-#define AUTOTUNE_ANGLE_MAX_RLLPIT          30.0f    // maximum allowable angle in degrees during testing
+#define AUTOTUNE_LEVEL_TIMEOUT_MS           2000     // time out for level
+#define AUTOTUNE_ANGLE_MAX_RLLPIT           30.0f    // maximum allowable angle in degrees during testing
 
 AC_AutoTune::AC_AutoTune()
 {
@@ -270,20 +269,16 @@ void AC_AutoTune::run()
 // return true if vehicle is close to level
 bool AC_AutoTune::currently_level()
 {
-    float threshold_mul = 1.0;
-
     uint32_t now_ms = AP_HAL::millis();
-    if (now_ms - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
-        // after a long wait we use looser threshold, to allow tuning
-        // with poor initial gains
-        threshold_mul *= 2;
-    }
 
-    // display warning if vehicle fails to level
-    if ((now_ms - level_start_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS) &&
-        (now_ms - level_fail_warning_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS)) {
-        gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: failing to level, please tune manually");
-        level_fail_warning_time_ms = now_ms;
+    // slew threshold to ensure settling time
+    // relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS
+    float threshold_mul = constrain_float((now_ms - level_start_time_ms) / AUTOTUNE_LEVEL_TIMEOUT_MS, 0.0, 2.0);
+    // abort AutoTune if we pass 2 * AUTOTUNE_LEVEL_TIMEOUT_MS
+    if (now_ms - level_start_time_ms > 2 * AUTOTUNE_LEVEL_TIMEOUT_MS) {
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Failed to level, please tune manually");
+        mode = FAILED;
+        LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
     }
 
     if (fabsf(ahrs_view->roll_sensor - roll_cd) > threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD) {

diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h
@@ -330,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
-    uint32_t level_fail_warning_time_ms;            // last time level failure warning message was sent to GCS
 
     // 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
@@ -41,7 +41,17 @@
  *      h) invokes a 20deg angle request on roll or pitch
  *      i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg)
  *      j) decreases stab P by 25%
- *
+ * 
+ * New Work:
+ *      1) Calculate maximum safe values based on a maximum test angle of 2/3 ANGLE_MAX
+ *      2) Maximum test angle and rate must stop before 2/3 ANGLE_MAX using ATC_ACCEL_X_MAX and ATC_RATE_X_MAX
+ *      3) Test will update ATC so recovery is smooth
+ *      4) Test will stop making measurments but not stop the twich
+ *      5) We will update twitch rate target based on the results of the last four twitches
+ *      6) We will stop and fail if tuning parameters hit minimum P values
+ *      7) We will stop and fail if aircraft can't stabilize effectivly
+ *      8) Twiches will be symetrical and no longer skip one side
+ *      9) Maybe use Quaternion rotation from starting point for angle test
  */
 
 #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS   1000U     // timeout for tuning mode's testing step
@@ -916,10 +926,10 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi
         // do not decrease the P term past the minimum
         if (tune_p <= tune_p_min) {
             tune_p = tune_p_min;
-            LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Max Rate P Gain Determination Failed");
+            mode = FAILED;
+            LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
         }
-        // cancel change in direction
-        positive_direction = !positive_direction;
     } else {
         if (ignore_next == false) {
             // if maximum measurement was lower than target so decrement the success counter
@@ -965,6 +975,9 @@ void AC_AutoTune_Multi::updating_angle_p_down(float &tune_p, float tune_p_min, f
             tune_p = tune_p_min;
             counter = AUTOTUNE_SUCCESS_COUNT;
             LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Max Angle P Gain Determination Failed");
+            mode = FAILED;
+            LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
        }
     }
 }