Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: reworked updating angle p up

Browse Source
This commit is contained in:
bnsgeyer 2024-04-12 22:26:56 -04:00 committed by Bill Geyer
parent cd18607f25
commit 989f48f97a
2 changed files with 102 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

182
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -217,19 +217,15 @@ void AC_AutoTune_Heli::test_init()
num_dwell_cycles = 6; num_dwell_cycles = 6;
break; break;
case SP_UP: case SP_UP:
// initialize start frequency // initialize start frequency for sweep
if (is_zero(start_freq)) { if (!is_positive(next_test_freq)) {
if (!is_zero(sweep_tgt.maxgain.freq)) { start_freq = min_sweep_freq;
freq_cnt = 12; stop_freq = max_sweep_freq;
test_freq[freq_cnt] = sweep_tgt.maxgain.freq - 0.25f * 3.14159f * 2.0f; sweep_complete = true;
curr_test.freq = test_freq[freq_cnt]; } else {
start_freq = curr_test.freq; start_freq = next_test_freq;
stop_freq = curr_test.freq; stop_freq = start_freq;
test_accel_max = 0.0f; test_accel_max = 0.0f;
} else {
start_freq = min_sweep_freq;
stop_freq = max_sweep_freq;
}
} }
attitude_control->bf_feedforward(false); attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false); attitude_control->use_sqrt_controller(false);
@ -243,10 +239,10 @@ void AC_AutoTune_Heli::test_init()
break; break;
case TUNE_CHECK: case TUNE_CHECK:
// initialize start frequency // initialize start frequency
if (is_zero(start_freq)) { start_freq = min_sweep_freq;
start_freq = min_sweep_freq; stop_freq = max_sweep_freq;
stop_freq = max_sweep_freq; test_accel_max = 0.0f;
}
// variables needed to initialize frequency response object and test method // variables needed to initialize frequency response object and test method
resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE; resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE;
calc_type = ANGLE; calc_type = ANGLE;
@ -1054,16 +1050,30 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
{ {
attitude_control->bf_feedforward(orig_bf_feedforward); attitude_control->bf_feedforward(orig_bf_feedforward);
// sweep doesn't require gain update so return immediately after setting next test freq
if (input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
// determine next_test_freq for dwell testing
if (sweep_complete){
// if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
if (!is_zero(sweep_tgt.maxgain.freq)) {
next_test_freq = sweep_tgt.maxgain.freq - 0.25f * 3.14159f * 2.0f;
} else {
next_test_freq = min_sweep_freq;
}
}
return;
}
switch (test_axis) { switch (test_axis) {
case ROLL: case ROLL:
updating_angle_p_up(tune_roll_sp, test_freq, test_gain, test_phase, freq_cnt); updating_angle_p_up(tune_roll_sp, curr_data, next_test_freq);
break; break;
case PITCH: case PITCH:
updating_angle_p_up(tune_pitch_sp, test_freq, test_gain, test_phase, freq_cnt); updating_angle_p_up(tune_pitch_sp, curr_data, next_test_freq);
break; break;
case YAW: case YAW:
case YAW_D: case YAW_D:
updating_angle_p_up(tune_yaw_sp, test_freq, test_gain, test_phase, freq_cnt); updating_angle_p_up(tune_yaw_sp, curr_data, next_test_freq);
break; break;
} }
} }
@ -1236,99 +1246,93 @@ 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
void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt) void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq)
{ {
float test_freq_incr = 0.5f * 3.14159f * 2.0f; float test_freq_incr = 0.5f * 3.14159f * 2.0f;
float gain_incr = 0.5f; float gain_incr = 0.5f;
if (!is_equal(start_freq,stop_freq)) { if (is_zero(test_data.phase)) {
if (!is_zero(sweep_tgt.maxgain.freq)) { // bad test point. increase slightly in hope of getting better result
frq_cnt = 12; next_freq += 0.5f * test_freq_incr;
freq[frq_cnt] = sweep_tgt.maxgain.freq - 0.5f * test_freq_incr; return;
freq_cnt_max = frq_cnt;
} else {
frq_cnt = 1;
freq[frq_cnt] = min_sweep_freq;
freq_cnt_max = 0;
}
curr_test.freq = freq[frq_cnt];
} }
if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) {
if (gain[frq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { if (!found_max_gain_freq) {
if (test_data.gain > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) {
// exceeded max response gain already, reduce tuning gain to remain under max response gain // exceeded max response gain already, reduce tuning gain to remain under max response gain
tune_p -= gain_incr; tune_p -= gain_incr;
// force counter to stay on frequency // force counter to stay on frequency
frq_cnt -= 1; next_freq = test_data.freq;
} else if (gain[frq_cnt] > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) { return;
} else if (test_data.gain > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) {
// exceeded max response gain at the minimum allowable tuning gain. terminate testing. // exceeded max response gain at the minimum allowable tuning gain. terminate testing.
tune_p = AUTOTUNE_SP_MIN; tune_p = AUTOTUNE_SP_MIN;
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
} else if (gain[frq_cnt] > gain[freq_cnt_max]) { } else if (test_data.gain > sp_prev_gain) {
freq_cnt_max = frq_cnt; freq_max = test_data.freq;
phase_max = phase[frq_cnt]; phase_max = test_data.phase;
sp_prev_gain = gain[frq_cnt]; sp_prev_gain = test_data.gain;
} else if (freq[frq_cnt] > max_sweep_freq || (gain[frq_cnt] > 0.0f && gain[frq_cnt] < 0.5f)) { next_freq = test_data.freq + test_freq_incr;
// must be past peak, continue on to determine angle p return;
frq_cnt = 11; // Gain is expected to continue decreasing past gain peak. declare max gain freq found and refine search.
} } else if (test_data.gain < 0.9f * sp_prev_gain) {
frq_cnt++; found_max_gain_freq = true;
if (frq_cnt == 12) { next_freq = freq_max + 0.5 * test_freq_incr;
freq[frq_cnt] = freq[freq_cnt_max]; return;
curr_test.freq = freq[frq_cnt];
} else { } else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; next_freq = test_data.freq + test_freq_incr;
curr_test.freq = freq[frq_cnt]; return;
} }
} }
// once finished with sweep of frequencies, cnt = 12 is used to then tune for max response gain // refine peak
if (frq_cnt >= 12 && is_equal(start_freq,stop_freq)) { if (!found_peak) {
if (gain[frq_cnt] < max_resp_gain && tune_p < AUTOTUNE_SP_MAX && !find_peak) { // look at frequency above max gain freq found
// keep increasing tuning gain unless phase changes or max response gain is achieved if (test_data.freq > freq_max && test_data.gain > sp_prev_gain) {
if (phase[frq_cnt]-phase_max > 20.0f && phase[frq_cnt] < 210.0f) { // found max at frequency greater than initial max gain frequency
freq[frq_cnt] += 0.5 * test_freq_incr; found_peak = true;
find_peak = true; } else if (test_data.freq > freq_max && test_data.gain < sp_prev_gain) {
} else { // look at frequency below initial max gain frequency
tune_p += gain_incr; next_freq = test_data.freq - 0.5 * test_freq_incr;
freq[frq_cnt] = freq[freq_cnt_max]; return;
if (tune_p >= AUTOTUNE_SP_MAX) { } else if (test_data.freq < freq_max && test_data.gain > sp_prev_gain) {
tune_p = AUTOTUNE_SP_MAX; // found max at frequency less than initial max gain frequency
counter = AUTOTUNE_SUCCESS_COUNT; found_peak = true;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } else {
} found_peak = true;
test_data.freq = freq_max;
test_data.gain = sp_prev_gain;
}
sp_prev_gain = test_data.gain;
}
// start increasing gain
if (found_max_gain_freq && found_peak) {
if (test_data.gain < max_resp_gain && tune_p < AUTOTUNE_SP_MAX) {
tune_p += gain_incr;
next_freq = test_data.freq;
if (tune_p >= AUTOTUNE_SP_MAX) {
tune_p = AUTOTUNE_SP_MAX;
counter = AUTOTUNE_SUCCESS_COUNT;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
} }
curr_test.freq = freq[frq_cnt]; sp_prev_gain = test_data.gain;
sp_prev_gain = gain[frq_cnt]; } else if (test_data.gain > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN) {
} else if (gain[frq_cnt] > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN && !find_peak) {
tune_p -= gain_incr; tune_p -= gain_incr;
} else if (find_peak) {
// find the frequency where the response gain is maximum
if (gain[frq_cnt] > sp_prev_gain) {
freq[frq_cnt] += 0.5 * test_freq_incr;
} else {
find_peak = false;
phase_max = phase[frq_cnt];
}
curr_test.freq = freq[frq_cnt];
sp_prev_gain = gain[frq_cnt];
} else { } else {
// adjust tuning gain so max response gain is not exceeded // adjust tuning gain so max response gain is not exceeded
if (sp_prev_gain < max_resp_gain && gain[frq_cnt] > max_resp_gain) { if (sp_prev_gain < max_resp_gain && test_data.gain > max_resp_gain) {
float adj_factor = (max_resp_gain - gain[frq_cnt]) / (gain[frq_cnt] - sp_prev_gain); float adj_factor = (max_resp_gain - test_data.gain) / (test_data.gain - sp_prev_gain);
tune_p = tune_p + gain_incr * adj_factor; tune_p = tune_p + gain_incr * adj_factor;
} }
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
} }
} }
if (counter == AUTOTUNE_SUCCESS_COUNT) { if (counter == AUTOTUNE_SUCCESS_COUNT) {
start_freq = 0.0f; //initializes next test that uses dwell test next_freq = 0.0f; //initializes next test that uses dwell test
sweep_complete = false;
reset_sweep_variables(); reset_sweep_variables();
curr_test.freq = freq[0];
frq_cnt = 0;
} else {
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
} }
} }
@ -1602,6 +1606,8 @@ void AC_AutoTune_Heli::reset_vehicle_test_variables()
freq_cnt = 0; freq_cnt = 0;
start_freq = 0.0f; start_freq = 0.0f;
stop_freq = 0.0f; stop_freq = 0.0f;
next_test_freq = 0.0f;
sweep_complete = false;
} }
} }
@ -1620,8 +1626,10 @@ void AC_AutoTune_Heli::reset_update_gain_variables()
// reset sp_up variables // reset sp_up variables
phase_max = 0.0f; phase_max = 0.0f;
freq_max = 0.0f;
sp_prev_gain = 0.0f; sp_prev_gain = 0.0f;
find_peak = false; found_max_gain_freq = false;
found_peak = false;
} }

10
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -192,7 +192,7 @@ private:
void updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d); void updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d);
// 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
void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt); void updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq);
// 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
void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d); void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
@ -234,12 +234,15 @@ private:
bool find_middle; bool find_middle;
// updating angle P up variables // updating angle P up variables
// track the maximum phase // track the maximum phase and freq
float phase_max; float phase_max;
float freq_max;
// previous gain // previous gain
float sp_prev_gain; float sp_prev_gain;
// flag for finding max gain frequency
bool found_max_gain_freq;
// flag for finding the peak of the gain response // flag for finding the peak of the gain response
bool find_peak; bool found_peak;
// updating rate P up // updating rate P up
// counter value of previous good frequency // counter value of previous good frequency
@ -299,6 +302,7 @@ private:
}; };
sweep_data sweep_mtr; sweep_data sweep_mtr;
sweep_data sweep_tgt; sweep_data sweep_tgt;
bool sweep_complete;
// fix the frequency sweep time to 23 seconds // fix the frequency sweep time to 23 seconds
const float sweep_time_ms = 23000; const float sweep_time_ms = 23000;