diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp index 27a5dbc4c996ba..5c5105e1daa037 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -25,8 +25,12 @@ void AC_AutoTune_FreqResp::init(InputType input_type, ResponseType response_type max_accel = 0.0f; max_meas_rate = 0.0f; max_command = 0.0f; - meas_peak_info_buffer.clear(); - tgt_peak_info_buffer.clear(); + if (meas_peak_info_buffer != nullptr) { + meas_peak_info_buffer->clear(); + } + if (tgt_peak_info_buffer != nullptr) { + tgt_peak_info_buffer->clear(); + } cycle_complete = false; } @@ -258,14 +262,14 @@ void AC_AutoTune_FreqResp::push_to_meas_buffer(uint16_t count, float amplitude, sample.curr_count = count; sample.amplitude = amplitude; sample.time_ms = time_ms; - meas_peak_info_buffer.push(sample); + meas_peak_info_buffer->push(sample); } // pull measured peak info from buffer void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &litude, uint32_t &time_ms) { peak_info sample; - if (!meas_peak_info_buffer.pop(sample)) { + if (!meas_peak_info_buffer->pop(sample)) { // no sample return; } @@ -281,7 +285,7 @@ void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, u sample.curr_count = count; sample.amplitude = amplitude; sample.time_ms = time_ms; - tgt_peak_info_buffer.push(sample); + tgt_peak_info_buffer->push(sample); } @@ -289,7 +293,7 @@ void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, u void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &litude, uint32_t &time_ms) { peak_info sample; - if (!tgt_peak_info_buffer.pop(sample)) { + if (!tgt_peak_info_buffer->pop(sample)) { // no sample return; } @@ -297,3 +301,13 @@ void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &litud amplitude = sample.amplitude; time_ms = sample.time_ms; } + +void AC_AutoTune_FreqResp::set_dwell_cycles(uint8_t cycles) +{ + dwell_cycles = cycles; + if (meas_peak_info_buffer != nullptr) { delete meas_peak_info_buffer;} + meas_peak_info_buffer = new ObjectBuffer(cycles); + if (tgt_peak_info_buffer != nullptr) { delete tgt_peak_info_buffer;} + tgt_peak_info_buffer = new ObjectBuffer(cycles); + +} diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h index 74e50ff521fee8..6afb7b469780d5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h @@ -11,6 +11,9 @@ class AC_AutoTune_FreqResp { // Constructor AC_AutoTune_FreqResp() { + dwell_cycles = 6; + meas_peak_info_buffer = new ObjectBuffer(dwell_cycles); + tgt_peak_info_buffer = new ObjectBuffer(dwell_cycles); } // Enumeration of input type @@ -38,7 +41,7 @@ class AC_AutoTune_FreqResp { // Reset cycle_complete flag void reset_cycle_complete() { cycle_complete = false; } - void set_dwell_cycles(uint8_t cycles) { dwell_cycles = cycles; } + void set_dwell_cycles(uint8_t cycles); uint8_t get_dwell_cycles() { return dwell_cycles;} @@ -184,10 +187,10 @@ class AC_AutoTune_FreqResp { }; // Buffer object for measured peak data - ObjectBuffer meas_peak_info_buffer{6}; + ObjectBuffer *meas_peak_info_buffer; // Buffer object for target peak data - ObjectBuffer tgt_peak_info_buffer{6}; + 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);