From 227768bdf95191a8a1b4ba54a0f17b33c40db75c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 18 Feb 2024 21:05:14 +1030 Subject: [PATCH] Copter: Autotune: More Feedback and Fixes --- libraries/AC_AutoTune/AC_AutoTune.cpp | 6 +++--- libraries/AC_AutoTune/AC_AutoTune.h | 8 ++++---- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index e1ed6e1c0f833..ec71c44fd02d9 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -21,7 +21,7 @@ #define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw #define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the aircraft to be level #define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level -#define AUTOTUNE_ANGLE_MAX_RLLPIT 2.0 / 3.0 // maximum allowable angle in degrees during testing +#define AUTOTUNE_ANGLE_MAX_RLLPIT 2.0 / 3.0 // maximum allowable angle during testing, as a ratio of angle_max AC_AutoTune::AC_AutoTune() { @@ -395,7 +395,7 @@ void AC_AutoTune::control_attitude() } // protect from roll over - float resultant_angle = degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch())); + float resultant_angle = 100 * degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch())); if (resultant_angle > attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_MAX_RLLPIT) { step = WAITING_FOR_LEVEL; positive_direction = twitch_reverse_direction(); @@ -410,7 +410,7 @@ void AC_AutoTune::control_attitude() log_pids(); #endif - if (axis == YAW || axis == YAW_D ) { + if (axis == YAW || axis == YAW_D) { desired_yaw_cd = ahrs_view->yaw_sensor; } break; diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index d80f65a89852c..1d13aff855e4c 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -44,10 +44,10 @@ #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 -#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT 1.0 / 4.0 // minimum target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_ANGLE_RLLPIT 1.0 / 2.0 // target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_MIN_ANGLE_YAW 1.0 / 8.0 // minimum target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_ANGLE_YAW 2.0 / 3.0 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT 1.0 / 4.0 // minimum target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_ANGLE_RLLPIT 1.0 / 2.0 // target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_ANGLE_YAW 1.0 / 8.0 // minimum target angle, as a ratio of angle_max, during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_ANGLE_YAW 2.0 / 3.0 // target angle, as a ratio 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 8a98b5845bfce..5f60bbadc878e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -516,7 +516,7 @@ void AC_AutoTune_Multi::twitching_test_rate(float rate, float rate_target_max, f } // calculate early stopping time based on the time it takes to get to 75% - if (meas_rate_max < rate_target_max * 63.21) { + if (meas_rate_max < rate_target_max * 0.6321) { // the measurement not reached the 63.21% threshold yet step_time_limit_ms = (now - step_start_time_ms) * 3; step_time_limit_ms = MIN(step_time_limit_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS); @@ -592,7 +592,7 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl } // calculate early stopping time based on the time it takes to get to 75% - if (meas_angle_max < angle_target_max * 63.21) { + if (meas_angle_max < angle_target_max * 0.6321) { // the measurement not reached the 63.21% threshold yet step_time_limit_ms = (now - step_start_time_ms) * 3; step_time_limit_ms = MIN(step_time_limit_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS);