Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Autotune fix P test

Browse Source
This commit is contained in:
Leonard Hall 2015-03-22 21:53:11 +10:30 committed by Randy Mackay
parent cedd423c30
commit 35d1cbd053
1 changed files with 18 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
ArduCopter/control_autotune.pde
View File

@ -461,14 +461,14 @@ static void autotune_attitude_control()
switch (autotune_state.tune_type) { switch (autotune_state.tune_type) {
case AUTOTUNE_TYPE_RD_UP: case AUTOTUNE_TYPE_RD_UP:
case AUTOTUNE_TYPE_RD_DOWN: 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); autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max);
if (lean_angle >= autotune_target_angle) { if (lean_angle >= autotune_target_angle) {
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
} }
break; break;
case AUTOTUNE_TYPE_RP_UP: 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); autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max);
if (lean_angle >= autotune_target_angle) { if (lean_angle >= autotune_target_angle) {
autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS;
@ -476,7 +476,7 @@ static void autotune_attitude_control()
break; break;
case AUTOTUNE_TYPE_SP_DOWN: case AUTOTUNE_TYPE_SP_DOWN:
case AUTOTUNE_TYPE_SP_UP: 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); autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate - direction_sign * autotune_start_rate, rate_max);
break; break;
} }
@ -606,16 +606,16 @@ static void autotune_attitude_control()
autotune_state.tune_type++; autotune_state.tune_type++;
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: 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_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 = tune_roll_rp * (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; break;
case AUTOTUNE_AXIS_PITCH: 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_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 = tune_pitch_rp * (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; break;
case AUTOTUNE_AXIS_YAW: 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_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 = tune_yaw_rp * (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;
} }
break; break;
@ -623,13 +623,13 @@ static void autotune_attitude_control()
autotune_state.tune_type++; autotune_state.tune_type++;
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: 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; break;
case AUTOTUNE_AXIS_PITCH: 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; break;
case AUTOTUNE_AXIS_YAW: 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;
} }
break; break;
@ -644,7 +644,7 @@ static void autotune_attitude_control()
bool autotune_complete = false; bool autotune_complete = false;
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: 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); tune_roll_accel = max(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
if (autotune_pitch_enabled()) { if (autotune_pitch_enabled()) {
autotune_state.axis = AUTOTUNE_AXIS_PITCH; autotune_state.axis = AUTOTUNE_AXIS_PITCH;
@ -655,7 +655,7 @@ static void autotune_attitude_control()
} }
break; break;
case AUTOTUNE_AXIS_PITCH: 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); tune_pitch_accel = max(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
if (autotune_yaw_enabled()) { if (autotune_yaw_enabled()) {
autotune_state.axis = AUTOTUNE_AXIS_YAW; autotune_state.axis = AUTOTUNE_AXIS_YAW;
@ -664,7 +664,7 @@ static void autotune_attitude_control()
} }
break; break;
case AUTOTUNE_AXIS_YAW: 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); tune_yaw_accel = max(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
autotune_complete = true; autotune_complete = true;
break; break;
@ -1024,50 +1024,9 @@ inline bool autotune_yaw_enabled() {
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
} }
// autotune_twitching_test_p - twitching tests for P // autotune_twitching_test - twitching tests
// update minimum and max and test for end conditions of P test // update min and max and test for end conditions
void autotune_twitching_test_p(float measurement, float target, float &measurement_min, float &measurement_max) void autotune_twitching_test(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)
{ {
// capture maximum measurement // capture maximum measurement
if (measurement > measurement_max) { if (measurement > measurement_max) {