From ad1b01cd566e31b8b48b3a9add3a04910a0bc739 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sun, 19 May 2024 22:36:38 -0400 Subject: [PATCH] AC_AutoTune: improve RFF dwell test and data processing --- libraries/AC_AutoTune/AC_AutoTune.h | 6 +- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 141 ++++++++++++++------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 5 +- 3 files changed, 100 insertions(+), 52 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 4a8eb6352b..510fd6c59f 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -283,9 +283,9 @@ protected: LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second // backup of currently being tuned parameter values - float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_dff, orig_roll_fltt, orig_roll_smax, orig_roll_sp, orig_roll_accel; - float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_dff, orig_pitch_fltt, orig_pitch_smax, orig_pitch_sp, orig_pitch_accel; - float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_dff, orig_yaw_fltt, orig_yaw_smax, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel; + float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_dff, orig_roll_fltt, orig_roll_smax, orig_roll_sp, orig_roll_accel, orig_roll_rate; + float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_dff, orig_pitch_fltt, orig_pitch_smax, orig_pitch_sp, orig_pitch_accel, orig_pitch_rate; + float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_dff, orig_yaw_fltt, orig_yaw_smax, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel, orig_yaw_rate; bool orig_bf_feedforward; // currently being tuned parameter values diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 6312267da0..d78f4970ac 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -142,6 +142,8 @@ void AC_AutoTune_Heli::test_init() AC_AutoTune_FreqResp::ResponseType resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; FreqRespCalcType calc_type = RATE; FreqRespInput freq_resp_input = TARGET; + float freq_resp_amplitude = 5.0f; // amplitude in deg + float filter_freq = 10.0f; switch (tune_type) { case RFF_UP: if (!is_positive(next_test_freq)) { @@ -150,10 +152,12 @@ void AC_AutoTune_Heli::test_init() start_freq = next_test_freq; } stop_freq = start_freq; - + filter_freq = start_freq; + attitude_control->bf_feedforward(false); attitude_control->use_sqrt_controller(false); - +// attitude_control->use_sqrt_controller(true); // invoke rate and acceleration limits to provide square wave rate response. +// freq_resp_amplitude = 10.0f; // increase amplitude with limited rate to get desired square wave rate response // variables needed to initialize frequency response object and test method resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; calc_type = RATE; @@ -172,6 +176,7 @@ void AC_AutoTune_Heli::test_init() stop_freq = start_freq; test_accel_max = 0.0f; } + filter_freq = start_freq; attitude_control->bf_feedforward(false); attitude_control->use_sqrt_controller(false); @@ -201,6 +206,7 @@ void AC_AutoTune_Heli::test_init() start_freq = next_test_freq; } stop_freq = start_freq; + filter_freq = start_freq; attitude_control->bf_feedforward(false); attitude_control->use_sqrt_controller(false); @@ -223,6 +229,7 @@ void AC_AutoTune_Heli::test_init() stop_freq = start_freq; test_accel_max = 0.0f; } + filter_freq = start_freq; attitude_control->bf_feedforward(false); attitude_control->use_sqrt_controller(false); @@ -238,6 +245,7 @@ void AC_AutoTune_Heli::test_init() start_freq = min_sweep_freq; stop_freq = max_sweep_freq; test_accel_max = 0.0f; + filter_freq = start_freq; // variables needed to initialize frequency response object and test method resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE; @@ -256,7 +264,7 @@ void AC_AutoTune_Heli::test_init() // initialize dwell test method - dwell_test_init(start_freq, stop_freq, start_freq, freq_resp_input, calc_type, resp_type, input_type); + dwell_test_init(start_freq, stop_freq, freq_resp_amplitude, filter_freq, freq_resp_input, calc_type, resp_type, input_type); start_angles = Vector3f(roll_cd, pitch_cd, desired_yaw_cd); // heli specific } @@ -418,6 +426,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise() orig_roll_smax = attitude_control->get_rate_roll_pid().slew_limit(); orig_roll_sp = attitude_control->get_angle_roll_p().kP(); orig_roll_accel = attitude_control->get_accel_roll_max_cdss(); + orig_roll_rate = attitude_control->get_ang_vel_roll_max_degs(); tune_roll_rp = attitude_control->get_rate_roll_pid().kP(); tune_roll_rd = attitude_control->get_rate_roll_pid().kD(); tune_roll_rff = attitude_control->get_rate_roll_pid().ff(); @@ -432,6 +441,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise() orig_pitch_smax = attitude_control->get_rate_pitch_pid().slew_limit(); orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); orig_pitch_accel = attitude_control->get_accel_pitch_max_cdss(); + orig_pitch_rate = attitude_control->get_ang_vel_pitch_max_degs(); tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); tune_pitch_rff = attitude_control->get_rate_pitch_pid().ff(); @@ -447,6 +457,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise() orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz(); orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss(); orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); + orig_yaw_rate = attitude_control->get_ang_vel_yaw_max_degs(); tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); tune_yaw_rff = attitude_control->get_rate_yaw_pid().ff(); @@ -463,13 +474,13 @@ void AC_AutoTune_Heli::load_orig_gains() { attitude_control->bf_feedforward(orig_bf_feedforward); if (roll_enabled()) { - load_gain_set(ROLL, orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); + load_gain_set(ROLL, orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate); } if (pitch_enabled()) { - load_gain_set(PITCH, orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); + load_gain_set(PITCH, orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate); } if (yaw_enabled()) { - load_gain_set(YAW, orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax); + load_gain_set(YAW, orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate); } } @@ -482,14 +493,14 @@ void AC_AutoTune_Heli::load_tuned_gains() attitude_control->set_accel_pitch_max_cdss(0.0f); } if (roll_enabled()) { - load_gain_set(ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); + load_gain_set(ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate); } if (pitch_enabled()) { - load_gain_set(PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); + load_gain_set(PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate); } if (yaw_enabled()) { if (!is_zero(tune_yaw_rp)) { - load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax); + load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax, orig_yaw_rate); } } } @@ -502,13 +513,13 @@ void AC_AutoTune_Heli::load_intra_test_gains() // sanity check the gains attitude_control->bf_feedforward(true); if (roll_enabled()) { - load_gain_set(ROLL, orig_roll_rp, orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); + load_gain_set(ROLL, orig_roll_rp, orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate); } if (pitch_enabled()) { - load_gain_set(PITCH, orig_pitch_rp, orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); + load_gain_set(PITCH, orig_pitch_rp, orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate); } if (yaw_enabled()) { - load_gain_set(YAW, orig_yaw_rp, orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax); + load_gain_set(YAW, orig_yaw_rp, orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate); } } @@ -516,7 +527,7 @@ void AC_AutoTune_Heli::load_intra_test_gains() // called by control_attitude() just before it beings testing a gain (i.e. just before it twitches) void AC_AutoTune_Heli::load_test_gains() { - float rate_p, rate_i, rate_d; + float rate_p, rate_i, rate_d, rate_test_max; switch (axis) { case ROLL: if (tune_type == SP_UP || tune_type == TUNE_CHECK) { @@ -532,7 +543,14 @@ void AC_AutoTune_Heli::load_test_gains() rate_p = tune_roll_rp; rate_d = tune_roll_rd; } - load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, 0.0f); +/* if (tune_type == RFF_UP) { + rate_test_max = 5.0f; + } else { + rate_test_max = orig_roll_rate; + } +*/ + rate_test_max = orig_roll_rate; + load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, 0.0f, rate_test_max); break; case PITCH: if (tune_type == SP_UP || tune_type == TUNE_CHECK) { @@ -548,7 +566,14 @@ void AC_AutoTune_Heli::load_test_gains() rate_p = tune_pitch_rp; rate_d = tune_pitch_rd; } - load_gain_set(PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, 0.0f); +/* if (tune_type == RFF_UP) { + rate_test_max = 5.0f; + } else { + rate_test_max = orig_pitch_rate; + } +*/ + rate_test_max = orig_pitch_rate; + load_gain_set(PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, 0.0f, rate_test_max); break; case YAW: case YAW_D: @@ -558,13 +583,20 @@ void AC_AutoTune_Heli::load_test_gains() // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_yaw_ri; } - load_gain_set(YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, 0.0f); +/* if (tune_type == RFF_UP) { + rate_test_max = 5.0f; + } else { + rate_test_max = orig_yaw_rate; + } +*/ + rate_test_max = orig_yaw_rate; + load_gain_set(YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, 0.0f, rate_test_max); break; } } // load gains -void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax) +void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax, float max_rate) { switch (s_axis) { case ROLL: @@ -576,6 +608,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i attitude_control->get_rate_roll_pid().slew_limit(smax); attitude_control->get_angle_roll_p().kP(angle_p); attitude_control->set_accel_roll_max_cdss(max_accel); + attitude_control->set_ang_vel_roll_max_degs(max_rate); break; case PITCH: attitude_control->get_rate_pitch_pid().kP(rate_p); @@ -586,6 +619,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i attitude_control->get_rate_pitch_pid().slew_limit(smax); attitude_control->get_angle_pitch_p().kP(angle_p); attitude_control->set_accel_pitch_max_cdss(max_accel); + attitude_control->set_ang_vel_pitch_max_degs(max_rate); break; case YAW: case YAW_D: @@ -598,6 +632,7 @@ void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i attitude_control->get_rate_yaw_pid().filt_E_hz(rate_flte); attitude_control->get_angle_yaw_p().kP(angle_p); attitude_control->set_accel_yaw_max_cdss(max_accel); + attitude_control->set_ang_vel_yaw_max_degs(max_rate); break; } } @@ -619,7 +654,7 @@ void AC_AutoTune_Heli::save_tuning_gains() // sanity check the rate P values if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) { - load_gain_set(ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); + load_gain_set(ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate); // save rate roll gains attitude_control->get_rate_roll_pid().save_gains(); @@ -636,7 +671,7 @@ void AC_AutoTune_Heli::save_tuning_gains() } if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) { - load_gain_set(PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); + load_gain_set(PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate); // save rate pitch gains attitude_control->get_rate_pitch_pid().save_gains(); @@ -653,7 +688,7 @@ void AC_AutoTune_Heli::save_tuning_gains() } if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) { - load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax); + load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate); // save rate yaw gains attitude_control->get_rate_yaw_pid().save_gains(); @@ -702,12 +737,14 @@ void AC_AutoTune_Heli::report_axis_gains(const char* axis_string, float rate_P, gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.2f, Max Accel:%0.0f",axis_string,angle_P,max_accel); } -void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type) +void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type) { test_input_type = waveform_input_type; test_freq_resp_input = freq_resp_input; test_calc_type = calc_type; test_start_freq = start_frq; + //target attitude magnitude + tgt_attitude = amplitude * 0.01745f; // initialize frequency response object if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { @@ -744,10 +781,10 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float fi filt_target_rate = 0.0f; // filter at lower frequency to remove steady state - filt_command_reading.set_cutoff_frequency(0.2f * start_frq); - filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq); - filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq); - filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq); + filt_command_reading.set_cutoff_frequency(0.2f * filt_freq); + filt_gyro_reading.set_cutoff_frequency(0.05f * filt_freq); + filt_tgt_rate_reading.set_cutoff_frequency(0.05f * filt_freq); + filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * filt_freq); curr_test_mtr = {}; curr_test_tgt = {}; @@ -762,7 +799,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) float gyro_reading = 0.0f; float command_reading = 0.0f; float tgt_rate_reading = 0.0f; - float tgt_attitude; const uint32_t now = AP_HAL::millis(); float target_angle_cd = 0.0f; float dwell_freq = test_start_freq; @@ -772,9 +808,6 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) cycle_time_ms = 1000.0f * M_2PI / dwell_freq; } - //Determine target attitude magnitude limiting acceleration and rate - tgt_attitude = 5.0f * 0.01745f; - // body frame calculation of velocity Vector3f velocity_ned, velocity_bf; if (ahrs_view->get_velocity_NED(velocity_ned)) { @@ -1059,6 +1092,10 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis) // 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 = constrain_float(sweep_tgt.maxgain.freq, min_sweep_freq, max_sweep_freq); + freq_max = next_test_freq; + sp_prev_gain = sweep_tgt.maxgain.gain; + phase_max = sweep_tgt.maxgain.phase; + found_max_gain_freq = true; } else { next_test_freq = min_sweep_freq; } @@ -1173,25 +1210,35 @@ void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis) // FF is adjusted until rate requested is achieved void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq) { - float test_freq_incr = 0.05f * M_2PI; + float tune_tgt = 0.95; + float tune_tol = 0.025; next_freq = test_data.freq; - if (test_data.phase > 15.0f) { - next_freq = constrain_float(next_freq - test_freq_incr, 0.1 * M_2PI, max_sweep_freq); - } else if (test_data.phase < 0.0f) { - next_freq = constrain_float(next_freq + test_freq_incr, 0.1 * M_2PI, max_sweep_freq); - } else { - if ((test_data.gain > 0.1 && test_data.gain < 0.93) || test_data.gain > 0.98) { - if (tune_ff > 0.0f) { - tune_ff = 0.95f * tune_ff / test_data.gain; - } else { - tune_ff = 0.03f; - } - } else if (test_data.gain >= 0.93 && test_data.gain <= 0.98) { - counter = AUTOTUNE_SUCCESS_COUNT; - // reset next_freq for next test - next_freq = 0.0f; - tune_ff = constrain_float(tune_ff,0.0f,1.0f); + + // handle axes where FF gain is initially zero + if (test_data.gain < tune_tgt - tune_tol && !is_positive(tune_ff)) { + tune_ff = 0.03f; + return; + } + + if (test_data.gain < tune_tgt - 0.2 || test_data.gain > tune_tgt + 0.2) { + tune_ff = tune_ff * constrain_float(tune_tgt / test_data.gain, 0.75, 1.25); //keep changes less than 25% + } else if (test_data.gain < tune_tgt - 0.1 || test_data.gain > tune_tgt + 0.1) { + if (test_data.gain < tune_tgt - 0.1) { + tune_ff *= 1.05; + } else { + tune_ff *= 0.95; } + } else if (test_data.gain < tune_tgt - tune_tol || test_data.gain > tune_tgt + tune_tol) { + if (test_data.gain < tune_tgt - tune_tol) { + tune_ff *= 1.02; + } else { + tune_ff *= 0.98; + } + } else if (test_data.gain >= tune_tgt - tune_tol && test_data.gain <= tune_tgt + tune_tol) { + counter = AUTOTUNE_SUCCESS_COUNT; + // reset next_freq for next test + next_freq = 0.0f; + tune_ff = constrain_float(tune_ff,0.0f,1.0f); } } @@ -1276,7 +1323,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, next_freq = test_data.freq + test_freq_incr; return; // 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) { + } else if (test_data.gain < 0.95f * sp_prev_gain) { found_max_gain_freq = true; next_freq = freq_max + 0.5 * test_freq_incr; return; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 8970dd644b..269d3047c1 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -50,7 +50,7 @@ protected: void backup_gains_and_initialise() override; // load gains - void load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax); + void load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax, float max_rate); // switch to use original gains void load_orig_gains() override; @@ -176,7 +176,7 @@ private: float angle_lim_neg_rpy_cd() const override; // initialize dwell test or angle dwell test variables - void dwell_test_init(float start_frq, float stop_frq, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type); + void dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type); // dwell test used to perform frequency dwells for rate gains void dwell_test_run(sweep_info &test_data); @@ -248,6 +248,7 @@ private: FreqRespInput test_freq_resp_input; uint8_t num_dwell_cycles; float test_start_freq; + float tgt_attitude; float pre_calc_cycles; // number of cycles to complete before running frequency response calculations float command_out; // test axis command output