diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index fa1eecaab4..3e8fd42fbd 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -461,14 +461,14 @@ static void autotune_attitude_control() switch (autotune_state.tune_type) { case AUTOTUNE_TYPE_RD_UP: case AUTOTUNE_TYPE_RD_DOWN: - autotune_twitching_test_d(rotation_rate, autotune_target_rate, autotune_test_min, autotune_test_max); + autotune_twitching_test(rotation_rate, autotune_target_rate, 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; } break; case AUTOTUNE_TYPE_RP_UP: - autotune_twitching_test_p(rotation_rate, autotune_target_rate, autotune_test_min, autotune_test_max); + autotune_twitching_test(rotation_rate, autotune_target_rate, 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; @@ -476,7 +476,7 @@ static void autotune_attitude_control() break; case AUTOTUNE_TYPE_SP_DOWN: case AUTOTUNE_TYPE_SP_UP: - autotune_twitching_test_p(lean_angle, autotune_target_angle, autotune_test_min, autotune_test_max); + autotune_twitching_test(lean_angle, autotune_target_angle, autotune_test_min, autotune_test_max); autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate - direction_sign * autotune_start_rate, rate_max); break; } @@ -606,16 +606,16 @@ static void autotune_attitude_control() autotune_state.tune_type++; switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - tune_roll_rd = tune_roll_rd * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))); - tune_roll_rp = 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 * (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)))); break; case AUTOTUNE_AXIS_PITCH: - tune_pitch_rd = tune_pitch_rd * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))); - tune_pitch_rp = 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 * (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)))); break; case AUTOTUNE_AXIS_YAW: - tune_yaw_rLPF = tune_yaw_rLPF * (1.0f-AUTOTUNE_RD_BACKOFF*max(0.0f,(g.autotune_aggressiveness-AUTOTUNE_RD_TEST_NOISE))); - tune_yaw_rp = 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 * (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)))); break; } break; @@ -623,13 +623,13 @@ static void autotune_attitude_control() autotune_state.tune_type++; switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - tune_roll_rp = tune_roll_rp * AUTOTUNE_RP_BACKOFF; + tune_roll_rp = max(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); break; case AUTOTUNE_AXIS_PITCH: - tune_pitch_rp = tune_pitch_rp * AUTOTUNE_RP_BACKOFF; + tune_pitch_rp = max(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); break; case AUTOTUNE_AXIS_YAW: - tune_yaw_rp = tune_yaw_rp * AUTOTUNE_RP_BACKOFF; + tune_yaw_rp = max(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); break; } break; @@ -644,7 +644,7 @@ static void autotune_attitude_control() bool autotune_complete = false; switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - tune_roll_sp = tune_roll_sp * AUTOTUNE_SP_BACKOFF; + tune_roll_sp = max(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); tune_roll_accel = max(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); if (autotune_pitch_enabled()) { autotune_state.axis = AUTOTUNE_AXIS_PITCH; @@ -655,7 +655,7 @@ static void autotune_attitude_control() } break; case AUTOTUNE_AXIS_PITCH: - tune_pitch_sp = tune_pitch_sp * AUTOTUNE_SP_BACKOFF; + tune_pitch_sp = max(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); tune_pitch_accel = max(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); if (autotune_yaw_enabled()) { autotune_state.axis = AUTOTUNE_AXIS_YAW; @@ -664,7 +664,7 @@ static void autotune_attitude_control() } break; case AUTOTUNE_AXIS_YAW: - tune_yaw_sp = tune_yaw_sp * AUTOTUNE_SP_BACKOFF; + tune_yaw_sp = max(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); tune_yaw_accel = max(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); autotune_complete = true; break; @@ -1024,50 +1024,9 @@ inline bool autotune_yaw_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; } -// autotune_twitching_test_p - twitching tests for P -// update minimum and max and test for end conditions of P test -void autotune_twitching_test_p(float measurement, float target, float &measurement_min, float &measurement_max) -{ - // capture maximum measurement - if (measurement > measurement_max) { - if ((measurement_min < measurement_max) && (measurement_max > target * 0.5f)) { - // the measurement has stopped, bounced back and is starting to increase again. - measurement_max = measurement; - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; - } else { - // the measurement is continuing to increase without stopping - measurement_max = measurement; - measurement_min = measurement; - } - } - - // capture minimum measurement after the measurement has peaked (aka "bounce back") - if ((measurement < measurement_min) && (measurement_max > target * 0.5f)) { - // the measurement is bouncing back - measurement_min = measurement; - } - - // calculate early stopping time based on the time it takes to get to 90% - if (measurement_max < target * 0.9f) { - // the measurement not reached the 90% threshold yet - autotune_step_stop_time = autotune_step_start_time + (millis() - autotune_step_start_time) * 3.0f; - autotune_step_stop_time = min(autotune_step_stop_time, autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); - } - - if (measurement_max > target) { - // the measurement has passed the target - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; - } - - if (millis() >= autotune_step_stop_time) { - // we have passed the maximum stop time - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; - } -} - -// autotune_twitching_test_d - twitching tests for D -// update min and max and test for end conditions of D test -void autotune_twitching_test_d(float measurement, float target, float &measurement_min, float &measurement_max) +// autotune_twitching_test - twitching tests +// update min and max and test for end conditions +void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) { // capture maximum measurement if (measurement > measurement_max) {