From 9e4d118cc9ab25a678b046e98a1dbf1a10fd5938 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sat, 6 Apr 2024 23:55:51 -0400 Subject: [PATCH] AC_AutoTune: remove sweep for Rate D up --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 272 ++++++++++++--------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 10 +- 2 files changed, 170 insertions(+), 112 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index f16329c90c..18cb47befc 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -149,7 +149,8 @@ void AC_AutoTune_Heli::test_init() attitude_control->bf_feedforward(false); attitude_control->use_sqrt_controller(false); - freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp_tgt.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp_mtr.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, stop_freq, start_freq, RATE); step_time_limit_ms = (uint32_t) 23000; @@ -160,14 +161,14 @@ void AC_AutoTune_Heli::test_init() case RD_UP: // initialize start frequency if (is_zero(start_freq)) { - if (tune_type == RP_UP) { + if (tune_type == RP_UP || tune_type == RD_UP) { // continue using frequency where testing left off or RD_UP completed if (test_phase[12] > 0.0f && test_phase[12] < 180.0f) { freq_cnt = 12; // start with freq found for sweep where phase was 180 deg - } else if (!is_zero(sweep.ph180.freq)) { + } else if (!is_zero(sweep_tgt.ph180.freq)) { freq_cnt = 12; - test_freq[freq_cnt] = sweep.ph180.freq - 0.25f * 3.14159f * 2.0f; + test_freq[freq_cnt] = 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; @@ -177,11 +178,11 @@ void AC_AutoTune_Heli::test_init() start_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 starts with a sweep initially but if it has been completed then start dwells at the freq for 180 deg phase } else { - if (!is_zero(sweep.ph180.freq)) { + if (!is_zero(sweep_mtr.ph180.freq)) { freq_cnt = 12; - test_freq[freq_cnt] = sweep.ph180.freq - 0.25f * 3.14159f * 2.0f; + test_freq[freq_cnt] = sweep_mtr.ph180.freq - 0.25f * 3.14159f * 2.0f; curr_test.freq = test_freq[freq_cnt]; start_freq = curr_test.freq; stop_freq = curr_test.freq; @@ -199,11 +200,13 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine_gain function whenever test is initialized - freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp_tgt.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp_mtr.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, stop_freq, stop_freq, RATE); } else { // initialize determine_gain function whenever test is initialized - freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp_tgt.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp_mtr.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, stop_freq, start_freq, RATE); } if (!is_zero(start_freq)) { @@ -214,9 +217,9 @@ void AC_AutoTune_Heli::test_init() case SP_UP: // initialize start frequency if (is_zero(start_freq)) { - if (!is_zero(sweep.maxgain.freq)) { + if (!is_zero(sweep_tgt.maxgain.freq)) { freq_cnt = 12; - test_freq[freq_cnt] = sweep.maxgain.freq - 0.25f * 3.14159f * 2.0f; + test_freq[freq_cnt] = sweep_tgt.maxgain.freq - 0.25f * 3.14159f * 2.0f; curr_test.freq = test_freq[freq_cnt]; start_freq = curr_test.freq; stop_freq = curr_test.freq; @@ -231,11 +234,13 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine gain function - freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp_tgt.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp_mtr.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); dwell_test_init(start_freq, stop_freq, stop_freq, DRB); } else { // initialize determine gain function - freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp_tgt.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp_mtr.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); dwell_test_init(start_freq, stop_freq, start_freq, DRB); } @@ -252,7 +257,8 @@ void AC_AutoTune_Heli::test_init() stop_freq = max_sweep_freq; } // initialize determine gain function - freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp_tgt.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp_mtr.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); dwell_test_init(start_freq, stop_freq, stop_freq, ANGLE); // TODO add time limit for sweep test if (!is_zero(start_freq)) { @@ -411,8 +417,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(tune_accel), (double)(test_accel_max)); } } 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: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep_tgt.ph180.freq), (double)(sweep_tgt.ph180.gain)); } break; default: @@ -870,30 +876,64 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, command_out = command_filt.apply((command_reading - filt_command_reading.get()), AP::scheduler().get_loop_period_s()); + float dwell_gain_mtr = 0.0f; + float dwell_phase_mtr = 0.0f; + float dwell_gain_tgt = 0.0f; + float dwell_phase_tgt = 0.0f; // wait for dwell to start before determining gain and phase if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { - if (freq_resp_input == 1) { - freqresp.update(command_out,filt_target_rate,rotation_rate, dwell_freq); - } else { - freqresp.update(command_out,command_out,rotation_rate, dwell_freq); - } + freqresp_mtr.update(command_out,command_out,rotation_rate, dwell_freq); + freqresp_tgt.update(command_out,filt_target_rate,rotation_rate, dwell_freq); - if (freqresp.is_cycle_complete()) { + if (freqresp_mtr.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { - curr_test.freq = freqresp.get_freq(); - curr_test.gain = freqresp.get_gain(); - curr_test.phase = freqresp.get_phase(); - if (dwell_type == DRB) {test_accel_max = freqresp.get_accel_max();} + curr_test_mtr.freq = freqresp_mtr.get_freq(); + curr_test_mtr.gain = freqresp_mtr.get_gain(); + curr_test_mtr.phase = freqresp_mtr.get_phase(); // reset cycle_complete to allow indication of next cycle - freqresp.reset_cycle_complete(); + freqresp_mtr.reset_cycle_complete(); #if HAL_LOGGING_ENABLED // log sweep data Log_AutoTuneSweep(); #endif } else { - dwell_gain = freqresp.get_gain(); - dwell_phase = freqresp.get_phase(); - if (dwell_type == DRB) {test_accel_max = freqresp.get_accel_max();} + dwell_gain_mtr = freqresp_mtr.get_gain(); + dwell_phase_mtr = freqresp_mtr.get_phase(); + } + } + + if (freqresp_tgt.is_cycle_complete()) { + if (!is_equal(start_frq,stop_frq)) { + curr_test_tgt.freq = freqresp_tgt.get_freq(); + curr_test_tgt.gain = freqresp_tgt.get_gain(); + curr_test_tgt.phase = freqresp_tgt.get_phase(); + if (dwell_type == DRB) {test_accel_max = freqresp_tgt.get_accel_max();} + // reset cycle_complete to allow indication of next cycle + freqresp_tgt.reset_cycle_complete(); +#if HAL_LOGGING_ENABLED + // log sweep data + Log_AutoTuneSweep(); +#endif + } else { + dwell_gain_tgt = freqresp_tgt.get_gain(); + dwell_phase_tgt = freqresp_tgt.get_phase(); + if (dwell_type == DRB) {test_accel_max = freqresp_tgt.get_accel_max();} + } + } + + if (freq_resp_input == 1) { + if (!is_equal(start_frq,stop_frq)) { + curr_test = curr_test_tgt; + } else { + dwell_gain = dwell_gain_tgt; + dwell_phase = dwell_phase_tgt; + } + } else { + if (!is_equal(start_frq,stop_frq)) { + curr_test = curr_test_mtr; + } else { + dwell_gain = dwell_gain_mtr; + dwell_phase = dwell_phase_mtr; } } } @@ -901,26 +941,42 @@ 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 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. - if (curr_test.phase > 180.0f && sweep.progress == 0) { - sweep.progress = 1; - } else if (curr_test.phase > 270.0f && sweep.progress == 1) { - sweep.progress = 2; + if (curr_test_tgt.phase > 180.0f && sweep_tgt.progress == 0) { + sweep_tgt.progress = 1; + } else if (curr_test_tgt.phase > 270.0f && sweep_tgt.progress == 1) { + sweep_tgt.progress = 2; } - if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f && sweep.progress == 0) { - sweep.ph180 = curr_test; + if (curr_test_tgt.phase <= 160.0f && curr_test_tgt.phase >= 150.0f && sweep_tgt.progress == 0) { + sweep_tgt.ph180 = curr_test_tgt; } - if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f && sweep.progress == 1) { - sweep.ph270 = curr_test; + if (curr_test_tgt.phase <= 250.0f && curr_test_tgt.phase >= 240.0f && sweep_tgt.progress == 1) { + sweep_tgt.ph270 = curr_test_tgt; } - if (curr_test.gain > sweep.maxgain.gain) { - sweep.maxgain = curr_test; + if (curr_test_tgt.gain > sweep_tgt.maxgain.gain) { + sweep_tgt.maxgain = curr_test_tgt; } + // Determine sweep info for motor input to response output + if (curr_test_mtr.phase > 180.0f && sweep_mtr.progress == 0) { + sweep_mtr.progress = 1; + } else if (curr_test_mtr.phase > 270.0f && sweep_mtr.progress == 1) { + sweep_mtr.progress = 2; + } + if (curr_test_mtr.phase <= 160.0f && curr_test_mtr.phase >= 150.0f && sweep_mtr.progress == 0) { + sweep_mtr.ph180 = curr_test_mtr; + } + if (curr_test_mtr.phase <= 250.0f && curr_test_mtr.phase >= 240.0f && sweep_mtr.progress == 1) { + sweep_mtr.ph270 = curr_test_mtr; + } + if (curr_test_mtr.gain > sweep_mtr.maxgain.gain) { + sweep_mtr.maxgain = curr_test_mtr; + } + if (now - step_start_time_ms >= sweep_time_ms + 200) { // we have passed the maximum stop time step = UPDATE_GAINS; } } else { - if (now - step_start_time_ms >= step_time_limit_ms || freqresp.is_cycle_complete()) { + if (now - step_start_time_ms >= step_time_limit_ms || (freqresp_tgt.is_cycle_complete() && freqresp_mtr.is_cycle_complete())) { // we have passed the maximum stop time step = UPDATE_GAINS; } @@ -1166,18 +1222,6 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai { float test_freq_incr = 0.25f * 3.14159f * 2.0f; // set for 1/4 hz increments - // 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_zero(sweep.ph180.freq)) { - freq[frq_cnt] = sweep.ph180.freq - 0.5f * test_freq_incr; - frq_cnt = 12; - freq_cnt_max = frq_cnt; - } else { - frq_cnt = 1; - freq[frq_cnt] = min_sweep_freq; - } - curr_test.freq = freq[frq_cnt]; - } // 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])) { @@ -1221,7 +1265,6 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai } if (counter == AUTOTUNE_SUCCESS_COUNT) { start_freq = 0.0f; //initializes next test that uses dwell test - reset_sweep_variables(); } else { start_freq = curr_test.freq; stop_freq = curr_test.freq; @@ -1235,9 +1278,9 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga float gain_incr = 0.5f; if (!is_equal(start_freq,stop_freq)) { - if (!is_zero(sweep.maxgain.freq)) { + if (!is_zero(sweep_tgt.maxgain.freq)) { frq_cnt = 12; - freq[frq_cnt] = sweep.maxgain.freq - 0.5f * test_freq_incr; + freq[frq_cnt] = sweep_tgt.maxgain.freq - 0.5f * test_freq_incr; freq_cnt_max = frq_cnt; } else { frq_cnt = 1; @@ -1246,69 +1289,69 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga } curr_test.freq = freq[frq_cnt]; } - if (freq_cnt < 12 && is_equal(start_freq,stop_freq)) { - if (gain[freq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { + if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { + if (gain[frq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { // exceeded max response gain already, reduce tuning gain to remain under max response gain tune_p -= gain_incr; // force counter to stay on frequency - freq_cnt -= 1; - } else if (gain[freq_cnt] > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) { + frq_cnt -= 1; + } else if (gain[frq_cnt] > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) { // exceeded max response gain at the minimum allowable tuning gain. terminate testing. tune_p = AUTOTUNE_SP_MIN; counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); - } else if (gain[freq_cnt] > gain[freq_cnt_max]) { - freq_cnt_max = freq_cnt; - phase_max = phase[freq_cnt]; - sp_prev_gain = gain[freq_cnt]; - } else if (freq[freq_cnt] > max_sweep_freq || (gain[freq_cnt] > 0.0f && gain[freq_cnt] < 0.5f)) { + } else if (gain[frq_cnt] > gain[freq_cnt_max]) { + freq_cnt_max = frq_cnt; + phase_max = phase[frq_cnt]; + sp_prev_gain = gain[frq_cnt]; + } else if (freq[frq_cnt] > max_sweep_freq || (gain[frq_cnt] > 0.0f && gain[frq_cnt] < 0.5f)) { // must be past peak, continue on to determine angle p - freq_cnt = 11; + frq_cnt = 11; } - freq_cnt++; - if (freq_cnt == 12) { - freq[freq_cnt] = freq[freq_cnt_max]; - curr_test.freq = freq[freq_cnt]; + frq_cnt++; + if (frq_cnt == 12) { + freq[frq_cnt] = freq[freq_cnt_max]; + curr_test.freq = freq[frq_cnt]; } else { - freq[freq_cnt] = freq[freq_cnt-1] + test_freq_incr; - curr_test.freq = freq[freq_cnt]; + freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; + curr_test.freq = freq[frq_cnt]; } } // once finished with sweep of frequencies, cnt = 12 is used to then tune for max response gain - if (freq_cnt >= 12 && is_equal(start_freq,stop_freq)) { - if (gain[freq_cnt] < max_resp_gain && tune_p < AUTOTUNE_SP_MAX && !find_peak) { + if (frq_cnt >= 12 && is_equal(start_freq,stop_freq)) { + if (gain[frq_cnt] < max_resp_gain && tune_p < AUTOTUNE_SP_MAX && !find_peak) { // keep increasing tuning gain unless phase changes or max response gain is achieved - if (phase[freq_cnt]-phase_max > 20.0f && phase[freq_cnt] < 210.0f) { - freq[freq_cnt] += 0.5 * test_freq_incr; + if (phase[frq_cnt]-phase_max > 20.0f && phase[frq_cnt] < 210.0f) { + freq[frq_cnt] += 0.5 * test_freq_incr; find_peak = true; } else { tune_p += gain_incr; - freq[freq_cnt] = freq[freq_cnt_max]; + freq[frq_cnt] = freq[freq_cnt_max]; 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[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) { + curr_test.freq = freq[frq_cnt]; + sp_prev_gain = gain[frq_cnt]; + } else if (gain[frq_cnt] > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN && !find_peak) { tune_p -= gain_incr; } else if (find_peak) { // find the frequency where the response gain is maximum - if (gain[freq_cnt] > sp_prev_gain) { - freq[freq_cnt] += 0.5 * test_freq_incr; + if (gain[frq_cnt] > sp_prev_gain) { + freq[frq_cnt] += 0.5 * test_freq_incr; } else { find_peak = false; - phase_max = phase[freq_cnt]; + phase_max = phase[frq_cnt]; } - curr_test.freq = freq[freq_cnt]; - sp_prev_gain = gain[freq_cnt]; + curr_test.freq = freq[frq_cnt]; + sp_prev_gain = gain[frq_cnt]; } else { // adjust tuning gain so max response gain is not exceeded - if (sp_prev_gain < max_resp_gain && gain[freq_cnt] > max_resp_gain) { - float adj_factor = (max_resp_gain - gain[freq_cnt]) / (gain[freq_cnt] - sp_prev_gain); + if (sp_prev_gain < max_resp_gain && gain[frq_cnt] > max_resp_gain) { + float adj_factor = (max_resp_gain - gain[frq_cnt]) / (gain[frq_cnt] - sp_prev_gain); tune_p = tune_p + gain_incr * adj_factor; } counter = AUTOTUNE_SUCCESS_COUNT; @@ -1318,7 +1361,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga start_freq = 0.0f; //initializes next test that uses dwell test reset_sweep_variables(); curr_test.freq = freq[0]; - freq_cnt = 0; + frq_cnt = 0; } else { start_freq = curr_test.freq; stop_freq = curr_test.freq; @@ -1332,8 +1375,8 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase if (!is_equal(start_freq,stop_freq)) { frq_cnt = 2; - if (!is_zero(sweep.ph180.freq)) { - freq[frq_cnt] = sweep.ph180.freq - 0.5f * test_freq_incr; + if (!is_zero(sweep_mtr.ph180.freq)) { + freq[frq_cnt] = sweep_mtr.ph180.freq - 0.5f * test_freq_incr; } else { freq[frq_cnt] = min_sweep_freq; } @@ -1362,11 +1405,11 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase found_max_p = true; find_middle = false; - if (!is_zero(sweep.ph270.freq)) { + if (!is_zero(sweep_mtr.ph270.freq)) { // set freq cnt back to reinitialize process 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 - freq[1] = sweep.ph270.freq - 1.5f * test_freq_incr; + freq[1] = sweep_mtr.ph270.freq - 1.5f * test_freq_incr; } } if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f && @@ -1400,7 +1443,6 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase curr_test.freq = freq[0]; frq_cnt = 0; start_freq = 0.0f; //initializes next test that uses dwell test - reset_sweep_variables(); } else { if (frq_cnt == 3 && phase[2] >= 161.0f && !found_max_p) { // phase greater than 161 deg won't allow max p to be found @@ -1493,7 +1535,7 @@ void AC_AutoTune_Heli::Log_AutoTuneDetails() // log autotune frequency response data void AC_AutoTune_Heli::Log_AutoTuneSweep() { - Log_Write_AutoTuneSweep(curr_test.freq, curr_test.gain, curr_test.phase); + Log_Write_AutoTuneSweep(curr_test_mtr.freq, curr_test_mtr.gain, curr_test_mtr.phase,curr_test_tgt.freq, curr_test_tgt.gain, curr_test_tgt.phase); } // @LoggerMessage: ATNH @@ -1560,25 +1602,31 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate } // Write an Autotune frequency response data packet -void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float phase) +void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq_mtr, float gain_mtr, float phase_mtr, float freq_tgt, float gain_tgt, float phase_tgt) { // @LoggerMessage: ATSH // @Description: Heli AutoTune Sweep packet // @Vehicles: Copter // @Field: TimeUS: Time since system startup - // @Field: freq: current frequency - // @Field: gain: current response gain - // @Field: phase: current response phase + // @Field: freq_m: current frequency for motor input to rate + // @Field: gain_m: current response gain for motor input to rate + // @Field: phase_m: current response phase for motor input to rate + // @Field: freq_t: current frequency for target rate to rate + // @Field: gain_t: current response gain for target rate to rate + // @Field: phase_t: current response phase for target rate to rate AP::logger().WriteStreaming( "ATSH", - "TimeUS,freq,gain,phase", - "sE-d", - "F000", - "Qfff", + "TimeUS,freq_m,gain_m,phase_m,freq_t,gain_t,phase_t", + "sE-dE-d", + "F000000", + "Qffffff", AP_HAL::micros64(), - freq, - gain, - phase); + freq_mtr, + gain_mtr, + phase_mtr, + freq_tgt, + gain_tgt, + phase_tgt); } #endif // HAL_LOGGING_ENABLED @@ -1631,11 +1679,17 @@ void AC_AutoTune_Heli::reset_maxgains_update_gain_variables() // reset the max_gains update gain variables void AC_AutoTune_Heli::reset_sweep_variables() { - sweep.ph180 = {}; - sweep.ph270 = {}; - sweep.maxgain = {}; + sweep_tgt.ph180 = {}; + sweep_tgt.ph270 = {}; + sweep_tgt.maxgain = {}; + sweep_tgt.progress = 0; + + sweep_mtr.ph180 = {}; + sweep_mtr.ph270 = {}; + sweep_mtr.maxgain = {}; + + sweep_mtr.progress = 0; - sweep.progress = 0; } // set the tuning test sequence diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index b9733c6643..e768b1a98c 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -114,7 +114,7 @@ protected: // methods to log autotune frequency response results void Log_AutoTuneSweep() override; - void Log_Write_AutoTuneSweep(float freq, float gain, float phase); + void Log_Write_AutoTuneSweep(float freq_mtr, float gain_mtr, float phase_mtr, float freq_tgt, float gain_tgt, float phase_tgt); #endif // send intermittent updates to user on status of tune @@ -253,6 +253,8 @@ private: float phase; }; sweep_info curr_test; + sweep_info curr_test_mtr; + sweep_info curr_test_tgt; 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 @@ -285,7 +287,8 @@ private: 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_mtr; + sweep_data sweep_tgt; // fix the frequency sweep time to 23 seconds const float sweep_time_ms = 23000; @@ -302,7 +305,8 @@ private: AP_Float rate_max; // maximum autotune angular rate // freqresp object for the frequency response tests - AC_AutoTune_FreqResp freqresp; + AC_AutoTune_FreqResp freqresp_mtr; // frequency response of output to motor mixer input + AC_AutoTune_FreqResp freqresp_tgt; // frequency response of output to target input Chirp chirp_input; };