From e2560371cf3806712af1989af5f1cb999ec143c8 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 19 Feb 2024 14:02:02 +1030 Subject: [PATCH] Copter: Autotune: Base angles limits on lean_angle_max --- libraries/AC_AutoTune/AC_AutoTune.cpp | 15 ++++++++------- libraries/AC_AutoTune/AC_AutoTune.h | 7 ++++--- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 11 +++++------ 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 998297d021..3bd69757ab 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -22,7 +22,7 @@ #define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level #define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level #define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users -#define AUTOTUNE_ANGLE_MAX_RLLPIT 30.0f // maximum allowable angle in degrees during testing +#define AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE 2.0 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max AC_AutoTune::AC_AutoTune() { @@ -360,18 +360,19 @@ void AC_AutoTune::control_attitude() // Initialize test-specific variables switch (axis) { case ROLL: - abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD; + abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE; start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f; start_angle = ahrs_view->roll_sensor; break; case PITCH: - abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD; + abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE; start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f; start_angle = ahrs_view->pitch_sensor; break; case YAW: case YAW_D: - abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD; + // Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size + abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE; start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f; start_angle = ahrs_view->yaw_sensor; break; @@ -391,13 +392,13 @@ void AC_AutoTune::control_attitude() test_run(axis, direction_sign); // Check for failure causing reverse response - if (lean_angle <= -AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD) { + if (lean_angle <= -attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE) { step = WAITING_FOR_LEVEL; } // protect from roll over - float resultant_angle = degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch())); - if (resultant_angle > AUTOTUNE_ANGLE_MAX_RLLPIT) { + const float resultant_angle_cd = 100 * degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch())); + if (resultant_angle_cd > attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE) { step = WAITING_FOR_LEVEL; } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index aff6c928f8..098a2a4824 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -44,9 +44,10 @@ #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 -#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE 1.0 / 4.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE 1.0 / 2.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE 1.0 / 8.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_ANGLE_YAW_SCALE 2.0 / 3.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step class AC_AutoTune { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 91876f0218..2287eedb19 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -75,9 +75,8 @@ #define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step // yaw axis -#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step -#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step // second table of user settable parameters for quadplanes, this // allows us to go beyond the 64 parameter limit @@ -1133,14 +1132,14 @@ void AC_AutoTune_Multi::twitch_test_init() case ROLL: { target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_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); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE); rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f); break; } case PITCH: { target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_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); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE); rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f); break; } @@ -1148,7 +1147,7 @@ void AC_AutoTune_Multi::twitch_test_init() case YAW_D: { target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_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); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE); if (axis == YAW_D) { rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz()*2.0f); } else {