From 05103d9f9cefca0e6e62eedeb111c1a75d4a8bae Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Mon, 4 May 2015 21:07:06 +0930
Subject: [PATCH] Copter: Autotune update
---
ArduCopter/control_autotune.pde | 33 ++++++++++++++++-----------------
1 file changed, 16 insertions(+), 17 deletions(-)
diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde
index ee461d36fd..5f9a273810 100644
--- a/ArduCopter/control_autotune.pde
+++ b/ArduCopter/control_autotune.pde
@@ -52,12 +52,8 @@
#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term
#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term
#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
-#define AUTOTUNE_RD_TEST_NOISE 0.05f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
-#define AUTOTUNE_RD_BACKOFF 4.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
-#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
-#define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 60% of their maximum value discovered during tuning
#define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
-#define AUTOTUNE_PI_RATIO_FINAL 2.5f // I is set 1x P after testing
+#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing
#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing
#define AUTOTUNE_RD_MIN 0.004f // minimum Rate D value
#define AUTOTUNE_RD_MAX 0.050f // maximum Rate D value
@@ -67,13 +63,16 @@
#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value
#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value
#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value
-#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
-#define AUTOTUNE_ACCEL_Y_BACKOFF 0.75f // back off from maximum acceleration
#define AUTOTUNE_RP_ACCEL_MIN 36000.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_Y_ACCEL_MIN 9000.0f // Minimum acceleration for Yaw
#define AUTOTUNE_Y_FILT_FREQ 10.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_SUCCESS_COUNT 4 // how many successful iterations we need to freeze at current gains
#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in
+#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
+#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
+#define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 60% of their maximum value discovered during tuning
+#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
+#define AUTOTUNE_ACCEL_Y_BACKOFF 0.75f // back off from maximum acceleration
// roll and pitch axes
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
@@ -495,7 +494,7 @@ static void autotune_attitude_control()
}
break;
case AUTOTUNE_TYPE_RP_UP:
- autotune_twitching_test(rotation_rate, autotune_target_rate, autotune_test_min, autotune_test_max);
+ autotune_twitching_test(rotation_rate, autotune_target_rate*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max);
autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max);
if (lean_angle >= autotune_target_angle) {
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
@@ -578,13 +577,13 @@ static void autotune_attitude_control()
case AUTOTUNE_TYPE_RP_UP:
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
- autotune_updating_p_up_d_down(tune_roll_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+ autotune_updating_p_up_d_down(tune_roll_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max);
break;
case AUTOTUNE_AXIS_PITCH:
- autotune_updating_p_up_d_down(tune_pitch_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+ autotune_updating_p_up_d_down(tune_pitch_rd, AUTOTUNE_RD_MIN, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max);
break;
case AUTOTUNE_AXIS_YAW:
- autotune_updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max);
+ autotune_updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max);
break;
}
break;
@@ -633,16 +632,16 @@ static void autotune_attitude_control()
autotune_state.tune_type++;
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
- tune_roll_rd = max(AUTOTUNE_RD_MIN, tune_roll_rd * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))));
- tune_roll_rp = max(AUTOTUNE_RP_MIN, tune_roll_rp * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))));
+ tune_roll_rd = max(AUTOTUNE_RD_MIN, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
+ tune_roll_rp = max(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break;
case AUTOTUNE_AXIS_PITCH:
- tune_pitch_rd = max(AUTOTUNE_RD_MIN, tune_pitch_rd * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))));
- tune_pitch_rp = max(AUTOTUNE_RP_MIN, tune_pitch_rp * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))));
+ tune_pitch_rd = max(AUTOTUNE_RD_MIN, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
+ tune_pitch_rp = max(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break;
case AUTOTUNE_AXIS_YAW:
- tune_yaw_rLPF = max(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))));
- tune_yaw_rp = max(AUTOTUNE_RP_MIN, tune_yaw_rp * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))));
+ tune_yaw_rLPF = max(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
+ tune_yaw_rp = max(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
}
break;