AC_AutoTune: fix object buffers to dynamically size

ArduPilot · Apr 9, 2024 · 28dc4bb · 28dc4bb
1 parent ef4d3c1
commit 28dc4bb
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 9 deletions.
diff --git 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 &amplitude, 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,19 +285,29 @@ 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);
 
 }
 
 // pull target peak info from buffer
 void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
 {
     peak_info sample;
-    if (!tgt_peak_info_buffer.pop(sample)) {
+    if (!tgt_peak_info_buffer->pop(sample)) {
         // no sample
         return;
     }
     count = sample.curr_count;
     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<peak_info>(cycles);
+    if (tgt_peak_info_buffer != nullptr) { delete tgt_peak_info_buffer;}
+    tgt_peak_info_buffer = new ObjectBuffer<peak_info>(cycles);
+
+}
diff --git 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<peak_info>(dwell_cycles);
+    tgt_peak_info_buffer = new ObjectBuffer<peak_info>(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<peak_info> meas_peak_info_buffer{6};
+    ObjectBuffer<peak_info> *meas_peak_info_buffer;
 
     // Buffer object for target peak data
-    ObjectBuffer<peak_info> tgt_peak_info_buffer{6};
+    ObjectBuffer<peak_info> *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);