From a9d47532d0b0c07fd015adc6cfc1cd13894742a3 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Wed, 22 Dec 2021 22:37:31 -0500 Subject: [PATCH] AC_AutoTune: make initialize methods to restart testing --- libraries/AC_AutoTune/AC_AutoTune.cpp | 24 +++----------------- libraries/AC_AutoTune/AC_AutoTune.h | 15 +++++++------ libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 25 ++++++++++++++++----- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 3 +++ libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 2 -- libraries/AC_AutoTune/AC_AutoTune_Multi.h | 3 +++ 6 files changed, 36 insertions(+), 36 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 031ff0547a..b3b0d93f19 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -104,12 +104,6 @@ bool AC_AutoTune::init_internals(bool _use_poshold, FALLTHROUGH; case UNINITIALISED: - // initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli - freq_cnt = 0; - start_freq = 0.0f; - stop_freq = 0.0f; - ff_up_first_iter = true; - // autotune has never been run // so store current gains as original gains backup_gains_and_initialise(); @@ -120,14 +114,10 @@ bool AC_AutoTune::init_internals(bool _use_poshold, break; case TUNING: + // reset test variables for each vehicle + reset_vehicle_test_variables(); + // we are restarting tuning so restart where we left off - // reset test variables to continue where we left off - // reset dwell test variables if sweep was interrupted in order to restart sweep - if (!is_equal(start_freq, stop_freq)) { - freq_cnt = 0; - start_freq = 0.0f; - stop_freq = 0.0f; - } step = WAITING_FOR_LEVEL; step_start_time_ms = AP_HAL::millis(); level_start_time_ms = step_start_time_ms; @@ -228,12 +218,8 @@ const char *AC_AutoTune::type_string() const return "Rate D Down"; case RP_UP: return "Rate P Up"; - case RP_DOWN: - return "Rate P Down"; case RFF_UP: return "Rate FF Up"; - case RFF_DOWN: - return "Rate FF Down"; case SP_UP: return "Angle P Up"; case SP_DOWN: @@ -526,8 +512,6 @@ void AC_AutoTune::control_attitude() case MAX_GAINS: updating_max_gains_all(axis); break; - case RP_DOWN: - case RFF_DOWN: case TUNE_COMPLETE: break; } @@ -602,9 +586,7 @@ void AC_AutoTune::control_attitude() break; } break; - case RP_DOWN: case RFF_UP: - case RFF_DOWN: case MAX_GAINS: case TUNE_COMPLETE: break; diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 7c60728bc9..05ce31cd6d 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -115,6 +115,9 @@ protected: // load gains for next test. relies on axis variable being set virtual void load_test_gains() = 0; + // reset the test vaariables for each vehicle + virtual void reset_vehicle_test_variables() = 0; + // test initialization and run methods that should be overridden for each vehicle virtual void test_init() = 0; virtual void test_run(AxisType test_axis, const float dir_sign) = 0; @@ -201,13 +204,11 @@ protected: RD_UP = 0, // rate D is being tuned up RD_DOWN = 1, // rate D is being tuned down RP_UP = 2, // rate P is being tuned up - RP_DOWN = 3, // rate P is being tuned down - RFF_UP = 4, // rate FF is being tuned up - RFF_DOWN = 5, // rate FF is being tuned down - SP_UP = 6, // angle P is being tuned up - SP_DOWN = 7, // angle P is being tuned down - MAX_GAINS = 8, // max allowable stable gains are determined - TUNE_COMPLETE = 9 // Reached end of tuning + RFF_UP = 3, // rate FF is being tuned up + SP_UP = 4, // angle P is being tuned up + SP_DOWN = 5, // angle P is being tuned down + MAX_GAINS = 6, // max allowable stable gains are determined + TUNE_COMPLETE = 7 // Reached end of tuning }; TuneType tune_seq[6]; // holds sequence of tune_types to be performed uint8_t tune_seq_curr; // current tune sequence step diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 7213e25755..14398604d4 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -102,7 +102,7 @@ AC_AutoTune_Heli::AC_AutoTune_Heli() // initialize tests for each tune type void AC_AutoTune_Heli::test_init() { - if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) { + if (tune_type == RFF_UP) { rate_ff_test_init(); step_time_limit_ms = 10000; } else if (tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP) { @@ -187,7 +187,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) if (tune_type == SP_UP) { angle_dwell_test_run(start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); - } else if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) { + } else if (tune_type == RFF_UP) { rate_ff_test_run(AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD, AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS, dir_sign); } else if (tune_type == RP_UP || tune_type == RD_UP) { dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); @@ -247,7 +247,6 @@ void AC_AutoTune_Heli::do_gcs_announcements() // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f d=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)tune_rd); break; case RD_DOWN: - case RP_DOWN: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd); break; case RP_UP: @@ -259,9 +258,6 @@ void AC_AutoTune_Heli::do_gcs_announcements() } gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff); break; - case RFF_DOWN: - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff); - break; case SP_DOWN: case SP_UP: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel); @@ -281,6 +277,12 @@ void AC_AutoTune_Heli::backup_gains_and_initialise() { AC_AutoTune::backup_gains_and_initialise(); + // initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli + freq_cnt = 0; + start_freq = 0.0f; + stop_freq = 0.0f; + ff_up_first_iter = true; + orig_bf_feedforward = attitude_control->get_bf_feedforward(); // backup original pids and initialise tuned pid values @@ -2061,6 +2063,17 @@ float AC_AutoTune_Heli::get_yaw_rate_filt_min() const return AUTOTUNE_RLPF_MIN; } +// reset the test vaariables for each vehicle +void AC_AutoTune_Heli::reset_vehicle_test_variables() +{ + // reset dwell test variables if sweep was interrupted in order to restart sweep + if (!is_equal(start_freq, stop_freq)) { + freq_cnt = 0; + start_freq = 0.0f; + stop_freq = 0.0f; + } +} + // set the tuning test sequence void AC_AutoTune_Heli::set_tune_sequence() { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 9a46c5e1d1..004e03f39d 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -53,6 +53,9 @@ protected: // load test gains void load_test_gains() override; + // reset the test vaariables for heli + void reset_vehicle_test_variables() override; + // initializes test void test_init() override; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 3164da1ff7..9354dc3416 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -120,11 +120,9 @@ void AC_AutoTune_Multi::do_gcs_announcements() case RD_UP: case RD_DOWN: case RP_UP: - case RP_DOWN: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd); break; case RFF_UP: - case RFF_DOWN: break; case SP_DOWN: case SP_UP: diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index b68a8c2100..5720a6bddc 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -53,6 +53,9 @@ protected: // load test gains void load_test_gains() override; + // reset the test vaariables for heli + void reset_vehicle_test_variables() override {}; + void test_init() override; void test_run(AxisType test_axis, const float dir_sign) override; void do_gcs_announcements() override;