From fafa0bfd67732dd3765c388fe36707f3dee11276 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 6 Jun 2024 23:23:58 -0400 Subject: [PATCH] AC_AutoTune: rate and accel limits --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 84 +++++++++++++++------- 1 file changed, 57 insertions(+), 27 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 604b767f44b552..da2c06e996c49e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -109,14 +109,14 @@ const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = { // @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, 5000.0f), + 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, 50.0f), + AP_GROUPINFO("RAT_MAX", 8, AC_AutoTune_Heli, rate_max, 0.0f), AP_GROUPEND }; @@ -519,11 +519,21 @@ 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, rate_test_max; + float rate_p, rate_i, rate_d, rate_test_max, accel_test_max; switch (axis) { case ROLL: + rate_test_max = orig_roll_rate; + accel_test_max = tune_roll_accel; if (tune_type == SP_UP || tune_type == TUNE_CHECK) { rate_i = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL; + // have attitude controller use rate limit if non-zero + if (is_positive(rate_max)) { + rate_test_max = rate_max; + } + // have attitude controller use accel limit if non-zero + if (is_positive(accel_max)) { + accel_test_max = accel_max; + } } else { // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_roll_ri; @@ -535,18 +545,21 @@ void AC_AutoTune_Heli::load_test_gains() rate_p = tune_roll_rp; rate_d = tune_roll_rd; } -/* 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); + load_gain_set(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 PITCH: + rate_test_max = orig_pitch_rate; + accel_test_max = tune_pitch_accel; if (tune_type == SP_UP || tune_type == TUNE_CHECK) { rate_i = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL; + // have attitude controller use rate limit if non-zero + if (is_positive(rate_max)) { + rate_test_max = rate_max; + } + // have attitude controller use accel limit if non-zero + if (is_positive(accel_max)) { + accel_test_max = accel_max; + } } else { // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_pitch_ri; @@ -558,31 +571,27 @@ void AC_AutoTune_Heli::load_test_gains() rate_p = tune_pitch_rp; rate_d = tune_pitch_rd; } -/* 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); + load_gain_set(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 YAW: case YAW_D: + rate_test_max = orig_yaw_rate; + accel_test_max = tune_yaw_accel; if (tune_type == SP_UP || tune_type == TUNE_CHECK) { rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL; + // have attitude controller use rate limit if non-zero + if (is_positive(rate_max)) { + rate_test_max = rate_max; + } + // have attitude controller use accel limit if non-zero + if (is_positive(accel_max)) { + accel_test_max = accel_max; + } } else { // freeze integrator to hold trim by making i term small during rate controller tuning rate_i = 0.01f * orig_yaw_ri; } -/* 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); + load_gain_set(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; } } @@ -810,6 +819,27 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) if (settle_time == 0) { target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, tgt_attitude * 5730.0f); dwell_freq = chirp_input.get_frequency_rads(); + // limit requested angle based on calculated max accel and max rate if non zero parameters are set + if (test_calc_type == RATE) { + // apply rate limit for RFF, RP_UP and RD_UP test types + float peak_rate = dwell_freq * tgt_attitude; + float peak_accel = sq(dwell_freq) * tgt_attitude; + float rate_knockdown = 1.0f; + float accel_knockdown = 1.0f; + // reduce target angle by ratio of peak rate to max rate + if (is_positive(rate_max) && peak_rate > rate_max) { + rate_knockdown = rate_max / peak_rate; + } + // reduce target angle by ratio of peak accel to max accel + if (is_positive(accel_max) && peak_accel > accel_max) { + accel_knockdown = accel_max / peak_accel; + } + if (rate_knockdown < accel_knockdown) { + target_angle_cd *= rate_knockdown; + } else if (accel_knockdown < rate_knockdown) { + target_angle_cd *= accel_knockdown; + } + } const Vector2f att_fdbk { -5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x