diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 88ed6d5fc9..909e0c4899 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -1823,9 +1823,9 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float // 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.determine_gain(filt_target_rate,rotation_rate, dwell_freq); + freqresp_rate.update_rate(filt_target_rate,rotation_rate, dwell_freq); } else { - freqresp_rate.determine_gain(command_out,rotation_rate, dwell_freq); + freqresp_rate.update_rate(command_out,rotation_rate, dwell_freq); } if (freqresp_rate.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { @@ -2024,7 +2024,7 @@ void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &d // 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.determine_gain_angle(command_out, filt_target_rate, rotation_rate, dwell_freq); + freqresp_angle.update_angle(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(); diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp index 2d3bf159d9..da1caf15ec 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -7,13 +7,7 @@ is set. This function must be reset using the reset flag prior to the next dwel #include #include "AC_AutoTune_FreqResp.h" -float AC_AutoTune_FreqResp::update() -{ - float dummy = 0.0f; - return dummy; -} - -// Initialize the Frequency Response methods. Must be called before running dwell or frequency sweep tests +// Initialize the Frequency Response Object. Must be called before running dwell or frequency sweep tests void AC_AutoTune_FreqResp::init(InputType input_type) { excitation = input_type; @@ -43,7 +37,7 @@ void AC_AutoTune_FreqResp::init(InputType input_type) // 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::determine_gain(float tgt_rate, float meas_rate, float tgt_freq) +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; @@ -209,12 +203,12 @@ void AC_AutoTune_FreqResp::determine_gain(float tgt_rate, float meas_rate, float prev_meas = meas_rate; } -// determine_gain_angle - this function receives time history data during a dwell and frequency sweep tests for angle_p tuning +// 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::determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq) +void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq) { uint32_t now = AP_HAL::millis(); diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h index f75743256b..a124590613 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h @@ -19,30 +19,28 @@ public: tgt_peak_info_buffer = new ObjectBuffer(AUTOTUNE_DWELL_CYCLES); } -// CLASS_NO_COPY(AC_PI); - - // update calculations - float update(); - enum InputType { DWELL = 0, SWEEP = 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 for a dwell - void determine_gain(float tgt_rate, float meas_rate, float tgt_freq); + // 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); - // determines the gain and phase for a dwell - void determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq); + // 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); - // enable external query if cycle is complete and phase and gain data are available + // enable external query if cycle is complete and freq response data are available bool is_cycle_complete() { return cycle_complete;} + // reset cycle_complete flag void reset_cycle_complete() { cycle_complete = false; } - // frequency response accessors + // frequency response data accessors float get_freq() { return curr_test_freq; } float get_gain() { return curr_test_gain; } float get_phase() { return curr_test_phase; } @@ -72,7 +70,7 @@ private: sweep_peak_finding_data sweep_meas; sweep_peak_finding_data sweep_tgt; - //store determine gain data in ring buffer + //store gain data in ring buffer struct peak_info { uint16_t curr_count; float amplitude;