From a39040d864900998afc089e05919ef1e9981c64f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Dec 2021 13:38:01 +1100 Subject: [PATCH] AC_AutoTune: limit autotune target rates to parameter limits this makes it safer to autotune a large vehicle which may become unstable at very high target rates --- libraries/AC_AutoTune/AC_AutoTune.cpp | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 4e6ec92b31..ccdc47800a 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -527,8 +527,12 @@ void AC_AutoTune::control_attitude() float target_max_rate; switch (axis) { - case ROLL: + case ROLL: { target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + const float roll_rate_max_cds = degrees(attitude_control->get_ang_vel_roll_max_rads()) * 100; + if (is_positive(roll_rate_max_cds)) { + target_max_rate = MIN(target_max_rate, roll_rate_max_cds); + } target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD; @@ -536,8 +540,13 @@ void AC_AutoTune::control_attitude() start_angle = ahrs_view->roll_sensor; rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f); break; - case PITCH: + } + case PITCH: { target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + const float pitch_rate_max_cds = degrees(attitude_control->get_ang_vel_pitch_max_rads()) * 100; + if (is_positive(pitch_rate_max_cds)) { + target_max_rate = MIN(target_max_rate, pitch_rate_max_cds); + } target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD; @@ -545,8 +554,13 @@ void AC_AutoTune::control_attitude() start_angle = ahrs_view->pitch_sensor; rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f); break; - case YAW: + } + case YAW: { target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS); + const float yaw_rate_max_cds = degrees(attitude_control->get_ang_vel_yaw_max_rads()) * 100; + if (is_positive(yaw_rate_max_cds)) { + target_max_rate = MIN(target_max_rate, yaw_rate_max_cds); + } target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD; @@ -555,6 +569,7 @@ void AC_AutoTune::control_attitude() rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ); break; } + } if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { rotation_rate_filt.reset(start_rate); } else {