/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* support for autotune of helicopters. Based on original autotune code from ArduCopter, written by Leonard Hall Converted to a library by Andrew Tridgell, and rewritten to include helicopters by Bill Geyer */ #include "AC_AutoTune_config.h" #if AC_AUTOTUNE_ENABLED #include "AC_AutoTune_Heli.h" #include #include #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 5000U // timeout for tuning mode's testing step #define AUTOTUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term #define AUTOTUNE_RP_STEP 0.005f // minimum increment when increasing/decreasing Rate P term #define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term #define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing #define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing #define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing #define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value #define AUTOTUNE_RP_MIN 0.02f // minimum Rate P value #define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value #define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value #define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value #define AUTOTUNE_RFF_MAX 0.5f // maximum Stab P value #define AUTOTUNE_RFF_MIN 0.025f // maximum Stab P value #define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning #define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning #define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration #define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration #define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD 1500 // target roll/pitch angle during AUTOTUNE FeedForward rate test #define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS 5000 // target roll/pitch rate during AUTOTUNE FeedForward rate test #define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing #define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after testing #define AUTOTUNE_RP_ACCEL_MIN 20000.0f // Minimum acceleration for Roll and Pitch #define AUTOTUNE_Y_ACCEL_MIN 10000.0f // Minimum acceleration for Yaw #define AUTOTUNE_SEQ_BITMASK_VFF 1 #define AUTOTUNE_SEQ_BITMASK_RATE_D 2 #define AUTOTUNE_SEQ_BITMASK_ANGLE_P 4 #define AUTOTUNE_SEQ_BITMASK_MAX_GAIN 8 #define AUTOTUNE_SEQ_BITMASK_TUNE_CHECK 16 // angle limits preserved from previous behaviour as Multi changed: #define AUTOTUNE_ANGLE_TARGET_MAX_RP_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_ANGLE_TARGET_MIN_RP_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_ANGLE_TARGET_MAX_Y_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_ANGLE_TARGET_MIN_Y_CD 500 // target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_ANGLE_MAX_RP_CD 3000 // maximum allowable angle in degrees during testing #define AUTOTUNE_ANGLE_NEG_RPY_CD 1000 // maximum allowable angle in degrees during testing const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = { // @Param: AXES // @DisplayName: Autotune axis bitmask // @Description: 1-byte bitmap of axes to autotune // @Bitmask: 0:Roll,1:Pitch,2:Yaw // @User: Standard AP_GROUPINFO("AXES", 1, AC_AutoTune_Heli, axis_bitmask, 1), // @Param: SEQ // @DisplayName: AutoTune Sequence Bitmask // @Description: 2-byte bitmask to select what tuning should be performed. Max gain automatically performed if Rate D is selected. Values: 7:All,1:VFF Only,2:Rate D/Rate P Only(incl max gain),4:Angle P Only,8:Max Gain Only,16:Tune Check,3:VFF and Rate D/Rate P(incl max gain),5:VFF and Angle P,6:Rate D/Rate P(incl max gain) and angle P // @Bitmask: 0:VFF,1:Rate D/Rate P(incl max gain),2:Angle P,3:Max Gain Only,4:Tune Check // @User: Standard AP_GROUPINFO("SEQ", 2, AC_AutoTune_Heli, seq_bitmask, 3), // @Param: FRQ_MIN // @DisplayName: AutoTune minimum sweep frequency // @Description: Defines the start frequency for sweeps and dwells // @Range: 10 30 // @User: Standard AP_GROUPINFO("FRQ_MIN", 3, AC_AutoTune_Heli, min_sweep_freq, 10.0f), // @Param: FRQ_MAX // @DisplayName: AutoTune maximum sweep frequency // @Description: Defines the end frequency for sweeps and dwells // @Range: 50 120 // @User: Standard AP_GROUPINFO("FRQ_MAX", 4, AC_AutoTune_Heli, max_sweep_freq, 70.0f), // @Param: GN_MAX // @DisplayName: AutoTune maximum response gain // @Description: Defines the response gain (output/input) to tune // @Range: 1 2.5 // @User: Standard AP_GROUPINFO("GN_MAX", 5, AC_AutoTune_Heli, max_resp_gain, 1.0f), // @Param: VELXY_P // @DisplayName: AutoTune velocity xy P gain // @Description: Velocity xy P gain used to hold position during Max Gain, Rate P, and Rate D frequency sweeps // @Range: 0 1 // @User: Standard AP_GROUPINFO("VELXY_P", 6, AC_AutoTune_Heli, vel_hold_gain, 0.1f), // @Param: ACC_MAX // @DisplayName: AutoTune maximum allowable angular acceleration // @Description: maximum angular acceleration in deg/s/s allowed during autotune maneuvers // @Range: 1 4000 // @User: Standard AP_GROUPINFO("ACC_MAX", 7, AC_AutoTune_Heli, accel_max, 0.0f), // @Param: RAT_MAX // @DisplayName: Autotune maximum allowable angular rate // @Description: maximum angular rate in deg/s allowed during autotune maneuvers // @Range: 0 500 // @User: Standard AP_GROUPINFO("RAT_MAX", 8, AC_AutoTune_Heli, rate_max, 0.0f), AP_GROUPEND }; // constructor AC_AutoTune_Heli::AC_AutoTune_Heli() { tune_seq[0] = TUNE_COMPLETE; AP_Param::setup_object_defaults(this, var_info); } // initialize tests for each tune type 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)) { start_freq = 0.25f * M_2PI; } else { start_freq = next_test_freq; } stop_freq = start_freq; filter_freq = start_freq; attitude_control->bf_feedforward(false); // variables needed to initialize frequency response object and test method resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; calc_type = RATE; freq_resp_input = TARGET; pre_calc_cycles = 1.0f; num_dwell_cycles = 3; break; case MAX_GAINS: // initialize start frequency for sweep if (!is_positive(next_test_freq)) { start_freq = min_sweep_freq; stop_freq = max_sweep_freq; sweep_complete = true; } else { start_freq = next_test_freq; stop_freq = start_freq; test_accel_max = 0.0f; } filter_freq = start_freq; attitude_control->bf_feedforward(false); // variables needed to initialize frequency response object and test method resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; calc_type = RATE; freq_resp_input = MOTOR; pre_calc_cycles = 6.25f; num_dwell_cycles = 6; break; case RP_UP: case RD_UP: // initialize start frequency if (!is_positive(next_test_freq)) { // continue using frequency where testing left off with RD_UP completed 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)) { start_freq = sweep_tgt.ph180.freq; // otherwise start at min freq to step up in dwell frequency until phase > 160 deg } else { start_freq = min_sweep_freq; } } else { start_freq = next_test_freq; } stop_freq = start_freq; filter_freq = start_freq; attitude_control->bf_feedforward(false); // variables needed to initialize frequency response object and test method resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; calc_type = RATE; freq_resp_input = TARGET; pre_calc_cycles = 6.25f; num_dwell_cycles = 6; break; case SP_UP: // initialize start frequency for sweep if (!is_positive(next_test_freq)) { start_freq = min_sweep_freq; stop_freq = max_sweep_freq; sweep_complete = true; } else { start_freq = next_test_freq; stop_freq = start_freq; test_accel_max = 0.0f; } filter_freq = start_freq; attitude_control->bf_feedforward(false); // variables needed to initialize frequency response object and test method resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE; calc_type = DRB; freq_resp_input = TARGET; pre_calc_cycles = 6.25f; num_dwell_cycles = 6; break; case TUNE_CHECK: // initialize start frequency 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; calc_type = ANGLE; freq_resp_input = TARGET; break; default: break; } if (!is_equal(start_freq,stop_freq)) { input_type = AC_AutoTune_FreqResp::InputType::SWEEP; } else { input_type = AC_AutoTune_FreqResp::InputType::DWELL; } // initialize dwell test method 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 } // run tests for each tune type void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) { // if tune complete or beyond frequency range or no max allowed gains then exit testing if (tune_type == TUNE_COMPLETE || ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) || ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq))){ load_gains(GAIN_ORIGINAL); attitude_control->use_sqrt_controller(true); get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd); // hold level attitude attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true); if ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed"); mode = FAILED; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq)){ gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range"); mode = FAILED; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if (tune_type == TUNE_COMPLETE) { counter = AUTOTUNE_SUCCESS_COUNT; step = UPDATE_GAINS; } return; } dwell_test_run(curr_data); } // heli specific gcs announcements void AC_AutoTune_Heli::do_gcs_announcements() { const uint32_t now = AP_HAL::millis(); if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { return; } gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: %s %s", axis_string(), type_string()); send_step_string(); switch (tune_type) { case RFF_UP: case RD_UP: case RP_UP: case MAX_GAINS: case SP_UP: case TUNE_CHECK: if (is_equal(start_freq,stop_freq)) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Dwell"); } else { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Sweep"); 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)); } } break; default: break; } announce_time = now; } // send post test updates to user void AC_AutoTune_Heli::do_post_test_gcs_announcements() { float tune_rp = 0.0f; float tune_rd = 0.0f; float tune_rff = 0.0f; float tune_sp = 0.0f; float tune_accel = 0.0f; switch (axis) { case AxisType::ROLL: tune_rp = tune_roll_rp; tune_rd = tune_roll_rd; tune_rff = tune_roll_rff; tune_sp = tune_roll_sp; tune_accel = tune_roll_accel; break; case AxisType::PITCH: tune_rp = tune_pitch_rp; tune_rd = tune_pitch_rd; tune_rff = tune_pitch_rff; tune_sp = tune_pitch_sp; tune_accel = tune_pitch_accel; break; case AxisType::YAW: case AxisType::YAW_D: tune_rp = tune_yaw_rp; tune_rd = tune_yaw_rd; tune_rff = tune_yaw_rff; tune_sp = tune_yaw_sp; tune_accel = tune_yaw_accel; break; } if (step == UPDATE_GAINS) { switch (tune_type) { case RFF_UP: case RP_UP: case RD_UP: case SP_UP: case MAX_GAINS: if (is_equal(start_freq,stop_freq)) { // announce results of dwell gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(curr_data.freq), (double)(curr_data.gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(curr_data.phase)); if (tune_type == RP_UP) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp)); } else if (tune_type == RD_UP) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_d=%f", (double)(tune_rd)); } else if (tune_type == RFF_UP) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff)); } else if (tune_type == SP_UP) { 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_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: break; } } } // backup_gains_and_initialise - store current gains as originals // called before tuning starts to backup original gains void AC_AutoTune_Heli::backup_gains_and_initialise() { AC_AutoTune::backup_gains_and_initialise(); // initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli next_test_freq = 0.0f; start_freq = 0.0f; stop_freq = 0.0f; orig_bf_feedforward = attitude_control->get_bf_feedforward(); // backup original pids and initialise tuned pid values orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); orig_roll_rff = attitude_control->get_rate_roll_pid().ff(); orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz(); 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(); tune_roll_sp = attitude_control->get_angle_roll_p().kP(); tune_roll_accel = attitude_control->get_accel_roll_max_cdss(); orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff(); orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz(); 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(); tune_pitch_sp = attitude_control->get_angle_pitch_p().kP(); tune_pitch_accel = attitude_control->get_accel_pitch_max_cdss(); orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff(); orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz(); orig_yaw_smax = attitude_control->get_rate_yaw_pid().slew_limit(); 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(); tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz(); tune_yaw_sp = attitude_control->get_angle_yaw_p().kP(); tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss(); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED); } // load_orig_gains - set gains to their original values // called by stop and failed functions void AC_AutoTune_Heli::load_orig_gains() { attitude_control->bf_feedforward(orig_bf_feedforward); if (roll_enabled()) { load_gain_set(AxisType::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(AxisType::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(AxisType::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); } } // load_tuned_gains - load tuned gains void AC_AutoTune_Heli::load_tuned_gains() { if (!attitude_control->get_bf_feedforward()) { attitude_control->bf_feedforward(true); attitude_control->set_accel_roll_max_cdss(0.0f); attitude_control->set_accel_pitch_max_cdss(0.0f); } if (roll_enabled()) { load_gain_set(AxisType::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(AxisType::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(AxisType::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); } } } // load_intra_test_gains - gains used between tests // called during testing mode's update-gains step to set gains ahead of return-to-level step void AC_AutoTune_Heli::load_intra_test_gains() { // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) // sanity check the gains attitude_control->bf_feedforward(true); if (roll_enabled()) { load_gain_set(AxisType::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(AxisType::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(AxisType::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); } } // load_test_gains - load the to-be-tested gains for a single axis // 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, rate_test_max, accel_test_max; switch (axis) { case AxisType::ROLL: if (tune_type == TUNE_CHECK) { rate_test_max = orig_roll_rate; accel_test_max = tune_roll_accel; } else { // have attitude controller use accel and rate limit parameter rate_test_max = rate_max; accel_test_max = accel_max; } if (tune_type == SP_UP || tune_type == TUNE_CHECK) { rate_i = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL; } else { // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_roll_ri; } if (tune_type == MAX_GAINS && !is_zero(tune_roll_rff)) { rate_p = 0.0f; rate_d = 0.0f; } else { rate_p = tune_roll_rp; rate_d = tune_roll_rd; } load_gain_set(AxisType::ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, accel_test_max, orig_roll_fltt, 0.0f, 0.0f, rate_test_max); break; case AxisType::PITCH: if (tune_type == TUNE_CHECK) { rate_test_max = orig_pitch_rate; accel_test_max = tune_pitch_accel; } else { // have attitude controller use accel and rate limit parameter rate_test_max = rate_max; accel_test_max = accel_max; } if (tune_type == SP_UP || tune_type == TUNE_CHECK) { rate_i = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL; } else { // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_pitch_ri; } if (tune_type == MAX_GAINS && !is_zero(tune_pitch_rff)) { rate_p = 0.0f; rate_d = 0.0f; } else { rate_p = tune_pitch_rp; rate_d = tune_pitch_rd; } load_gain_set(AxisType::PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, accel_test_max, orig_pitch_fltt, 0.0f, 0.0f, rate_test_max); break; case AxisType::YAW: case AxisType::YAW_D: if (tune_type == TUNE_CHECK) { rate_test_max = orig_yaw_rate; accel_test_max = tune_yaw_accel; } else { // have attitude controller use accel and rate limit parameter rate_test_max = rate_max; accel_test_max = accel_max; } if (tune_type == SP_UP || tune_type == TUNE_CHECK) { rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL; } else { // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_yaw_ri; } load_gain_set(AxisType::YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, accel_test_max, 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, float max_rate) { switch (s_axis) { case AxisType::ROLL: attitude_control->get_rate_roll_pid().set_kP(rate_p); attitude_control->get_rate_roll_pid().set_kI(rate_i); attitude_control->get_rate_roll_pid().set_kD(rate_d); attitude_control->get_rate_roll_pid().set_ff(rate_ff); attitude_control->get_rate_roll_pid().set_filt_T_hz(rate_fltt); attitude_control->get_rate_roll_pid().set_slew_limit(smax); attitude_control->get_angle_roll_p().set_kP(angle_p); attitude_control->set_accel_roll_max_cdss(max_accel); attitude_control->set_ang_vel_roll_max_degs(max_rate); break; case AxisType::PITCH: attitude_control->get_rate_pitch_pid().set_kP(rate_p); attitude_control->get_rate_pitch_pid().set_kI(rate_i); attitude_control->get_rate_pitch_pid().set_kD(rate_d); attitude_control->get_rate_pitch_pid().set_ff(rate_ff); attitude_control->get_rate_pitch_pid().set_filt_T_hz(rate_fltt); attitude_control->get_rate_pitch_pid().set_slew_limit(smax); attitude_control->get_angle_pitch_p().set_kP(angle_p); attitude_control->set_accel_pitch_max_cdss(max_accel); attitude_control->set_ang_vel_pitch_max_degs(max_rate); break; case AxisType::YAW: case AxisType::YAW_D: attitude_control->get_rate_yaw_pid().set_kP(rate_p); attitude_control->get_rate_yaw_pid().set_kI(rate_i); attitude_control->get_rate_yaw_pid().set_kD(rate_d); attitude_control->get_rate_yaw_pid().set_ff(rate_ff); attitude_control->get_rate_yaw_pid().set_filt_T_hz(rate_fltt); attitude_control->get_rate_yaw_pid().set_slew_limit(smax); attitude_control->get_rate_yaw_pid().set_filt_E_hz(rate_flte); attitude_control->get_angle_yaw_p().set_kP(angle_p); attitude_control->set_accel_yaw_max_cdss(max_accel); attitude_control->set_ang_vel_yaw_max_degs(max_rate); break; } } // save_tuning_gains - save the final tuned gains for each axis // save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position) void AC_AutoTune_Heli::save_tuning_gains() { // see if we successfully completed tuning of at least one axis if (axes_completed == 0) { return; } if (!attitude_control->get_bf_feedforward()) { attitude_control->bf_feedforward_save(true); attitude_control->save_accel_roll_max_cdss(0.0f); attitude_control->save_accel_pitch_max_cdss(0.0f); } // sanity check the rate P values if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) { load_gain_set(AxisType::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(); // save stabilize roll attitude_control->get_angle_roll_p().save_gains(); // resave pids to originals in case the autotune is run again orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); orig_roll_rff = attitude_control->get_rate_roll_pid().ff(); orig_roll_sp = attitude_control->get_angle_roll_p().kP(); orig_roll_accel = attitude_control->get_accel_roll_max_cdss(); } if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) { load_gain_set(AxisType::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(); // save stabilize pitch attitude_control->get_angle_pitch_p().save_gains(); // resave pids to originals in case the autotune is run again orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff(); orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); orig_pitch_accel = attitude_control->get_accel_pitch_max_cdss(); } if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) { load_gain_set(AxisType::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(); // save stabilize yaw attitude_control->get_angle_yaw_p().save_gains(); // resave pids to originals in case the autotune is run again orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff(); orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz(); orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); orig_yaw_accel = attitude_control->get_accel_yaw_max_cdss(); } // update GCS and log save gains event update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS); reset(); } // report final gains for a given axis to GCS void AC_AutoTune_Heli::report_final_gains(AxisType test_axis) const { switch (test_axis) { case AxisType::ROLL: report_axis_gains("Roll", tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel); break; case AxisType::PITCH: report_axis_gains("Pitch", tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel); break; case AxisType::YAW: case AxisType::YAW_D: report_axis_gains("Yaw", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel); break; } } // report gain formatting helper void AC_AutoTune_Heli::report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float rate_ff, float angle_P, float max_accel) const { gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string); gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.4f, I:%0.4f, D:%0.5f, FF:%0.4f",axis_string,rate_P,rate_I,rate_D,rate_ff); 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 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 = radians(amplitude); // initialize frequency response object if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { step_time_limit_ms = sweep_time_ms + 500; reset_sweep_variables(); curr_test.gain = 0.0f; curr_test.phase = 0.0f; chirp_input.init(0.001f * sweep_time_ms, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * sweep_time_ms, 0.0f); } else { if (!is_zero(start_frq)) { // time limit set by adding the pre calc cycles with the dwell cycles. 500 ms added to account for settling with buffer. step_time_limit_ms = (uint32_t) (2000 + ((float)num_dwell_cycles + pre_calc_cycles + 2.0f) * 1000.0f * M_2PI / start_frq); } chirp_input.init(0.001f * step_time_limit_ms, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * step_time_limit_ms, 0.0f); } freqresp_tgt.init(test_input_type, resp_type, num_dwell_cycles); freqresp_mtr.init(test_input_type, resp_type, num_dwell_cycles); dwell_start_time_ms = 0.0f; settle_time = 200; rotation_rate_filt.set_cutoff_frequency(filt_freq); command_filt.set_cutoff_frequency(filt_freq); target_rate_filt.set_cutoff_frequency(filt_freq); rotation_rate_filt.reset(0); command_filt.reset(0); target_rate_filt.reset(0); rotation_rate = 0.0f; command_out = 0.0f; filt_target_rate = 0.0f; // filter at lower frequency to remove steady state 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 = {}; cycle_complete_tgt = false; cycle_complete_mtr = false; sweep_complete = false; } 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; const uint32_t now = AP_HAL::millis(); float target_angle_cd = 0.0f; float dwell_freq = test_start_freq; float cycle_time_ms = 0; if (!is_zero(dwell_freq)) { cycle_time_ms = 1000.0f * M_2PI / dwell_freq; } // body frame calculation of velocity Vector3f velocity_ned, velocity_bf; if (ahrs_view->get_velocity_NED(velocity_ned)) { velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw(); velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw(); } if (settle_time == 0) { target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, degrees(tgt_attitude) * 100.0f); dwell_freq = chirp_input.get_frequency_rads(); const Vector2f att_fdbk { -5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x }; filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s()); } else { target_angle_cd = 0.0f; trim_yaw_tgt_reading_cd = (float)attitude_control->get_att_target_euler_cd().z; trim_yaw_heading_reading_cd = (float)ahrs_view->yaw_sensor; dwell_start_time_ms = now; filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f)); settle_time--; } const Vector2f trim_angle_cd { constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f), constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f) }; switch (axis) { case AxisType::ROLL: attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f); command_reading = motors->get_roll(); if (test_calc_type == DRB) { tgt_rate_reading = radians(target_angle_cd * 0.01f); gyro_reading = radians(((float)ahrs_view->roll_sensor + trim_angle_cd.x - target_angle_cd) * 0.01f); } else if (test_calc_type == RATE) { tgt_rate_reading = attitude_control->rate_bf_targets().x; gyro_reading = ahrs_view->get_gyro().x; } else { tgt_rate_reading = radians((float)attitude_control->get_att_target_euler_cd().x * 0.01f); gyro_reading = radians((float)ahrs_view->roll_sensor * 0.01f); } break; case AxisType::PITCH: attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(trim_angle_cd.x, target_angle_cd + trim_angle_cd.y, 0.0f); command_reading = motors->get_pitch(); if (test_calc_type == DRB) { tgt_rate_reading = radians(target_angle_cd * 0.01f); gyro_reading = radians(((float)ahrs_view->pitch_sensor + trim_angle_cd.y - target_angle_cd) * 0.01f); } else if (test_calc_type == RATE) { tgt_rate_reading = attitude_control->rate_bf_targets().y; gyro_reading = ahrs_view->get_gyro().y; } else { tgt_rate_reading = radians((float)attitude_control->get_att_target_euler_cd().y * 0.01f); gyro_reading = radians((float)ahrs_view->pitch_sensor * 0.01f); } break; case AxisType::YAW: case AxisType::YAW_D: attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading_cd + target_angle_cd), false); command_reading = motors->get_yaw(); if (test_calc_type == DRB) { tgt_rate_reading = radians(target_angle_cd * 0.01f); gyro_reading = radians((wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading_cd - target_angle_cd)) * 0.01f); } else if (test_calc_type == RATE) { tgt_rate_reading = attitude_control->rate_bf_targets().z; gyro_reading = ahrs_view->get_gyro().z; } else { tgt_rate_reading = radians((wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading_cd)) * 0.01f); gyro_reading = radians((wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading_cd)) * 0.01f); } break; } if (settle_time == 0) { filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s()); filt_gyro_reading.apply(gyro_reading, AP::scheduler().get_loop_period_s()); filt_tgt_rate_reading.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s()); } else { filt_command_reading.reset(command_reading); filt_gyro_reading.reset(gyro_reading); filt_tgt_rate_reading.reset(tgt_rate_reading); } // looks at gain and phase of input rate to output rate rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()), AP::scheduler().get_loop_period_s()); filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()), AP::scheduler().get_loop_period_s()); 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) > pre_calc_cycles * cycle_time_ms || (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && settle_time == 0)) { 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_mtr.is_cycle_complete()) { if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { if (is_zero(curr_test_mtr.freq) && freqresp_mtr.get_freq() < test_start_freq) { // don't set data since captured frequency is below the start frequency } else { 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_mtr.reset_cycle_complete(); #if HAL_LOGGING_ENABLED // log sweep data Log_AutoTuneSweep(); #endif } else { dwell_gain_mtr = freqresp_mtr.get_gain(); dwell_phase_mtr = freqresp_mtr.get_phase(); cycle_complete_mtr = true; } } if (freqresp_tgt.is_cycle_complete()) { if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { if (is_zero(curr_test_tgt.freq) && freqresp_tgt.get_freq() < test_start_freq) { // don't set data since captured frequency is below the start frequency } else { 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 (test_calc_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 (test_calc_type == DRB) {test_accel_max = freqresp_tgt.get_accel_max();} cycle_complete_tgt = true; } } if (test_freq_resp_input == TARGET) { if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { curr_test = curr_test_tgt; } else { test_data.freq = test_start_freq; test_data.gain = dwell_gain_tgt; test_data.phase = dwell_phase_tgt; } } else { if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { curr_test = curr_test_mtr; } else { test_data.freq = test_start_freq; test_data.gain = dwell_gain_mtr; test_data.phase = dwell_phase_mtr; } } } // set sweep data if a frequency sweep is being conducted if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && (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_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_tgt.phase <= 160.0f && curr_test_tgt.phase >= 150.0f && sweep_tgt.progress == 0) { sweep_tgt.ph180 = curr_test_tgt; } 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_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 sweep_complete = true; step = UPDATE_GAINS; } } else { if (now - step_start_time_ms >= step_time_limit_ms || (freqresp_tgt.is_cycle_complete() && freqresp_mtr.is_cycle_complete())) { if (now - step_start_time_ms >= step_time_limit_ms) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Step time limit exceeded"); } cycle_complete_tgt = false; cycle_complete_tgt = false; // we have passed the maximum stop time step = UPDATE_GAINS; } } } // update gains for the rate p up tune type void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: updating_rate_p_up(tune_roll_rp, curr_data, next_test_freq, max_rate_p); break; case AxisType::PITCH: updating_rate_p_up(tune_pitch_rp, curr_data, next_test_freq, max_rate_p); break; case AxisType::YAW: case AxisType::YAW_D: updating_rate_p_up(tune_yaw_rp, curr_data, next_test_freq, max_rate_p); break; } } // update gains for the rate d up tune type void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: updating_rate_d_up(tune_roll_rd, curr_data, next_test_freq, max_rate_d); break; case AxisType::PITCH: updating_rate_d_up(tune_pitch_rd, curr_data, next_test_freq, max_rate_d); break; case AxisType::YAW: case AxisType::YAW_D: updating_rate_d_up(tune_yaw_rd, curr_data, next_test_freq, max_rate_d); break; } } // update gains for the rate ff up tune type void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: updating_rate_ff_up(tune_roll_rff, curr_data, next_test_freq); break; case AxisType::PITCH: updating_rate_ff_up(tune_pitch_rff, curr_data, next_test_freq); break; case AxisType::YAW: case AxisType::YAW_D: updating_rate_ff_up(tune_yaw_rff, curr_data, next_test_freq); // TODO make FF updating routine determine when to set rff gain to zero based on A/C response if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && counter == AUTOTUNE_SUCCESS_COUNT) { tune_yaw_rff = 0.0f; } break; } } // update gains for the angle p up tune type void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis) { attitude_control->bf_feedforward(orig_bf_feedforward); // sweep doesn't require gain update so return immediately after setting next test freq // determine next_test_freq for dwell testing if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP){ // 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; } return; } switch (test_axis) { case AxisType::ROLL: updating_angle_p_up(tune_roll_sp, curr_data, next_test_freq); break; case AxisType::PITCH: updating_angle_p_up(tune_pitch_sp, curr_data, next_test_freq); break; case AxisType::YAW: case AxisType::YAW_D: updating_angle_p_up(tune_yaw_sp, curr_data, next_test_freq); break; } } // update gains for the max gain tune type void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis) { // sweep doesn't require gain update so return immediately after setting next test freq // determine next_test_freq for dwell testing if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { // 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_mtr.ph180.freq)) { next_test_freq = constrain_float(sweep_mtr.ph180.freq, min_sweep_freq, max_sweep_freq); reset_maxgains_update_gain_variables(); } else { next_test_freq = min_sweep_freq; } return; } switch (test_axis) { case AxisType::ROLL: updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd); break; case AxisType::PITCH: updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_pitch_rp, tune_pitch_rd); break; case AxisType::YAW: case AxisType::YAW_D: updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd); // rate P and rate D can be non zero for yaw and need to be included in the max allowed gain if (!is_zero(max_rate_p.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) { max_rate_p.max_allowed += tune_yaw_rp; } if (!is_zero(max_rate_d.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) { max_rate_d.max_allowed += tune_yaw_rd; } break; } } // set gains post tune for the tune type void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis) { switch (tune_type) { case RD_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_rd = MAX(0.0f, tune_roll_rd * AUTOTUNE_RD_BACKOFF); break; case AxisType::PITCH: tune_pitch_rd = MAX(0.0f, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); break; case AxisType::YAW: case AxisType::YAW_D: tune_yaw_rd = MAX(0.0f, tune_yaw_rd * AUTOTUNE_RD_BACKOFF); break; } break; case RP_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_rp = MAX(0.0f, tune_roll_rp * AUTOTUNE_RP_BACKOFF); break; case AxisType::PITCH: tune_pitch_rp = MAX(0.0f, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); break; case AxisType::YAW: case AxisType::YAW_D: tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); break; } break; case SP_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); break; case AxisType::PITCH: tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); break; case AxisType::YAW: case AxisType::YAW_D: tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); break; } break; case RFF_UP: break; default: break; } } // updating_rate_ff_up - adjust FF to ensure the target is reached // 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 tune_tgt = 0.95; float tune_tol = 0.025; next_freq = test_data.freq; // 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); } } // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not // exceed max response gain. A phase of 161 deg is used to conduct the tuning as this phase is where analytically // max gain to 6db gain margin is determined for a unity feedback controller. 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 * M_2PI; next_freq = test_data.freq; sweep_info data_at_ph161; float sugg_freq; if (freq_search_for_phase(test_data, 161.0f, test_freq_incr, data_at_ph161, sugg_freq)) { if (data_at_ph161.gain < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) { tune_p += 0.05f * max_gain_p.max_allowed; next_freq = data_at_ph161.freq; } 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 { next_freq = sugg_freq; } } // 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. A phase of 161 deg is used to conduct the tuning as this phase is where analytically // max gain to 6db gain margin is determined for a unity feedback controller. 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 * M_2PI; // set for 1/4 hz increments next_freq = test_data.freq; sweep_info data_at_ph161; float sugg_freq; if (freq_search_for_phase(test_data, 161.0f, test_freq_incr, data_at_ph161, sugg_freq)) { if ((data_at_ph161.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 = data_at_ph161.gain; next_freq = data_at_ph161.freq; } else { counter = AUTOTUNE_SUCCESS_COUNT; // 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); } } else { next_freq = sugg_freq; } } // updating_angle_p_up - determines maximum angle p gain for pitch and roll. This is accomplished by determining the frequency // for the maximum response gain that is the disturbance rejection peak. void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq) { float test_freq_incr = 0.5f * M_2PI; float gain_incr = 0.5f; 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 (!found_max_gain_freq) { if (test_data.gain > 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 next_freq = test_data.freq; return; } else if (test_data.gain > 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 (test_data.gain > sp_prev_gain) { freq_max = test_data.freq; phase_max = test_data.phase; sp_prev_gain = test_data.gain; 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.95f * sp_prev_gain) { found_max_gain_freq = true; next_freq = freq_max + 0.5 * test_freq_incr; return; } else { next_freq = test_data.freq + test_freq_incr; return; } } // refine peak if (!found_peak) { // look at frequency above max gain freq found if (test_data.freq > freq_max && test_data.gain > sp_prev_gain) { // found max at frequency greater than initial max gain frequency found_peak = true; } else if (test_data.freq > freq_max && test_data.gain < sp_prev_gain) { // look at frequency below initial max gain frequency next_freq = test_data.freq - 0.5 * test_freq_incr; return; } else if (test_data.freq < freq_max && test_data.gain > sp_prev_gain) { // found max at frequency less than initial max gain frequency found_peak = true; } else { found_peak = true; test_data.freq = freq_max; test_data.gain = sp_prev_gain; } sp_prev_gain = test_data.gain; } // start increasing gain if (found_max_gain_freq && found_peak) { if (test_data.gain < max_resp_gain && tune_p < AUTOTUNE_SP_MAX) { tune_p += gain_incr; next_freq = test_data.freq; if (tune_p >= AUTOTUNE_SP_MAX) { tune_p = AUTOTUNE_SP_MAX; counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } sp_prev_gain = test_data.gain; } else if (test_data.gain > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { tune_p -= gain_incr; } else { // adjust tuning gain so max response gain is not exceeded if (sp_prev_gain < max_resp_gain && test_data.gain > max_resp_gain) { float adj_factor = (max_resp_gain - test_data.gain) / (test_data.gain - sp_prev_gain); tune_p = tune_p + gain_incr * adj_factor; } counter = AUTOTUNE_SUCCESS_COUNT; } } if (counter == AUTOTUNE_SUCCESS_COUNT) { next_freq = 0.0f; //initializes next test that uses dwell test sweep_complete = false; reset_sweep_variables(); } } // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur. This uses the frequency // response of motor class input to rate response to determine the max allowable gain for rate P gain. A phase of 161 deg is used to // determine analytically the max gain to 6db gain margin for a unity feedback controller. Since acceleration can be more noisy, the // response of the motor class input to rate response to determine the max allowable gain for rate D gain. A phase of 251 deg is used // to determine analytically the max gain to 6db gain margin for a unity feedback controller. void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d) { float test_freq_incr = 0.5f * M_2PI; next_freq = test_data.freq; sweep_info data_at_phase; float sugg_freq; if (!found_max_p) { if (freq_search_for_phase(test_data, 161.0f, test_freq_incr, data_at_phase, sugg_freq)) { max_gain_p.freq = data_at_phase.freq; max_gain_p.gain = data_at_phase.gain; max_gain_p.phase = data_at_phase.phase; max_gain_p.max_allowed = powf(10.0f,-1 * (log10f(max_gain_p.gain) * 20.0f + 2.42) / 20.0f); // limit max gain allowed to be no more than 2x the max p gain limit to keep initial gains bounded max_gain_p.max_allowed = constrain_float(max_gain_p.max_allowed, 0.0f, 2.0f * AUTOTUNE_RP_MAX); found_max_p = true; if (!is_zero(sweep_mtr.ph270.freq)) { next_freq = sweep_mtr.ph270.freq; } else { next_freq = data_at_phase.freq; } } else { next_freq = sugg_freq; } } else if (!found_max_d) { if (freq_search_for_phase(test_data, 251.0f, test_freq_incr, data_at_phase, sugg_freq)) { max_gain_d.freq = data_at_phase.freq; max_gain_d.gain = data_at_phase.gain; max_gain_d.phase = data_at_phase.phase; max_gain_d.max_allowed = powf(10.0f,-1 * (log10f(max_gain_d.freq * max_gain_d.gain) * 20.0f + 2.42) / 20.0f); // limit max gain allowed to be no more than 2x the max d gain limit to keep initial gains bounded max_gain_d.max_allowed = constrain_float(max_gain_d.max_allowed, 0.0f, 2.0f * AUTOTUNE_RD_MAX); found_max_d = true; } else { next_freq = sugg_freq; } } if (found_max_p && found_max_d) { counter = AUTOTUNE_SUCCESS_COUNT; // reset variables for next test next_freq = 0.0f; //initializes next test that uses dwell test gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(max_rate_p.phase), (double)(max_rate_p.max_allowed)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f", (double)(max_rate_d.freq), (double)(max_rate_d.gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(max_rate_d.phase), (double)(max_rate_d.max_allowed)); } } float AC_AutoTune_Heli::target_angle_max_rp_cd() const { return AUTOTUNE_ANGLE_TARGET_MAX_RP_CD; } float AC_AutoTune_Heli::target_angle_max_y_cd() const { return AUTOTUNE_ANGLE_TARGET_MAX_Y_CD; } float AC_AutoTune_Heli::target_angle_min_rp_cd() const { return AUTOTUNE_ANGLE_TARGET_MIN_RP_CD; } float AC_AutoTune_Heli::target_angle_min_y_cd() const { return AUTOTUNE_ANGLE_TARGET_MIN_Y_CD; } float AC_AutoTune_Heli::angle_lim_max_rp_cd() const { return AUTOTUNE_ANGLE_MAX_RP_CD; } float AC_AutoTune_Heli::angle_lim_neg_rpy_cd() const { return AUTOTUNE_ANGLE_NEG_RPY_CD; } // freq_search_for_phase: general search strategy for specified phase. interpolation done once specified phase has been bounded. bool AC_AutoTune_Heli::freq_search_for_phase(sweep_info test, float desired_phase, float freq_incr, sweep_info &est_data, float &new_freq) { new_freq = test.freq; float phase_delta = 20.0f; // delta from desired phase below and above which full steps are taken if (is_zero(test.phase)) { // bad test point. increase slightly in hope of getting better result new_freq += 0.1f * freq_incr; return false; } // test to see if desired phase is bounded with a 0.5 freq_incr delta in freq float freq_delta = fabsf(prev_test.freq - test.freq); if (test.phase > desired_phase && prev_test.phase < desired_phase && freq_delta < 0.75f * freq_incr && is_positive(prev_test.freq)) { est_data.freq = linear_interpolate(prev_test.freq,test.freq,desired_phase,prev_test.phase,test.phase); est_data.gain = linear_interpolate(prev_test.gain,test.gain,desired_phase,prev_test.phase,test.phase); est_data.phase = desired_phase; prev_test = {}; return true; } else if (test.phase < desired_phase && prev_test.phase > desired_phase && freq_delta < 0.75f * freq_incr && is_positive(prev_test.freq)) { est_data.freq = linear_interpolate(test.freq,prev_test.freq,desired_phase,test.phase,prev_test.phase); est_data.gain = linear_interpolate(test.gain,prev_test.gain,desired_phase,test.phase,prev_test.phase); est_data.phase = desired_phase; prev_test = {}; return true; } if (test.phase < desired_phase - phase_delta) { new_freq += freq_incr; } else if (test.phase > desired_phase + phase_delta) { new_freq -= freq_incr; } else if (test.phase >= desired_phase - phase_delta && test.phase < desired_phase) { new_freq += 0.5f * freq_incr; } else if (test.phase <= desired_phase + phase_delta && test.phase >= desired_phase) { new_freq -= 0.5f * freq_incr; } prev_test = test; return false; } #if HAL_LOGGING_ENABLED // log autotune summary data void AC_AutoTune_Heli::Log_AutoTune() { switch (axis) { case AxisType::ROLL: Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); break; case AxisType::PITCH: Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); break; case AxisType::YAW: case AxisType::YAW_D: Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max); break; } } // log autotune time history results for command, angular rate, and attitude void AC_AutoTune_Heli::Log_AutoTuneDetails() { if (tune_type == SP_UP || tune_type == TUNE_CHECK) { Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate); } else { Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f); } } // log autotune frequency response data void AC_AutoTune_Heli::Log_AutoTuneSweep() { 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 // @Description: Heli AutoTune // @Vehicles: Copter // @Field: TimeUS: Time since system startup // @Field: Axis: which axis is currently being tuned // @Field: TuneStep: step in autotune process // @Field: Freq: target dwell frequency // @Field: Gain: measured gain of dwell // @Field: Phase: measured phase of dwell // @Field: RFF: new rate gain FF term // @Field: RP: new rate gain P term // @Field: RD: new rate gain D term // @Field: SP: new angle P term // @Field: ACC: new max accel term // Write an Autotune summary data packet void AC_AutoTune_Heli::Log_Write_AutoTune(AxisType _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel) { AP::logger().Write( "ATNH", "TimeUS,Axis,TuneStep,Freq,Gain,Phase,RFF,RP,RD,SP,ACC", "s--E-d-----", "F--000-----", "QBBffffffff", AP_HAL::micros64(), (uint8_t)axis, tune_step, dwell_freq, meas_gain, meas_phase, new_gain_rff, new_gain_rp, new_gain_rd, new_gain_sp, max_accel); } // Write an Autotune detailed data packet void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad) { // @LoggerMessage: ATDH // @Description: Heli AutoTune data packet // @Vehicles: Copter // @Field: TimeUS: Time since system startup // @Field: Cmd: current motor command // @Field: TRate: current target angular rate // @Field: Rate: current angular rate // @Field: TAng: current target angle // @Field: Ang: current angle AP::logger().WriteStreaming( "ATDH", "TimeUS,Cmd,TRate,Rate,TAng,Ang", "s-kkdd", "F00000", "Qfffff", AP_HAL::micros64(), motor_cmd, tgt_rate_rads*57.3, rate_rads*57.3f, tgt_ang_rad*57.3, ang_rad*57.3f); } // Write an Autotune frequency response data packet 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_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_m,gain_m,phase_m,freq_t,gain_t,phase_t", "sE-dE-d", "F000000", "Qffffff", AP_HAL::micros64(), freq_mtr, gain_mtr, phase_mtr, freq_tgt, gain_tgt, phase_tgt); } #endif // HAL_LOGGING_ENABLED // reset the test variables for each vehicle void AC_AutoTune_Heli::reset_vehicle_test_variables() { // reset dwell test variables if sweep was interrupted in order to restart sweep if (!is_equal(start_freq, stop_freq)) { start_freq = 0.0f; stop_freq = 0.0f; next_test_freq = 0.0f; sweep_complete = false; } } // reset the update gain variables for heli void AC_AutoTune_Heli::reset_update_gain_variables() { // reset max gain variables reset_maxgains_update_gain_variables(); // reset rd_up variables rd_prev_gain = 0.0f; // reset sp_up variables phase_max = 0.0f; freq_max = 0.0f; sp_prev_gain = 0.0f; found_max_gain_freq = false; found_peak = false; } // reset the max_gains update gain variables void AC_AutoTune_Heli::reset_maxgains_update_gain_variables() { max_rate_p = {}; max_rate_d = {}; found_max_p = false; found_max_d = false; } // reset the max_gains update gain variables void AC_AutoTune_Heli::reset_sweep_variables() { 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; } // set the tuning test sequence void AC_AutoTune_Heli::set_tune_sequence() { uint8_t seq_cnt = 0; if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_VFF) { tune_seq[seq_cnt] = RFF_UP; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D) { tune_seq[seq_cnt] = MAX_GAINS; seq_cnt++; tune_seq[seq_cnt] = RD_UP; seq_cnt++; tune_seq[seq_cnt] = RP_UP; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_ANGLE_P) { tune_seq[seq_cnt] = SP_UP; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_MAX_GAIN && !(seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D)) { tune_seq[seq_cnt] = MAX_GAINS; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_TUNE_CHECK) { tune_seq[seq_cnt] = TUNE_CHECK; seq_cnt++; } tune_seq[seq_cnt] = TUNE_COMPLETE; } // get_testing_step_timeout_ms accessor uint32_t AC_AutoTune_Heli::get_testing_step_timeout_ms() const { return AUTOTUNE_TESTING_STEP_TIMEOUT_MS; } // exceeded_freq_range - ensures tuning remains inside frequency range bool AC_AutoTune_Heli::exceeded_freq_range(float frequency) { bool ret = false; if (frequency < min_sweep_freq || frequency > max_sweep_freq) { ret = true; } return ret; } #endif // AC_AUTOTUNE_ENABLED