From bb1758ecb1d9167996a80c30f078dc838fc417d0 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 21 Jul 2024 22:41:41 +0930 Subject: [PATCH] AC_Autotune: Add ABORT state for consistency between tests --- libraries/AC_AutoTune/AC_AutoTune.cpp | 6 ++++++ libraries/AC_AutoTune/AC_AutoTune.h | 1 + libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 6 +----- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 40e2e9e2e2..bf86386c69 100644 --- 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 index fb4233a23e..68a8dcee50 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -198,6 +198,7 @@ protected: 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 index 89c1a45b24..434f67671d 100644 --- 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; }