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

AC_AutoTune: createstructure to hold specific test's sweep results

This commit is contained in:
Peter Barker 2022-02-09 10:31:51 +11:00 committed by Randy Mackay
parent 94c0701f17
commit 3686f32671
2 changed files with 126 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -124,26 +124,26 @@ void AC_AutoTune_Heli::test_init()
if (test_phase[12] > 0.0f && test_phase[12] < 180.0f) { if (test_phase[12] > 0.0f && test_phase[12] < 180.0f) {
freq_cnt = 12; freq_cnt = 12;
// start with freq found for sweep where phase was 180 deg // start with freq found for sweep where phase was 180 deg
} else if (!is_zero(sweep.ph180_freq)) { } else if (!is_zero(sweep.ph180.freq)) {
freq_cnt = 12; freq_cnt = 12;
test_freq[freq_cnt] = sweep.ph180_freq - 0.25f * 3.14159f * 2.0f; test_freq[freq_cnt] = sweep.ph180.freq - 0.25f * 3.14159f * 2.0f;
// otherwise start at min freq to step up in dwell frequency until phase > 160 deg // otherwise start at min freq to step up in dwell frequency until phase > 160 deg
} else { } else {
freq_cnt = 0; freq_cnt = 0;
test_freq[freq_cnt] = min_sweep_freq; test_freq[freq_cnt] = min_sweep_freq;
} }
curr_test_freq = test_freq[freq_cnt]; curr_test.freq = test_freq[freq_cnt];
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
// MAX_GAINS and RD_UP both start with a sweep initially but if it has been completed then start dwells at the freq for 180 deg phase // MAX_GAINS and RD_UP both start with a sweep initially but if it has been completed then start dwells at the freq for 180 deg phase
} else { } else {
if (!is_zero(sweep.ph180_freq)) { if (!is_zero(sweep.ph180.freq)) {
freq_cnt = 12; freq_cnt = 12;
test_freq[freq_cnt] = sweep.ph180_freq - 0.25f * 3.14159f * 2.0f; test_freq[freq_cnt] = sweep.ph180.freq - 0.25f * 3.14159f * 2.0f;
curr_test_freq = test_freq[freq_cnt]; curr_test.freq = test_freq[freq_cnt];
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
if (tune_type == MAX_GAINS) { if (tune_type == MAX_GAINS) {
reset_maxgains_update_gain_variables(); reset_maxgains_update_gain_variables();
} }
@ -170,12 +170,12 @@ void AC_AutoTune_Heli::test_init()
case SP_UP: case SP_UP:
// initialize start frequency // initialize start frequency
if (is_zero(start_freq)) { if (is_zero(start_freq)) {
if (!is_zero(sweep.maxgain_freq)) { if (!is_zero(sweep.maxgain.freq)) {
freq_cnt = 12; freq_cnt = 12;
test_freq[freq_cnt] = sweep.maxgain_freq - 0.25f * 3.14159f * 2.0f; test_freq[freq_cnt] = sweep.maxgain.freq - 0.25f * 3.14159f * 2.0f;
curr_test_freq = test_freq[freq_cnt]; curr_test.freq = test_freq[freq_cnt];
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
test_accel_max = 0.0f; test_accel_max = 0.0f;
} else { } else {
start_freq = min_sweep_freq; start_freq = min_sweep_freq;
@ -280,7 +280,7 @@ void AC_AutoTune_Heli::do_gcs_announcements()
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Sweep"); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Sweep");
if (settle_time == 0) { if (settle_time == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test.freq), (double)(curr_test.gain), (double)(curr_test.phase));
} }
} }
break; break;
@ -335,8 +335,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_rp)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_rp));
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain));
} }
break; break;
case RD_UP: case RD_UP:
@ -351,8 +351,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f angle_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_sp)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f angle_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_sp));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tune_accel=%f max_accel=%f", (double)(tune_accel), (double)(test_accel_max)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tune_accel=%f max_accel=%f", (double)(tune_accel), (double)(test_accel_max));
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain));
} }
break; break;
case MAX_GAINS: case MAX_GAINS:
@ -360,8 +360,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
// announce results of dwell and update // announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt])); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]));
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain));
} }
break; break;
default: default:
@ -938,8 +938,8 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq)
settle_time = 200; settle_time = 200;
if (!is_equal(start_freq, stop_freq)) { if (!is_equal(start_freq, stop_freq)) {
reset_sweep_variables(); reset_sweep_variables();
curr_test_gain = 0.0f; curr_test.gain = 0.0f;
curr_test_phase = 0.0f; curr_test.phase = 0.0f;
} }
// filter at lower frequency to remove steady state // filter at lower frequency to remove steady state
@ -1129,9 +1129,9 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
} }
if (freqresp_rate.is_cycle_complete()) { if (freqresp_rate.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) { if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_rate.get_freq(); curr_test.freq = freqresp_rate.get_freq();
curr_test_gain = freqresp_rate.get_gain(); curr_test.gain = freqresp_rate.get_gain();
curr_test_phase = freqresp_rate.get_phase(); curr_test.phase = freqresp_rate.get_phase();
// reset cycle_complete to allow indication of next cycle // reset cycle_complete to allow indication of next cycle
freqresp_rate.reset_cycle_complete(); freqresp_rate.reset_cycle_complete();
// log sweep data // log sweep data
@ -1147,25 +1147,25 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
// set sweep data if a frequency sweep is being conducted // set sweep data if a frequency sweep is being conducted
if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) { if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) {
// track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped. // track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped.
if (curr_test_phase > 180.0f && sweep.progress == 0) { if (curr_test.phase > 180.0f && sweep.progress == 0) {
sweep.progress = 1; sweep.progress = 1;
} else if (curr_test_phase > 270.0f && sweep.progress == 1) { } else if (curr_test.phase > 270.0f && sweep.progress == 1) {
sweep.progress = 2; sweep.progress = 2;
} }
if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f && sweep.progress == 0) { if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f && sweep.progress == 0) {
sweep.ph180_freq = curr_test_freq; sweep.ph180.freq = curr_test.freq;
sweep.ph180_gain = curr_test_gain; sweep.ph180.gain = curr_test.gain;
sweep.ph180_phase = curr_test_phase; sweep.ph180.phase = curr_test.phase;
} }
if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f && sweep.progress == 1) { if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f && sweep.progress == 1) {
sweep.ph270_freq = curr_test_freq; sweep.ph270.freq = curr_test.freq;
sweep.ph270_gain = curr_test_gain; sweep.ph270.gain = curr_test.gain;
sweep.ph270_phase = curr_test_phase; sweep.ph270.phase = curr_test.phase;
} }
if (curr_test_gain > sweep.maxgain_gain) { if (curr_test.gain > sweep.maxgain.gain) {
sweep.maxgain_gain = curr_test_gain; sweep.maxgain.gain = curr_test.gain;
sweep.maxgain_freq = curr_test_freq; sweep.maxgain.freq = curr_test.freq;
sweep.maxgain_phase = curr_test_phase; sweep.maxgain.phase = curr_test.phase;
} }
if (now - step_start_time_ms >= sweep_time_ms + 200) { if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time // we have passed the maximum stop time
@ -1223,8 +1223,8 @@ void AC_AutoTune_Heli::angle_dwell_test_init(float start_frq, float filt_freq)
if (!is_equal(start_freq, stop_freq)) { if (!is_equal(start_freq, stop_freq)) {
reset_sweep_variables(); reset_sweep_variables();
curr_test_gain = 0.0f; curr_test.gain = 0.0f;
curr_test_phase = 0.0f; curr_test.phase = 0.0f;
} }
} }
@ -1326,9 +1326,9 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq); freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq);
if (freqresp_angle.is_cycle_complete()) { if (freqresp_angle.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) { if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_angle.get_freq(); curr_test.freq = freqresp_angle.get_freq();
curr_test_gain = freqresp_angle.get_gain(); curr_test.gain = freqresp_angle.get_gain();
curr_test_phase = freqresp_angle.get_phase(); curr_test.phase = freqresp_angle.get_phase();
test_accel_max = freqresp_angle.get_accel_max(); test_accel_max = freqresp_angle.get_accel_max();
// reset cycle_complete to allow indication of next cycle // reset cycle_complete to allow indication of next cycle
freqresp_angle.reset_cycle_complete(); freqresp_angle.reset_cycle_complete();
@ -1344,20 +1344,20 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
// set sweep data if a frequency sweep is being conducted // set sweep data if a frequency sweep is being conducted
if (!is_equal(start_frq,stop_frq)) { if (!is_equal(start_frq,stop_frq)) {
if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f) { if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f) {
sweep.ph180_freq = curr_test_freq; sweep.ph180.freq = curr_test.freq;
sweep.ph180_gain = curr_test_gain; sweep.ph180.gain = curr_test.gain;
sweep.ph180_phase = curr_test_phase; sweep.ph180.phase = curr_test.phase;
} }
if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f) { if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f) {
sweep.ph270_freq = curr_test_freq; sweep.ph270.freq = curr_test.freq;
sweep.ph270_gain = curr_test_gain; sweep.ph270.gain = curr_test.gain;
sweep.ph270_phase = curr_test_phase; sweep.ph270.phase = curr_test.phase;
} }
if (curr_test_gain > sweep.maxgain_gain) { if (curr_test.gain > sweep.maxgain.gain) {
sweep.maxgain_gain = curr_test_gain; sweep.maxgain.gain = curr_test.gain;
sweep.maxgain_freq = curr_test_freq; sweep.maxgain.freq = curr_test.freq;
sweep.maxgain_phase = curr_test_phase; sweep.maxgain.phase = curr_test.phase;
} }
if (now - step_start_time_ms >= sweep_time_ms + 200) { if (now - step_start_time_ms >= sweep_time_ms + 200) {
// we have passed the maximum stop time // we have passed the maximum stop time
@ -1614,25 +1614,25 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai
frq_cnt++; frq_cnt++;
if (frq_cnt == 12) { if (frq_cnt == 12) {
freq[frq_cnt] = freq[rp_prev_good_frq_cnt]; freq[frq_cnt] = freq[rp_prev_good_frq_cnt];
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
} else { } else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
} }
} else if (is_equal(start_freq,stop_freq)) { } else if (is_equal(start_freq,stop_freq)) {
if (phase[frq_cnt] > 180.0f) { if (phase[frq_cnt] > 180.0f) {
curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; curr_test.freq = curr_test.freq - 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq; freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] < 160.0f) { } else if (phase[frq_cnt] < 160.0f) {
curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; curr_test.freq = curr_test.freq + 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq; freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { } 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) { 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; tune_p += 0.05f * max_gain_p.max_allowed;
} else { } else {
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test_freq and frq_cnt for next test // reset curr_test.freq and frq_cnt for next test
curr_test_freq = freq[0]; curr_test.freq = freq[0];
frq_cnt = 0; frq_cnt = 0;
tune_p -= 0.05f * max_gain_p.max_allowed; tune_p -= 0.05f * max_gain_p.max_allowed;
tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed);
@ -1643,8 +1643,8 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai
if (counter == AUTOTUNE_SUCCESS_COUNT) { if (counter == AUTOTUNE_SUCCESS_COUNT) {
start_freq = 0.0f; //initializes next test that uses dwell test start_freq = 0.0f; //initializes next test that uses dwell test
} else { } else {
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
} }
} }
@ -1655,15 +1655,15 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai
// frequency sweep was conducted. check to see if freq for 180 deg phase was determined and start there if it was // frequency sweep was conducted. check to see if freq for 180 deg phase was determined and start there if it was
if (!is_equal(start_freq,stop_freq)) { if (!is_equal(start_freq,stop_freq)) {
if (!is_zero(sweep.ph180_freq)) { if (!is_zero(sweep.ph180.freq)) {
freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; freq[frq_cnt] = sweep.ph180.freq - 0.5f * test_freq_incr;
frq_cnt = 12; frq_cnt = 12;
freq_cnt_max = frq_cnt; freq_cnt_max = frq_cnt;
} else { } else {
frq_cnt = 1; frq_cnt = 1;
freq[frq_cnt] = min_sweep_freq; freq[frq_cnt] = min_sweep_freq;
} }
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
} }
// if sweep failed to find frequency for 180 phase then use dwell to find frequency // 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 (frq_cnt < 12 && is_equal(start_freq,stop_freq)) {
@ -1679,26 +1679,26 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai
frq_cnt++; frq_cnt++;
if (frq_cnt == 12) { if (frq_cnt == 12) {
freq[frq_cnt] = freq[rd_prev_good_frq_cnt]; freq[frq_cnt] = freq[rd_prev_good_frq_cnt];
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
} else { } else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
} }
} else if (is_equal(start_freq,stop_freq)) { } else if (is_equal(start_freq,stop_freq)) {
if (phase[frq_cnt] > 180.0f) { if (phase[frq_cnt] > 180.0f) {
curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; curr_test.freq = curr_test.freq - 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq; freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] < 160.0f) { } else if (phase[frq_cnt] < 160.0f) {
curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; curr_test.freq = curr_test.freq + 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq; freq[frq_cnt] = curr_test.freq;
} else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { } 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 ((gain[frq_cnt] < 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; tune_d += 0.05f * max_gain_d.max_allowed;
rd_prev_gain = gain[frq_cnt]; rd_prev_gain = gain[frq_cnt];
} else { } else {
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test_freq and frq_cnt for next test // reset curr_test.freq and frq_cnt for next test
curr_test_freq = freq[0]; curr_test.freq = freq[0];
frq_cnt = 0; frq_cnt = 0;
rd_prev_gain = 0.0f; rd_prev_gain = 0.0f;
tune_d -= 0.05f * max_gain_d.max_allowed; tune_d -= 0.05f * max_gain_d.max_allowed;
@ -1710,8 +1710,8 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai
start_freq = 0.0f; //initializes next test that uses dwell test start_freq = 0.0f; //initializes next test that uses dwell test
reset_sweep_variables(); reset_sweep_variables();
} else { } else {
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
} }
} }
@ -1722,16 +1722,16 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
float gain_incr = 0.5f; float gain_incr = 0.5f;
if (!is_equal(start_freq,stop_freq)) { if (!is_equal(start_freq,stop_freq)) {
if (!is_zero(sweep.maxgain_freq)) { if (!is_zero(sweep.maxgain.freq)) {
frq_cnt = 12; frq_cnt = 12;
freq[frq_cnt] = sweep.maxgain_freq - 0.5f * test_freq_incr; freq[frq_cnt] = sweep.maxgain.freq - 0.5f * test_freq_incr;
freq_cnt_max = frq_cnt; freq_cnt_max = frq_cnt;
} else { } else {
frq_cnt = 1; frq_cnt = 1;
freq[frq_cnt] = min_sweep_freq; freq[frq_cnt] = min_sweep_freq;
freq_cnt_max = 0; freq_cnt_max = 0;
} }
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
} }
if (freq_cnt < 12 && is_equal(start_freq,stop_freq)) { if (freq_cnt < 12 && is_equal(start_freq,stop_freq)) {
if (gain[freq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { if (gain[freq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) {
@ -1755,10 +1755,10 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
freq_cnt++; freq_cnt++;
if (freq_cnt == 12) { if (freq_cnt == 12) {
freq[freq_cnt] = freq[freq_cnt_max]; freq[freq_cnt] = freq[freq_cnt_max];
curr_test_freq = freq[freq_cnt]; curr_test.freq = freq[freq_cnt];
} else { } else {
freq[freq_cnt] = freq[freq_cnt-1] + test_freq_incr; freq[freq_cnt] = freq[freq_cnt-1] + test_freq_incr;
curr_test_freq = freq[freq_cnt]; curr_test.freq = freq[freq_cnt];
} }
} }
@ -1778,7 +1778,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
} }
} }
curr_test_freq = freq[freq_cnt]; curr_test.freq = freq[freq_cnt];
sp_prev_gain = gain[freq_cnt]; sp_prev_gain = gain[freq_cnt];
} else if (gain[freq_cnt] > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN && !find_peak) { } else if (gain[freq_cnt] > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN && !find_peak) {
tune_p -= gain_incr; tune_p -= gain_incr;
@ -1790,7 +1790,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
find_peak = false; find_peak = false;
phase_max = phase[freq_cnt]; phase_max = phase[freq_cnt];
} }
curr_test_freq = freq[freq_cnt]; curr_test.freq = freq[freq_cnt];
sp_prev_gain = gain[freq_cnt]; sp_prev_gain = gain[freq_cnt];
} else { } else {
// adjust tuning gain so max response gain is not exceeded // adjust tuning gain so max response gain is not exceeded
@ -1804,11 +1804,11 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga
if (counter == AUTOTUNE_SUCCESS_COUNT) { if (counter == AUTOTUNE_SUCCESS_COUNT) {
start_freq = 0.0f; //initializes next test that uses dwell test start_freq = 0.0f; //initializes next test that uses dwell test
reset_sweep_variables(); reset_sweep_variables();
curr_test_freq = freq[0]; curr_test.freq = freq[0];
freq_cnt = 0; freq_cnt = 0;
} else { } else {
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
} }
} }
@ -1819,14 +1819,14 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
if (!is_equal(start_freq,stop_freq)) { if (!is_equal(start_freq,stop_freq)) {
frq_cnt = 2; frq_cnt = 2;
if (!is_zero(sweep.ph180_freq)) { if (!is_zero(sweep.ph180.freq)) {
freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; freq[frq_cnt] = sweep.ph180.freq - 0.5f * test_freq_incr;
} else { } else {
freq[frq_cnt] = min_sweep_freq; freq[frq_cnt] = min_sweep_freq;
} }
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
} else if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { } else if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) {
if (frq_cnt > 2 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 270.0f && if (frq_cnt > 2 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 270.0f &&
@ -1849,11 +1849,11 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
found_max_p = true; found_max_p = true;
find_middle = false; find_middle = false;
if (!is_zero(sweep.ph270_freq)) { if (!is_zero(sweep.ph270.freq)) {
// set freq cnt back to reinitialize process // set freq cnt back to reinitialize process
frq_cnt = 1; // set to 1 because it will be incremented frq_cnt = 1; // set to 1 because it will be incremented
// set frequency back at least one test_freq_incr as it will be added // set frequency back at least one test_freq_incr as it will be added
freq[1] = sweep.ph270_freq - 1.5f * test_freq_incr; freq[1] = sweep.ph270.freq - 1.5f * test_freq_incr;
} }
} }
if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f && if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f &&
@ -1884,7 +1884,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
if (frq_cnt == 12) { if (frq_cnt == 12) {
counter = AUTOTUNE_SUCCESS_COUNT; counter = AUTOTUNE_SUCCESS_COUNT;
// reset variables for next test // reset variables for next test
curr_test_freq = freq[0]; curr_test.freq = freq[0];
frq_cnt = 0; frq_cnt = 0;
start_freq = 0.0f; //initializes next test that uses dwell test start_freq = 0.0f; //initializes next test that uses dwell test
reset_sweep_variables(); reset_sweep_variables();
@ -1904,9 +1904,9 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase
} else { } else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
} }
curr_test_freq = freq[frq_cnt]; curr_test.freq = freq[frq_cnt];
start_freq = curr_test_freq; start_freq = curr_test.freq;
stop_freq = curr_test_freq; stop_freq = curr_test.freq;
} }
} }
if (found_max_p && found_max_d) { if (found_max_p && found_max_d) {
@ -1948,7 +1948,7 @@ void AC_AutoTune_Heli::Log_AutoTuneDetails()
// log autotune frequency response data // log autotune frequency response data
void AC_AutoTune_Heli::Log_AutoTuneSweep() void AC_AutoTune_Heli::Log_AutoTuneSweep()
{ {
Log_Write_AutoTuneSweep(curr_test_freq, curr_test_gain, curr_test_phase); Log_Write_AutoTuneSweep(curr_test.freq, curr_test.gain, curr_test.phase);
} }
// @LoggerMessage: ATNH // @LoggerMessage: ATNH
@ -2091,15 +2091,16 @@ void AC_AutoTune_Heli::reset_maxgains_update_gain_variables()
// reset the max_gains update gain variables // reset the max_gains update gain variables
void AC_AutoTune_Heli::reset_sweep_variables() void AC_AutoTune_Heli::reset_sweep_variables()
{ {
sweep.ph180_freq = 0.0f; sweep.ph180.freq = 0.0f;
sweep.ph180_gain = 0.0f; sweep.ph180.gain = 0.0f;
sweep.ph180_phase = 0.0f; sweep.ph180.phase = 0.0f;
sweep.ph270_freq = 0.0f; sweep.ph270.freq = 0.0f;
sweep.ph270_gain = 0.0f; sweep.ph270.gain = 0.0f;
sweep.ph270_phase = 0.0f; sweep.ph270.phase = 0.0f;
sweep.maxgain_gain = 0.0f; sweep.maxgain.gain = 0.0f;
sweep.maxgain_freq = 0.0f; sweep.maxgain.freq = 0.0f;
sweep.maxgain_phase = 0.0f; sweep.maxgain.phase = 0.0f;
sweep.progress = 0; sweep.progress = 0;
} }

25
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -228,9 +228,15 @@ private:
float test_phase[20]; // frequency response phase for each dwell test iteration float test_phase[20]; // frequency response phase for each dwell test iteration
float dwell_start_time_ms; // start time in ms of dwell test float dwell_start_time_ms; // start time in ms of dwell test
uint8_t freq_cnt_max; // counter number for frequency that produced max gain response uint8_t freq_cnt_max; // counter number for frequency that produced max gain response
float curr_test_freq; // current test frequency
float curr_test_gain; // current test frequency response gain // sweep_info contains information about a specific test's sweep results
float curr_test_phase; // current test frequency response phase struct sweep_info {
float freq;
float gain;
float phase;
};
sweep_info curr_test;
Vector3f start_angles; // aircraft attitude at the start of test Vector3f start_angles; // aircraft attitude at the start of test
uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test
uint32_t phase_out_time; // time in ms to phase out response uint32_t phase_out_time; // time in ms to phase out response
@ -263,15 +269,10 @@ private:
// sweep_data tracks the overall characteristics in the response to the frequency sweep // sweep_data tracks the overall characteristics in the response to the frequency sweep
struct sweep_data { struct sweep_data {
float maxgain_freq; sweep_info maxgain;
float maxgain_gain; sweep_info ph180;
float maxgain_phase; sweep_info ph270;
float ph180_freq;
float ph180_gain;
float ph180_phase;
float ph270_freq;
float ph270_gain;
float ph270_phase;
uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg; uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
}; };
sweep_data sweep; sweep_data sweep;