mirror of https://github.com/ArduPilot/ardupilot
AC_AutoTune: reworked updating rate p and d up
This commit is contained in:
parent
61182c0e96
commit
cd18607f25
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue