Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: more suggested changes

This commit is contained in:
bnsgeyer 2024-06-16 23:28:31 -04:00 committed by Bill Geyer
parent 088b7ec094
commit f50bb54112
2 changed files with 21 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
View File

@ -262,14 +262,16 @@ 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;
if (meas_peak_info_buffer != nullptr) {
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 == nullptr) || !meas_peak_info_buffer->pop(sample)) {
// no sample // no sample
return; return;
} }
@ -285,15 +287,16 @@ 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;
if (tgt_peak_info_buffer != nullptr) {
tgt_peak_info_buffer->push(sample); tgt_peak_info_buffer->push(sample);
}
} }
// pull target peak info from buffer // pull target peak info from buffer
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 == nullptr) || !tgt_peak_info_buffer->pop(sample)) {
// no sample // no sample
return; return;
} }

17
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -1241,7 +1241,9 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data
} }
} }
// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not
// exceed max response gain. A phase of 161 deg is used to conduct the tuning as this phase is where analytically
// max gain to 6db gain margin is determined for a unity feedback controller.
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p) void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p)
{ {
float test_freq_incr = 0.25f * M_2PI; float test_freq_incr = 0.25f * M_2PI;
@ -1265,7 +1267,9 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data,
} }
} }
// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response
// gain is at a minimum. A phase of 161 deg is used to conduct the tuning as this phase is where analytically
// max gain to 6db gain margin is determined for a unity feedback controller.
void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d) void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d)
{ {
float test_freq_incr = 0.25f * M_2PI; // set for 1/4 hz increments float test_freq_incr = 0.25f * M_2PI; // set for 1/4 hz increments
@ -1291,7 +1295,8 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data,
} }
} }
// updating_angle_p_up - determines maximum angle p gain for pitch and roll // updating_angle_p_up - determines maximum angle p gain for pitch and roll. This is accomplished by determining the frequency
// for the maximum response gain that is the disturbance rejection peak.
void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq) void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq)
{ {
float test_freq_incr = 0.5f * M_2PI; float test_freq_incr = 0.5f * M_2PI;
@ -1382,7 +1387,11 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data,
} }
} }
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur. This uses the frequency
// response of motor class input to rate response to determine the max allowable gain for rate P gain. A phase of 161 deg is used to
// determine analytically the max gain to 6db gain margin for a unity feedback controller. Since acceleration can be more noisy, the
// response of the motor class input to rate response to determine the max allowable gain for rate D gain. A phase of 251 deg is used
// to determine analytically the max gain to 6db gain margin for a unity feedback controller.
void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d) void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
{ {
float test_freq_incr = 0.5f * M_2PI; float test_freq_incr = 0.5f * M_2PI;