From 83fac326a4796bb6acce0bc3d74a40541aa668b0 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 1 Apr 2018 15:11:49 +0930 Subject: [PATCH] Copter: autotune check for vel overshoot in angle P this change reduces the chance of over tuned angle P --- ArduCopter/mode.h | 25 ++-- ArduCopter/mode_autotune.cpp | 265 +++++++++++++++++++++-------------- 2 files changed, 174 insertions(+), 116 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index da628a7ddb..9c23b4b28d 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -373,13 +373,14 @@ private: bool roll_enabled(); bool pitch_enabled(); bool yaw_enabled(); - void twitching_test(float measurement, float target, float &measurement_min, float &measurement_max); - void updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); - void updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); - void updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max); - void updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max); - void updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max); + void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); + void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); + void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); + void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); + void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); + void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); #if LOGGING_ENABLED == ENABLED @@ -450,17 +451,19 @@ private: // variables uint32_t override_time; // the last time the pilot overrode the controls - float test_min; // the minimum angular rate achieved during TESTING_RATE step - float test_max; // the maximum angular rate achieved during TESTING_RATE step + float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step + float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step + float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step + float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step uint32_t step_start_time; // start time of current tuning step (used for timeout checks) uint32_t step_stop_time; // start time of current tuning step (used for timeout checks) int8_t counter; // counter for tuning gains - float target_rate, start_rate; // target and start rate - float target_angle, start_angle; // target and start angles + float target_rate, start_rate; // target and start rate + float target_angle, start_angle; // target and start angles float desired_yaw; // yaw heading during tune float rate_max, test_accel_max; // maximum acceleration variables - LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second + LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second // backup of currently being tuned parameter values float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel; diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index c12bef6e18..435efaf54e 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -73,7 +73,7 @@ // 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 -#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 9000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step #define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step @@ -499,8 +499,10 @@ void Copter::ModeAutoTune::autotune_attitude_control() step_start_time = millis(); step_stop_time = step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; twitch_first_iter = true; - test_max = 0.0f; - test_min = 0.0f; + test_rate_max = 0.0f; + test_rate_min = 0.0f; + test_angle_max = 0.0f; + test_angle_min = 0.0f; rotation_rate_filt.reset(0.0f); rate_max = 0.0f; // set gains to their to-be-tested values @@ -628,14 +630,14 @@ void Copter::ModeAutoTune::autotune_attitude_control() switch (tune_type) { case RD_UP: case RD_DOWN: - twitching_test(rotation_rate, target_rate, test_min, test_max); + twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max); twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); if (lean_angle >= target_angle) { step = UPDATE_GAINS; } break; case RP_UP: - twitching_test(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_min, test_max); + twitching_test_rate(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_rate_min, test_rate_max); twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); if (lean_angle >= target_angle) { step = UPDATE_GAINS; @@ -643,7 +645,7 @@ void Copter::ModeAutoTune::autotune_attitude_control() break; case SP_DOWN: case SP_UP: - twitching_test(lean_angle, target_angle*(1+0.5f*g.autotune_aggressiveness), test_min, test_max); + twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*g.autotune_aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max); break; } @@ -665,25 +667,25 @@ void Copter::ModeAutoTune::autotune_attitude_control() if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { switch (axis) { case ROLL: - Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); break; case PITCH: - Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); break; case YAW: - Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); break; } } else { switch (axis) { case ROLL: - Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); break; case PITCH: - Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); break; case YAW: - Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); break; } } @@ -694,13 +696,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() case RD_UP: switch (axis) { case ROLL: - updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; case PITCH: - updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; case YAW: - updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; } break; @@ -708,13 +710,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() case RD_DOWN: switch (axis) { case ROLL: - updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; case PITCH: - updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; case YAW: - updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; } break; @@ -722,13 +724,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() case RP_UP: switch (axis) { case ROLL: - updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; case PITCH: - updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); + updating_rate_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); break; case YAW: - 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, target_rate, test_min, test_max); + updating_rate_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, target_rate, test_rate_min, test_rate_max); break; } break; @@ -736,13 +738,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() case SP_DOWN: switch (axis) { case ROLL: - updating_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max); + updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); break; case PITCH: - updating_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max); + updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); break; case YAW: - updating_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max); + updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); break; } break; @@ -750,13 +752,13 @@ void Copter::ModeAutoTune::autotune_attitude_control() case SP_UP: switch (axis) { case ROLL: - updating_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max); + updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); break; case PITCH: - updating_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max); + updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); break; case YAW: - updating_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max); + updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); break; } break; @@ -1182,37 +1184,37 @@ inline bool Copter::ModeAutoTune::yaw_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; } -// twitching_test - twitching tests +// twitching_test_rate - twitching tests // update min and max and test for end conditions -void Copter::ModeAutoTune::twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) +void Copter::ModeAutoTune::twitching_test_rate(float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max) { - // capture maximum measurement - if (measurement > measurement_max) { + // capture maximum rate + if (rate > meas_rate_max) { // the measurement is continuing to increase without stopping - measurement_max = measurement; - measurement_min = measurement; + meas_rate_max = rate; + meas_rate_min = rate; } // capture minimum measurement after the measurement has peaked (aka "bounce back") - if ((measurement < measurement_min) && (measurement_max > target * 0.5f)) { + if ((rate < meas_rate_min) && (meas_rate_max > rate_target_max * 0.5f)) { // the measurement is bouncing back - measurement_min = measurement; + meas_rate_min = rate; } // calculate early stopping time based on the time it takes to get to 90% - if (measurement_max < target * 0.75f) { + if (meas_rate_max < rate_target_max * 0.75f) { // the measurement not reached the 90% threshold yet step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f; step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); } - if (measurement_max > target) { - // the measurement has passed the target + if (meas_rate_max > rate_target_max) { + // the measured rate has passed the maximum target rate step = UPDATE_GAINS; } - if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { - // the measurement has passed 50% of the target and bounce back is larger than the threshold + if (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) { + // the measurement has passed 50% of the maximum rate and bounce back is larger than the threshold step = UPDATE_GAINS; } @@ -1222,11 +1224,73 @@ void Copter::ModeAutoTune::twitching_test(float measurement, float target, float } } -// updating_d_up - increase D and adjust P to optimize the D term for a little bounce back +// twitching_test_angle - twitching tests +// update min and max and test for end conditions +void Copter::ModeAutoTune::twitching_test_angle(float angle, float rate, float angle_target_max, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max) +{ + // capture maximum angle + if (angle > meas_angle_max) { + // the angle still increasing + meas_angle_max = angle; + meas_angle_min = angle; + } + + // capture minimum angle after we have reached a reasonable maximum angle + if ((angle < meas_angle_min) && (meas_angle_max > angle_target_max * 0.5f)) { + // the measurement is bouncing back + meas_angle_min = angle; + } + + // capture maximum rate + if (rate > meas_rate_max) { + // the measurement is still increasing + meas_rate_max = rate; + meas_rate_min = rate; + } + + // capture minimum rate after we have reached maximum rate + if (rate < meas_rate_min) { + // the measurement is still decreasing + meas_rate_min = rate; + } + + // calculate early stopping time based on the time it takes to get to 90% + if (meas_angle_max < angle_target_max * 0.75f) { + // the measurement not reached the 90% threshold yet + step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f; + step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + } + + if (meas_angle_max > angle_target_max) { + // the measurement has passed the maximum angle + step = UPDATE_GAINS; + } + + if (meas_angle_max-meas_angle_min > meas_angle_max*g.autotune_aggressiveness) { + // the measurement has passed 50% of the maximum angle and bounce back is larger than the threshold + step = UPDATE_GAINS; + } + + if (millis() >= step_stop_time) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + } +} + +// twitching_measure_acceleration - measure rate of change of measurement +void Copter::ModeAutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) +{ + if (rate_measurement_max < rate_measurement) { + rate_measurement_max = rate_measurement; + rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time); + } +} + +// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::ModeAutoTune::updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) { - if (measurement_max > target) { + if (meas_rate_max > rate_target) { // if maximum measurement was higher than target // reduce P gain (which should reduce maximum) tune_p -= tune_p*tune_p_step_ratio; @@ -1241,7 +1305,7 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } - }else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { + }else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { // we have not achieved a high enough maximum to get a good measurement of bounce back. // increase P gain (which should increase maximum) tune_p += tune_p*tune_p_step_ratio; @@ -1251,7 +1315,7 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float } }else{ // we have a good measurement of bounce back - if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { + if (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) { // ignore the next result unless it is the same as this one ignore_next = true; // bounce back is bigger than our threshold so increment the success counter @@ -1277,11 +1341,11 @@ void Copter::ModeAutoTune::updating_d_up(float &tune_d, float tune_d_min, float } } -// updating_d_down - decrease D and adjust P to optimize the D term for no bounce back +// updating_rate_d_down - decrease D and adjust P to optimize the D term for no bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::ModeAutoTune::updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) { - if (measurement_max > target) { + if (meas_rate_max > rate_target) { // if maximum measurement was higher than target // reduce P gain (which should reduce maximum) tune_p -= tune_p*tune_p_step_ratio; @@ -1296,7 +1360,7 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } - }else if ((measurement_max < target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { + }else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { // we have not achieved a high enough maximum to get a good measurement of bounce back. // increase P gain (which should increase maximum) tune_p += tune_p*tune_p_step_ratio; @@ -1306,7 +1370,7 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa } }else{ // we have a good measurement of bounce back - if (measurement_max-measurement_min < measurement_max*g.autotune_aggressiveness) { + if (meas_rate_max-meas_rate_min < meas_rate_max*g.autotune_aggressiveness) { if (ignore_next == false) { // bounce back is less than our threshold so increment the success counter counter++; @@ -1332,44 +1396,36 @@ void Copter::ModeAutoTune::updating_d_down(float &tune_d, float tune_d_min, floa } } -// updating_p_down - decrease P until we don't reach the target before time out -// P is decreased to ensure we are not overshooting the target -void Copter::ModeAutoTune::updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) +// updating_rate_p_up_d_down - increase P to ensure the target is reached while checking bounce back isn't increasing +// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold +void Copter::ModeAutoTune::updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) { - if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) { - if (ignore_next == false) { - // if maximum measurement was lower than target so increment the success counter - counter++; - } else { - ignore_next = false; - } - }else{ + if (meas_rate_max > rate_target*(1+0.5f*g.autotune_aggressiveness)) { // ignore the next result unless it is the same as this one ignore_next = true; - // if maximum measurement was higher than target so decrement the success counter + // if maximum measurement was greater than target so increment the success counter + counter++; + } else if ((meas_rate_max < rate_target) && (meas_rate_max > rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) { + // if bounce back was larger than the threshold so decrement the success counter if (counter > 0 ) { counter--; } - // decrease P gain (which should decrease the maximum) + // decrease D gain (which should decrease bounce back) + tune_d -= tune_d*tune_d_step_ratio; + // do not decrease the D term past the minimum + if (tune_d <= tune_d_min) { + tune_d = tune_d_min; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + // decrease P gain to match D gain reduction tune_p -= tune_p*tune_p_step_ratio; - // stop tuning if we hit maximum P + // do not decrease the P term past the minimum if (tune_p <= tune_p_min) { tune_p = tune_p_min; - counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } - } -} - -// updating_p_up - increase P to ensure the target is reached -// P is increased until we achieve our target within a reasonable time -void Copter::ModeAutoTune::updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) -{ - if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { - // ignore the next result unless it is the same as this one - ignore_next = 1; - // if maximum measurement was greater than target so increment the success counter - counter++; + // cancel change in direction + positive_direction = !positive_direction; }else{ if (ignore_next == false) { // if maximum measurement was lower than target so decrement the success counter @@ -1390,36 +1446,44 @@ void Copter::ModeAutoTune::updating_p_up(float &tune_p, float tune_p_max, float } } -// updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing -// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold -void Copter::ModeAutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +// updating_angle_p_down - decrease P until we don't reach the target before time out +// P is decreased to ensure we are not overshooting the target +void Copter::ModeAutoTune::updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max) { - if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { + if ((meas_angle_max < angle_target*(1+0.5f*g.autotune_aggressiveness)) && (meas_rate_min > -meas_rate_max*g.autotune_aggressiveness)) { + if (ignore_next == false) { + // if maximum measurement was lower than target so increment the success counter + counter++; + } else { + ignore_next = false; + } + }else{ // ignore the next result unless it is the same as this one ignore_next = true; - // if maximum measurement was greater than target so increment the success counter - counter++; - } else if ((measurement_max < target) && (measurement_max > target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) { - // if bounce back was larger than the threshold so decrement the success counter + // if maximum measurement was higher than target so decrement the success counter if (counter > 0 ) { counter--; } - // decrease D gain (which should decrease bounce back) - tune_d -= tune_d*tune_d_step_ratio; - // stop tuning if we hit minimum D - if (tune_d <= tune_d_min) { - tune_d = tune_d_min; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - // decrease P gain to match D gain reduction + // decrease P gain (which should decrease the maximum) tune_p -= tune_p*tune_p_step_ratio; - // stop tuning if we hit minimum P + // stop tuning if we hit maximum P if (tune_p <= tune_p_min) { tune_p = tune_p_min; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } - // cancel change in direction - positive_direction = !positive_direction; + } +} + +// updating_angle_p_up - increase P to ensure the target is reached +// P is increased until we achieve our target within a reasonable time +void Copter::ModeAutoTune::updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max) +{ + if ((meas_angle_max > angle_target*(1+0.5f*g.autotune_aggressiveness)) || (meas_rate_min < -meas_rate_max*g.autotune_aggressiveness)) { + // ignore the next result unless it is the same as this one + ignore_next = 1; + // if maximum measurement was greater than target so increment the success counter + counter++; }else{ if (ignore_next == false) { // if maximum measurement was lower than target so decrement the success counter @@ -1440,15 +1504,6 @@ void Copter::ModeAutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min, } } -// twitching_measure_acceleration - measure rate of change of measurement -void Copter::ModeAutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) -{ - if (rate_measurement_max < rate_measurement) { - rate_measurement_max = rate_measurement; - rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time); - } -} - // get attitude for slow position hold in autotune mode void Copter::ModeAutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out) {