From cd18607f2509c1f3715dd6b83db6df28b4238c59 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 12 Apr 2024 01:09:33 -0400 Subject: [PATCH] AC_AutoTune: reworked updating rate p and d up --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 155 ++++++++------------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 4 +- 2 files changed, 58 insertions(+), 101 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index fd82376d90..9bf549781c 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -190,23 +190,22 @@ void AC_AutoTune_Heli::test_init() case RP_UP: case RD_UP: // initialize start frequency - if (is_zero(start_freq)) { + if (!is_positive(next_test_freq)) { // continue using frequency where testing left off with RD_UP completed - if (test_phase[12] > 0.0f && test_phase[12] < 180.0f && tune_type == RP_UP) { - freq_cnt = 12; + if (curr_data.phase > 150.0f && curr_data.phase < 180.0f && tune_type == RP_UP) { + start_freq = curr_data.freq; // start with freq found for sweep where phase was 180 deg } else if (!is_zero(sweep_tgt.ph180.freq)) { - freq_cnt = 12; - test_freq[freq_cnt] = sweep_tgt.ph180.freq - 0.25f * 3.14159f * 2.0f; + start_freq = sweep_tgt.ph180.freq - 0.25f * 3.14159f * 2.0f; // otherwise start at min freq to step up in dwell frequency until phase > 160 deg } else { - freq_cnt = 0; - test_freq[freq_cnt] = min_sweep_freq; + start_freq = min_sweep_freq; } - curr_test.freq = test_freq[freq_cnt]; - start_freq = curr_test.freq; - stop_freq = curr_test.freq; + } else { + start_freq = next_test_freq; } + stop_freq = start_freq; + attitude_control->bf_feedforward(false); attitude_control->use_sqrt_controller(false); @@ -1000,14 +999,14 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis) { switch (test_axis) { case ROLL: - updating_rate_p_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); + updating_rate_p_up(tune_roll_rp, curr_data, next_test_freq, max_rate_p); break; case PITCH: - updating_rate_p_up(tune_pitch_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); + updating_rate_p_up(tune_pitch_rp, curr_data, next_test_freq, max_rate_p); break; case YAW: case YAW_D: - updating_rate_p_up(tune_yaw_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); + updating_rate_p_up(tune_yaw_rp, curr_data, next_test_freq, max_rate_p); break; } } @@ -1017,14 +1016,14 @@ void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis) { switch (test_axis) { case ROLL: - updating_rate_d_up(tune_roll_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d); + updating_rate_d_up(tune_roll_rd, curr_data, next_test_freq, max_rate_d); break; case PITCH: - updating_rate_d_up(tune_pitch_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d); + updating_rate_d_up(tune_pitch_rd, curr_data, next_test_freq, max_rate_d); break; case YAW: case YAW_D: - updating_rate_d_up(tune_yaw_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d); + updating_rate_d_up(tune_yaw_rd, curr_data, next_test_freq, max_rate_d); break; } } @@ -1173,108 +1172,66 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data } // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain -void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p) +void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p) { float test_freq_incr = 0.25f * 3.14159f * 2.0f; + next_freq = test_data.freq; - if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { - if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) { - rp_prev_good_frq_cnt = frq_cnt; - } else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) { - if (phase[frq_cnt] - 360.0f < 180.0f) { - rp_prev_good_frq_cnt = frq_cnt; - } - } else if (frq_cnt > 1 && phase[frq_cnt] > 200.0f && !is_zero(phase[frq_cnt])) { - frq_cnt = 11; - } - frq_cnt++; - if (frq_cnt == 12) { - freq[frq_cnt] = freq[rp_prev_good_frq_cnt]; - curr_test.freq = freq[frq_cnt]; - } else { - freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; - curr_test.freq = freq[frq_cnt]; - } - } else if (is_equal(start_freq,stop_freq)) { - if (phase[frq_cnt] > 180.0f) { - curr_test.freq = curr_test.freq - 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test.freq; - } else if (phase[frq_cnt] < 160.0f) { - curr_test.freq = curr_test.freq + 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test.freq; - } else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { - if (gain[frq_cnt] < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) { - tune_p += 0.05f * max_gain_p.max_allowed; - } else { - counter = AUTOTUNE_SUCCESS_COUNT; - // reset curr_test.freq and frq_cnt for next test - curr_test.freq = freq[0]; - frq_cnt = 0; - tune_p -= 0.05f * max_gain_p.max_allowed; - tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); - } - } + if (is_zero(test_data.phase)) { + // bad test point. increase slightly in hope of getting better result + next_freq += 0.5f * test_freq_incr; + return; } - if (counter == AUTOTUNE_SUCCESS_COUNT) { - start_freq = 0.0f; //initializes next test that uses dwell test - } else { - start_freq = curr_test.freq; - stop_freq = curr_test.freq; + if (test_data.phase < 150.0f) { + next_freq += test_freq_incr; + } else if (test_data.phase >= 150.0f && test_data.phase < 160.0f) { + next_freq += 0.5f * test_freq_incr; + } else if (test_data.phase >= 160.0f && test_data.phase < 180.0f) { + if (test_data.gain < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) { + tune_p += 0.05f * max_gain_p.max_allowed; + } else { + counter = AUTOTUNE_SUCCESS_COUNT; + // reset next_freq for next test + next_freq = 0.0f; + tune_p -= 0.05f * max_gain_p.max_allowed; + tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); + } + } else if (test_data.phase >= 180.0f) { + next_freq -= 0.5f * test_freq_incr; } } // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum -void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d) +void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d) { float test_freq_incr = 0.25f * 3.14159f * 2.0f; // set for 1/4 hz increments + next_freq = test_data.freq; - // if sweep failed to find frequency for 180 phase then use dwell to find frequency - if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { - if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) { - rd_prev_good_frq_cnt = frq_cnt; - } else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) { - if (phase[frq_cnt] - 360.0f < 180.0f) { - rd_prev_good_frq_cnt = frq_cnt; - } - } else if (frq_cnt > 1 && phase[frq_cnt] > 200.0f && !is_zero(phase[frq_cnt])) { - frq_cnt = 11; - } - frq_cnt++; - if (frq_cnt == 12) { - freq[frq_cnt] = freq[rd_prev_good_frq_cnt]; - curr_test.freq = freq[frq_cnt]; - } else { - freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; - curr_test.freq = freq[frq_cnt]; - } - } else if (is_equal(start_freq,stop_freq)) { - if (phase[frq_cnt] > 180.0f) { - curr_test.freq = curr_test.freq - 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test.freq; - } else if (phase[frq_cnt] < 160.0f) { - curr_test.freq = curr_test.freq + 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test.freq; - } else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { - if ((gain[frq_cnt] < rd_prev_gain || is_zero(rd_prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) { + if (is_zero(test_data.phase)) { + // bad test point. increase slightly in hope of getting better result + next_freq += 0.5f * test_freq_incr; + return; + } + + if (test_data.phase < 150.0f) { + next_freq += test_freq_incr; + } else if (test_data.phase >= 150.0f && test_data.phase < 160.0f) { + next_freq += 0.5f * test_freq_incr; + } else if (test_data.phase >= 160.0f && test_data.phase < 180.0f) { + if ((test_data.gain < rd_prev_gain || is_zero(rd_prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) { tune_d += 0.05f * max_gain_d.max_allowed; - rd_prev_gain = gain[frq_cnt]; + rd_prev_gain = test_data.gain; } else { counter = AUTOTUNE_SUCCESS_COUNT; - // reset curr_test.freq and frq_cnt for next test - curr_test.freq = freq[0]; - frq_cnt = 0; + // reset next freq and rd_prev_gain for next test + next_freq = 0; rd_prev_gain = 0.0f; tune_d -= 0.05f * max_gain_d.max_allowed; tune_d = constrain_float(tune_d,0.0f,0.6f * max_gain_d.max_allowed); } - } - } - if (counter == AUTOTUNE_SUCCESS_COUNT) { - start_freq = 0.0f; //initializes next test that uses dwell test - } else { - start_freq = curr_test.freq; - stop_freq = curr_test.freq; + } else if (test_data.phase >= 180.0f) { + next_freq -= 0.5f * test_freq_incr; } } diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index ab8642cff4..012094e721 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -186,10 +186,10 @@ private: void updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq); // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain - void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p); + void updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p); // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum - void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d); + void updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d); // updating_angle_p_up - determines maximum angle p gain for pitch and roll void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);