From 607004ce4ca9d6a47e06c993604ed60915ef5c5a Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Mon, 17 Jan 2022 09:37:43 -0500 Subject: [PATCH] AC_AutoTune: combine update rate and angle update methods in freqresp library --- .../AC_AutoTune/AC_AutoTune_FreqResp.cpp | 275 ++++-------------- libraries/AC_AutoTune/AC_AutoTune_FreqResp.h | 24 +- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 14 +- 3 files changed, 81 insertions(+), 232 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp index eba44c1d39..599b2a36ca 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -6,9 +6,10 @@ This library receives time history data (angular rate or angle) during a dwell t #include "AC_AutoTune_FreqResp.h" // Initialize the Frequency Response Object. Must be called before running dwell or frequency sweep tests -void AC_AutoTune_FreqResp::init(InputType input_type) +void AC_AutoTune_FreqResp::init(InputType input_type, ResponseType response_type) { excitation = input_type; + response = response_type; max_target_cnt = 0; min_target_cnt = 0; max_meas_cnt = 0; @@ -29,188 +30,12 @@ void AC_AutoTune_FreqResp::init(InputType input_type) cycle_complete = false; } -// update_rate - this function receives time history data during a dwell and frequency sweep tests for max gain, -// rate P and rate D tuning and determines the gain and phase of the response to the input. For dwell tests once -// the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles -// and the cycle_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and -// cycle_complete flag is set to indicate when to pull the phase and gain data. The flag is reset to enable the next -// cycle to be analyzed. -void AC_AutoTune_FreqResp::update_rate(float tgt_rate, float meas_rate, float tgt_freq) -{ - uint32_t now = AP_HAL::millis(); - uint32_t half_cycle_time_ms = 0; - uint32_t cycle_time_ms = 0; - - if (cycle_complete) { - return; - } - - if (!is_zero(tgt_freq)) { - half_cycle_time_ms = (uint32_t)(300 * M_2PI / tgt_freq); - cycle_time_ms = (uint32_t)(1000 * M_2PI / tgt_freq); - } - - if (input_start_time_ms == 0) { - input_start_time_ms = now; - } - - // cycles are complete! determine gain and phase and exit - if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) { - float delta_time = 0.0f; - float sum_gain = 0.0f; - uint8_t cnt = 0; - uint8_t gcnt = 0; - uint16_t meas_cnt, tgt_cnt; - float meas_ampl = 0.0f; - float tgt_ampl = 0.0f; - uint32_t meas_time = 0; - uint32_t tgt_time = 0; - for (uint8_t i = 0; i < AUTOTUNE_DWELL_CYCLES; i++) { - meas_cnt=0; - tgt_cnt=0; - pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time); - pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time); - push_to_meas_buffer(0, 0.0f, 0); - push_to_tgt_buffer(0, 0.0f, 0); - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tgt_cnt=%f meas_cnt=%f", (double)(tgt_cnt), (double)(meas_cnt)); - - if (meas_cnt == tgt_cnt && meas_cnt != 0) { - if (tgt_ampl > 0.0f) { - sum_gain += meas_ampl / tgt_ampl; - gcnt++; - } - float d_time = (float)(meas_time - tgt_time); - if (d_time < 2.0f * (float)cycle_time_ms) { - delta_time += d_time; - cnt++; - } - } else if (meas_cnt > tgt_cnt) { - pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time); - push_to_tgt_buffer(0, 0.0f, 0); - } else if (meas_cnt < tgt_cnt) { - pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time); - push_to_meas_buffer(0, 0.0f, 0); - } - - } - if (gcnt > 0) { - curr_test_gain = sum_gain / gcnt; - } - if (cnt > 0) { - delta_time = delta_time / cnt; - } - curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / M_2PI; - if (curr_test_phase > 360.0f) { - curr_test_phase = curr_test_phase - 360.0f; - } - curr_test_freq = tgt_freq; - cycle_complete = true; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed"); - return; - } - - // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought - if (!is_positive(prev_target) && is_positive(tgt_rate) && !new_target && now > new_tgt_time_ms) { - new_target = true; - new_tgt_time_ms = now + half_cycle_time_ms; - // reset max_target - max_target = 0.0f; - max_target_cnt++; - temp_min_target = min_target; - if (min_target_cnt > 0) { - sweep_tgt.max_time_m1 = temp_max_tgt_time; - temp_max_tgt_time = max_tgt_time; - sweep_tgt.count_m1 = min_target_cnt - 1; - sweep_tgt.amplitude_m1 = temp_tgt_ampl; - temp_tgt_ampl = temp_max_target - temp_min_target; - push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time); - } - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); - - } else if (is_positive(prev_target) && !is_positive(tgt_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { - new_target = false; - new_tgt_time_ms = now + half_cycle_time_ms; - min_target_cnt++; - temp_max_target = max_target; - min_target = 0.0f; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); - } - - // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought - if (!is_positive(prev_meas) && is_positive(meas_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { - new_meas = true; - new_meas_time_ms = now + half_cycle_time_ms; - // reset max_meas - max_meas = 0.0f; - max_meas_cnt++; - temp_min_meas = min_meas; - if (min_meas_cnt > 0 && min_target_cnt > 0) { - sweep_meas.max_time_m1 = temp_max_meas_time; - temp_max_meas_time = max_meas_time; - sweep_meas.count_m1 = min_meas_cnt - 1; - sweep_meas.amplitude_m1 = temp_meas_ampl; - temp_meas_ampl = temp_max_meas - temp_min_meas; - push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time); - - if (excitation == SWEEP) { - float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1); - if (!is_zero(tgt_period)) { - curr_test_freq = M_2PI / tgt_period; - } else { - curr_test_freq = 0.0f; - } - if (!is_zero(sweep_tgt.amplitude_m1)) { - curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1; - } else { - curr_test_gain = 0.0f; - } - curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / M_2PI; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f sweepgain=%f", (double)(curr_test_freq), (double)(curr_test_gain)); - cycle_complete = true; - } - } - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); - } else if (is_positive(prev_meas) && !is_positive(meas_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { - new_meas = false; - new_meas_time_ms = now + half_cycle_time_ms; - min_meas_cnt++; - temp_max_meas = max_meas; - min_meas = 0.0f; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); - } - - if (new_target) { - if (tgt_rate > max_target) { - max_target = tgt_rate; - max_tgt_time = now; - } - } else { - if (tgt_rate < min_target) { - min_target = tgt_rate; - } - } - - if (new_meas) { - if (meas_rate > max_meas) { - max_meas = meas_rate; - max_meas_time = now; - } - } else { - if (meas_rate < min_meas) { - min_meas = meas_rate; - } - } - - prev_target = tgt_rate; - prev_meas = meas_rate; -} - // update_angle - this function receives time history data during a dwell and frequency sweep tests for angle_p tuning // and determines the gain, phase, and max acceleration of the response to the input. For dwell tests once the designated number // of cycles are complete, the average of the gain, phase, and max acceleration are determined over the last 5 cycles and the // cycles_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and cycle_complete flag is set // to indicate when to pull the phase and gain data. The flag is reset to enable the next cycle to be analyzed. -void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq) +void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp, float tgt_freq) { uint32_t now = AP_HAL::millis(); @@ -229,14 +54,21 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me if (input_start_time_ms == 0) { input_start_time_ms = now; - prev_tgt_angle = tgt_angle; - prev_meas_angle = meas_angle; - prev_target = 0.0f; - prev_meas = 0.0f; + if (response == ANGLE) { + prev_tgt_resp = tgt_resp; + prev_meas_resp = meas_resp; + prev_target = 0.0f; + prev_meas = 0.0f; + } } - target_rate = (tgt_angle - prev_tgt_angle) / dt; - measured_rate = (meas_angle - prev_meas_angle) / dt; + if (response == ANGLE) { + target_rate = (tgt_resp - prev_tgt_resp) / dt; + measured_rate = (meas_resp - prev_meas_resp) / dt; + } else { + target_rate = tgt_resp; + measured_rate = meas_resp; + } // cycles are complete! determine gain and phase and exit if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) { @@ -287,14 +119,20 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me if (curr_test_phase > 360.0f) { curr_test_phase = curr_test_phase - 360.0f; } - float dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f; - if (!is_zero(max_command)) { - // normalize accel for input size - dwell_max_accel = dwell_max_accel / (2.0f * max_command); - } - if (dwell_max_accel > max_accel) { - max_accel = dwell_max_accel; + + // determine max accel for angle response type test + float dwell_max_accel; + if (response == ANGLE) { + dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f; + if (!is_zero(max_command)) { + // normalize accel for input size + dwell_max_accel = dwell_max_accel / (2.0f * max_command); + } + if (dwell_max_accel > max_accel) { + max_accel = dwell_max_accel; + } } + curr_test_freq = tgt_freq; cycle_complete = true; // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed"); @@ -302,7 +140,9 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me } // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought - if (is_positive(prev_target) && !is_positive(target_rate) && !new_target && now > new_tgt_time_ms) { + if (((response == ANGLE && is_positive(prev_target) && !is_positive(target_rate)) + || (response == RATE && !is_positive(prev_target) && is_positive(target_rate))) + && !new_target && now > new_tgt_time_ms) { new_target = true; new_tgt_time_ms = now + half_cycle_time_ms; // reset max_target @@ -317,19 +157,21 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me temp_tgt_ampl = temp_max_target - temp_min_target; push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time); } - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_tgt_cnt=%f", (double)(max_target_cnt)); - } else if (!is_positive(prev_target) && is_positive(target_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { + } else if (((response == ANGLE && !is_positive(prev_target) && is_positive(target_rate)) + || (response == RATE && is_positive(prev_target) && !is_positive(target_rate))) + && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { new_target = false; new_tgt_time_ms = now + half_cycle_time_ms; min_target_cnt++; temp_max_target = max_target; min_target = 0.0f; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); } // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought - if (is_positive(prev_meas) && !is_positive(measured_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { + if (((response == ANGLE && is_positive(prev_meas) && !is_positive(measured_rate)) + || (response == RATE && !is_positive(prev_meas) && is_positive(measured_rate))) + && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { new_meas = true; new_meas_time_ms = now + half_cycle_time_ms; // reset max_meas @@ -357,55 +199,56 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me curr_test_gain = 0.0f; } curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / M_2PI; - //gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase)); cycle_complete = true; } } - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_meas_cnt=%f", (double)(max_meas_cnt)); - } else if (!is_positive(prev_meas) && is_positive(measured_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { + } else if (((response == ANGLE && !is_positive(prev_meas) && is_positive(measured_rate)) + || (response == RATE && is_positive(prev_meas) && !is_positive(measured_rate))) + && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { new_meas = false; new_meas_time_ms = now + half_cycle_time_ms; min_meas_cnt++; temp_max_meas = max_meas; min_meas = 0.0f; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); } if (new_target) { - if (tgt_angle > max_target) { - max_target = tgt_angle; + if (tgt_resp > max_target) { + max_target = tgt_resp; max_tgt_time = now; } } else { - if (tgt_angle < min_target) { - min_target = tgt_angle; + if (tgt_resp < min_target) { + min_target = tgt_resp; } } if (new_meas) { - if (meas_angle > max_meas) { - max_meas = meas_angle; + if (meas_resp > max_meas) { + max_meas = meas_resp; max_meas_time = now; } } else { - if (meas_angle < min_meas) { - min_meas = meas_angle; + if (meas_resp < min_meas) { + min_meas = meas_resp; } } - if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) { - if (measured_rate > max_meas_rate) { - max_meas_rate = measured_rate; - } - if (command > max_command) { - max_command = command; + if (response == ANGLE) { + if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) { + if (measured_rate > max_meas_rate) { + max_meas_rate = measured_rate; + } + if (command > max_command) { + max_command = command; + } } + prev_tgt_resp = tgt_resp; + prev_meas_resp = meas_resp; } prev_target = target_rate; prev_meas = measured_rate; - prev_tgt_angle = tgt_angle; - prev_meas_angle = meas_angle; } // push measured peak info to buffer diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h index 09210c41e4..5e7d8c89ee 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h @@ -22,15 +22,18 @@ public: SWEEP = 1, }; + // Enumeration of type + enum ResponseType { + RATE = 0, + ANGLE = 1, + }; + // Initialize the Frequency Response Object. // Must be called before running dwell or frequency sweep tests - void init(InputType input_type); - - // Determines the gain and phase based on rate response for a dwell or sweep - void update_rate(float tgt_rate, float meas_rate, float tgt_freq); + void init(InputType input_type, ResponseType response_type); // Determines the gain and phase based on angle response for a dwell or sweep - void update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq); + void update(float command, float tgt_resp, float meas_resp, float tgt_freq); // Enable external query if cycle is complete and freq response data are available bool is_cycle_complete() { return cycle_complete;} @@ -78,8 +81,8 @@ private: // maximum target value from previous cycle float prev_target; - // maximum target angle from previous cycle - float prev_tgt_angle; + // maximum target response from previous cycle + float prev_tgt_resp; // holds target amplitude for gain calculation float temp_tgt_ampl; @@ -117,8 +120,8 @@ private: // maximum measured value from previous cycle float prev_meas; - // maximum measured angle from previous cycle - float prev_meas_angle; + // maximum measured response from previous cycle + float prev_meas_resp; // holds measured amplitude for gain calculation float temp_meas_ampl; @@ -152,6 +155,9 @@ private: // Input type for frequency response object InputType excitation; + // Response type for frequency response object + ResponseType response; + // sweep_peak_finding_data tracks the peak data struct sweep_peak_finding_data { uint16_t count_m1; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 01595a3483..386ac61be8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -155,11 +155,11 @@ void AC_AutoTune_Heli::test_init() } if (!is_equal(start_freq,stop_freq)) { // initialize determine_gain function whenever test is initialized - freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP); + freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, stop_freq); } else { // initialize determine_gain function whenever test is initialized - freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL); + freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, start_freq); } if (!is_zero(start_freq)) { @@ -185,11 +185,11 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine gain function - freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP); + freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); angle_dwell_test_init(start_freq, stop_freq); } else { // initialize determine gain function - freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL); + freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); angle_dwell_test_init(start_freq, start_freq); } @@ -1082,9 +1082,9 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, // wait for dwell to start before determining gain and phase or just start if sweep if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { if (freq_resp_input == 1) { - freqresp_rate.update_rate(filt_target_rate,rotation_rate, dwell_freq); + freqresp_rate.update(command_out,filt_target_rate,rotation_rate, dwell_freq); } else { - freqresp_rate.update_rate(command_out,rotation_rate, dwell_freq); + freqresp_rate.update(command_out,command_out,rotation_rate, dwell_freq); } if (freqresp_rate.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { @@ -1282,7 +1282,7 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo // wait for dwell to start before determining gain and phase if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { - freqresp_angle.update_angle(command_out, filt_target_rate, rotation_rate, dwell_freq); + freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq); if (freqresp_angle.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { curr_test_freq = freqresp_angle.get_freq();