Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: fix object buffers to dynamically size

Browse Source
This commit is contained in:
bnsgeyer 2024-04-08 23:28:10 -04:00 committed by Bill Geyer
parent 528b7eb438
commit 3943a69b33
2 changed files with 26 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
View File

@ -25,8 +25,12 @@ void AC_AutoTune_FreqResp::init(InputType input_type, ResponseType response_type
max_accel = 0.0f; max_accel = 0.0f;
max_meas_rate = 0.0f; max_meas_rate = 0.0f;
max_command = 0.0f; max_command = 0.0f;
meas_peak_info_buffer.clear(); if (meas_peak_info_buffer != nullptr) {
tgt_peak_info_buffer.clear(); meas_peak_info_buffer->clear();
}
if (tgt_peak_info_buffer != nullptr) {
tgt_peak_info_buffer->clear();
}
cycle_complete = false; 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.curr_count = count;
sample.amplitude = amplitude; sample.amplitude = amplitude;
sample.time_ms = time_ms; sample.time_ms = time_ms;
meas_peak_info_buffer.push(sample); meas_peak_info_buffer->push(sample);
} }
// pull measured peak info from buffer // pull measured peak info from buffer
void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms) void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
{ {
peak_info sample; peak_info sample;
if (!meas_peak_info_buffer.pop(sample)) { if (!meas_peak_info_buffer->pop(sample)) {
// no sample // no sample
return; return;
} }
@ -281,7 +285,7 @@ void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, u
sample.curr_count = count; sample.curr_count = count;
sample.amplitude = amplitude; sample.amplitude = amplitude;
sample.time_ms = time_ms; 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 &amplitude, uint32_t &time_ms) void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
{ {
peak_info sample; peak_info sample;
if (!tgt_peak_info_buffer.pop(sample)) { if (!tgt_peak_info_buffer->pop(sample)) {
// no sample // no sample
return; return;
} }
@ -297,3 +301,13 @@ void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitud
amplitude = sample.amplitude; amplitude = sample.amplitude;
time_ms = sample.time_ms; 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);
}

9
libraries/AC_AutoTune/AC_AutoTune_FreqResp.h
View File

@ -11,6 +11,9 @@ public:
// Constructor // Constructor
AC_AutoTune_FreqResp() 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 // Enumeration of input type
@ -38,7 +41,7 @@ public:
// Reset cycle_complete flag // Reset cycle_complete flag
void reset_cycle_complete() { cycle_complete = false; } 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;} uint8_t get_dwell_cycles() { return dwell_cycles;}
@ -184,10 +187,10 @@ private:
}; };
// Buffer object for measured peak data // 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 // 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 // Push data into measured peak data buffer object
void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms); void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms);