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

AC_AutoTune: reworked updating rate p and d up

This commit is contained in:
bnsgeyer 2024-04-12 01:09:33 -04:00 committed by Bill Geyer
parent 61182c0e96
commit cd18607f25
2 changed files with 58 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -190,23 +190,22 @@ void AC_AutoTune_Heli::test_init()
case RP_UP:
case RD_UP:
// initialize start frequency
if (is_zero(start_freq)) {
if (!is_positive(next_test_freq)) {
// continue using frequency where testing left off with RD_UP completed
if (test_phase[12] > 0.0f && test_phase[12] < 180.0f && tune_type == RP_UP) {
freq_cnt = 12;
if (curr_data.phase > 150.0f && curr_data.phase < 180.0f && tune_type == RP_UP) {
start_freq = curr_data.freq;
// start with freq found for sweep where phase was 180 deg
} else if (!is_zero(sweep_tgt.ph180.freq)) {
freq_cnt = 12;
test_freq[freq_cnt] = sweep_tgt.ph180.freq - 0.25f * 3.14159f * 2.0f;
start_freq = sweep_tgt.ph180.freq - 0.25f * 3.14159f * 2.0f;
// otherwise start at min freq to step up in dwell frequency until phase > 160 deg
} else {
freq_cnt = 0;
test_freq[freq_cnt] = min_sweep_freq;
start_freq = min_sweep_freq;
}
curr_test.freq = test_freq[freq_cnt];
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
} else {
start_freq = next_test_freq;
}
stop_freq = start_freq;
attitude_control->bf_feedforward(false);
attitude_control->use_sqrt_controller(false);
@ -1000,14 +999,14 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_p_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
updating_rate_p_up(tune_roll_rp, curr_data, next_test_freq, max_rate_p);
break;
case PITCH:
updating_rate_p_up(tune_pitch_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
updating_rate_p_up(tune_pitch_rp, curr_data, next_test_freq, max_rate_p);
break;
case YAW:
case YAW_D:
updating_rate_p_up(tune_yaw_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
updating_rate_p_up(tune_yaw_rp, curr_data, next_test_freq, max_rate_p);
break;
}
}
@ -1017,14 +1016,14 @@ void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_d_up(tune_roll_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
updating_rate_d_up(tune_roll_rd, curr_data, next_test_freq, max_rate_d);
break;
case PITCH:
updating_rate_d_up(tune_pitch_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
updating_rate_d_up(tune_pitch_rd, curr_data, next_test_freq, max_rate_d);
break;
case YAW:
case YAW_D:
updating_rate_d_up(tune_yaw_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
updating_rate_d_up(tune_yaw_rd, curr_data, next_test_freq, max_rate_d);
break;
}
}
@ -1173,108 +1172,66 @@ 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
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, 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 * 3.14159f * 2.0f;
next_freq = test_data.freq;
if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) {
if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) {
rp_prev_good_frq_cnt = frq_cnt;
} else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) {
if (phase[frq_cnt] - 360.0f < 180.0f) {
rp_prev_good_frq_cnt = frq_cnt;
}
} else if (frq_cnt > 1 && phase[frq_cnt] > 200.0f && !is_zero(phase[frq_cnt])) {
frq_cnt = 11;
}
frq_cnt++;
if (frq_cnt == 12) {
freq[frq_cnt] = freq[rp_prev_good_frq_cnt];
curr_test.freq = freq[frq_cnt];
} else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test.freq = freq[frq_cnt];
}
} else if (is_equal(start_freq,stop_freq)) {
if (phase[frq_cnt] > 180.0f) {
curr_test.freq = curr_test.freq - 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] < 160.0f) {
curr_test.freq = curr_test.freq + 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) {
if (gain[frq_cnt] < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) {
tune_p += 0.05f * max_gain_p.max_allowed;
} else {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test.freq and frq_cnt for next test
curr_test.freq = freq[0];
frq_cnt = 0;
tune_p -= 0.05f * max_gain_p.max_allowed;
tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed);
}
}
if (is_zero(test_data.phase)) {
// bad test point. increase slightly in hope of getting better result
next_freq += 0.5f * test_freq_incr;
return;
}
if (counter == AUTOTUNE_SUCCESS_COUNT) {
start_freq = 0.0f; //initializes next test that uses dwell test
} else {
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
if (test_data.phase < 150.0f) {
next_freq += test_freq_incr;
} else if (test_data.phase >= 150.0f && test_data.phase < 160.0f) {
next_freq += 0.5f * test_freq_incr;
} else if (test_data.phase >= 160.0f && test_data.phase < 180.0f) {
if (test_data.gain < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) {
tune_p += 0.05f * max_gain_p.max_allowed;
} else {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset next_freq for next test
next_freq = 0.0f;
tune_p -= 0.05f * max_gain_p.max_allowed;
tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed);
}
} else if (test_data.phase >= 180.0f) {
next_freq -= 0.5f * test_freq_incr;
}
}
// 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
void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, 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 * 3.14159f * 2.0f; // set for 1/4 hz increments
next_freq = test_data.freq;
// if sweep failed to find frequency for 180 phase then use dwell to find frequency
if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) {
if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) {
rd_prev_good_frq_cnt = frq_cnt;
} else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) {
if (phase[frq_cnt] - 360.0f < 180.0f) {
rd_prev_good_frq_cnt = frq_cnt;
}
} else if (frq_cnt > 1 && phase[frq_cnt] > 200.0f && !is_zero(phase[frq_cnt])) {
frq_cnt = 11;
}
frq_cnt++;
if (frq_cnt == 12) {
freq[frq_cnt] = freq[rd_prev_good_frq_cnt];
curr_test.freq = freq[frq_cnt];
} else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test.freq = freq[frq_cnt];
}
} else if (is_equal(start_freq,stop_freq)) {
if (phase[frq_cnt] > 180.0f) {
curr_test.freq = curr_test.freq - 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] < 160.0f) {
curr_test.freq = curr_test.freq + 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) {
if ((gain[frq_cnt] < rd_prev_gain || is_zero(rd_prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) {
if (is_zero(test_data.phase)) {
// bad test point. increase slightly in hope of getting better result
next_freq += 0.5f * test_freq_incr;
return;
}
if (test_data.phase < 150.0f) {
next_freq += test_freq_incr;
} else if (test_data.phase >= 150.0f && test_data.phase < 160.0f) {
next_freq += 0.5f * test_freq_incr;
} else if (test_data.phase >= 160.0f && test_data.phase < 180.0f) {
if ((test_data.gain < rd_prev_gain || is_zero(rd_prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) {
tune_d += 0.05f * max_gain_d.max_allowed;
rd_prev_gain = gain[frq_cnt];
rd_prev_gain = test_data.gain;
} else {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test.freq and frq_cnt for next test
curr_test.freq = freq[0];
frq_cnt = 0;
// reset next freq and rd_prev_gain for next test
next_freq = 0;
rd_prev_gain = 0.0f;
tune_d -= 0.05f * max_gain_d.max_allowed;
tune_d = constrain_float(tune_d,0.0f,0.6f * max_gain_d.max_allowed);
}
}
}
if (counter == AUTOTUNE_SUCCESS_COUNT) {
start_freq = 0.0f; //initializes next test that uses dwell test
} else {
start_freq = curr_test.freq;
stop_freq = curr_test.freq;
} else if (test_data.phase >= 180.0f) {
next_freq -= 0.5f * test_freq_incr;
}
}

4
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -186,10 +186,10 @@ private:
void updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq);
// 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
void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p);
void updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p);
// 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
void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, 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
void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);