Copter: Autotune: More Feedback and Fixes

lthall · Feb 18, 2024 · 227768b · 227768b
1 parent ab02152
commit 227768b
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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);