diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp index af11ad2665..108c30c219 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -65,7 +65,7 @@ void AC_AutoTune_FreqResp::update_rate(float tgt_rate, float meas_rate, float tg float tgt_ampl = 0.0f; uint32_t meas_time = 0; uint32_t tgt_time = 0; - for (int i = 0; i < AUTOTUNE_DWELL_CYCLES; i++) { + 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); @@ -245,7 +245,7 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me float tgt_ampl = 0.0f; uint32_t meas_time = 0; uint32_t tgt_time = 0; - for (int i = 0; i < AUTOTUNE_DWELL_CYCLES; i++) { + 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); diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h index a124590613..3ece343297 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h @@ -19,6 +19,7 @@ public: tgt_peak_info_buffer = new ObjectBuffer(AUTOTUNE_DWELL_CYCLES); } + // Enumeration of input type enum InputType { DWELL = 0, SWEEP = 1, @@ -28,37 +29,130 @@ public: // 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 + // 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 based on angle response for a dwell or sweep + // 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 freq response 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 + // Reset cycle_complete flag void reset_cycle_complete() { cycle_complete = false; } - // frequency response data 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; } float get_accel_max() { return max_accel; } private: - float max_target, max_meas, prev_target, prev_meas, prev_tgt_angle, prev_meas_angle; - float min_target, min_meas, temp_meas_ampl, temp_tgt_ampl; - float temp_max_target, temp_min_target, target_rate, measured_rate, max_meas_rate, max_command; - float temp_max_meas, temp_min_meas; - uint32_t temp_max_tgt_time, temp_max_meas_time; - uint32_t max_tgt_time, max_meas_time, new_tgt_time_ms, new_meas_time_ms, input_start_time_ms; - uint16_t min_target_cnt, max_target_cnt, max_meas_cnt, min_meas_cnt; + // time of the start of a new target value search. keeps noise from prematurely starting the search of a new target value. + uint32_t new_tgt_time_ms; + + // flag for searching for a new target peak bool new_target = false; + + // maximum target value + float max_target; + + // time of maximum target value in current cycle + uint32_t max_tgt_time; + + // counter for target value maximums + uint16_t max_target_cnt; + + // holds previously determined maximum target value while current cycle is running + float temp_max_target; + + // holds previously determined time of maximum target value while current cycle is running + uint32_t temp_max_tgt_time; + + // minimum target value + float min_target; + + // counter for target value minimums + uint16_t min_target_cnt; + + // holds previously determined minimum target value while current cycle is running + float temp_min_target; + + // maximum target value from previous cycle + float prev_target; + + // maximum target angle from previous cycle + float prev_tgt_angle; + + // holds target amplitude for gain calculation + float temp_tgt_ampl; + + // time of the start of a new measured value search. keeps noise from prematurely starting the search of a new measured value. + uint32_t new_meas_time_ms; + + // flag for searching for a new measured peak bool new_meas = false; + + // maximum measured value + float max_meas; + + // time of maximum measured value in current cycle + uint32_t max_meas_time; + + // counter for measured value maximums + uint16_t max_meas_cnt; + + // holds previously determined maximum measured value while current cycle is running + float temp_max_meas; + + // holds previously determined time of maximum measured value while current cycle is running + uint32_t temp_max_meas_time; + + // minimum measured value + float min_meas; + + // counter for measured value minimums + uint16_t min_meas_cnt; + + // holds previously determined minimum measured value while current cycle is running + float temp_min_meas; + + // maximum measured value from previous cycle + float prev_meas; + + // maximum measured angle from previous cycle + float prev_meas_angle; + + // holds measured amplitude for gain calculation + float temp_meas_ampl; + + // calculated target rate from angle data + float target_rate; + + // calculated measured rate from angle data + float measured_rate; + + // holds start time of input to track length of time that input in running + uint32_t input_start_time_ms; + + // flag indicating when one oscillation cycle is complete bool cycle_complete = false; - float curr_test_freq, curr_test_gain, curr_test_phase; + + // current test frequency, gain, and phase + float curr_test_freq; + float curr_test_gain; + float curr_test_phase; + + // maximum measured rate throughout excitation used for max accel calculation + float max_meas_rate; + + // maximum command associated with maximum rate used for max accel calculation + float max_command; + + // maximum acceleration in cdss determined during test float max_accel; + + // Input type for frequency response object InputType excitation; // sweep_peak_finding_data tracks the peak data @@ -67,7 +161,11 @@ private: float amplitude_m1; float max_time_m1; }; + + // Measured data for sweep peak sweep_peak_finding_data sweep_meas; + + // Target data for sweep peak sweep_peak_finding_data sweep_tgt; //store gain data in ring buffer @@ -77,11 +175,23 @@ private: uint32_t time_ms; }; + + // Buffer object for measured peak data ObjectBuffer *meas_peak_info_buffer; + + // Buffer object for target peak data ObjectBuffer *tgt_peak_info_buffer; + + // Push data into measured peak data buffer object void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms); + + // Pull data from measured peak data buffer object void pull_from_meas_buffer(uint16_t &count, float &litude, uint32_t &time_ms); + + // Push data into target peak data buffer object void push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms); + + // Pull data from target peak data buffer object void pull_from_tgt_buffer(uint16_t &count, float &litude, uint32_t &time_ms); };