diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 6b32d184f2..14faa8a424 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -987,7 +987,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Description: autotune_aggressiveness. Defines the bounce back used to detect size of the D term. // @Range: 0.05 0.10 // @User: Standard - GSCALAR(autotune_aggressiveness, "AUTOTUNE_AGGR", 0.05f), + GSCALAR(autotune_aggressiveness, "AUTOTUNE_AGGR", 0.1f), AP_VAREND }; diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 33ffec3b95..2a57152ed5 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -46,7 +46,9 @@ #define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 500 // timeout for tuning mode's testing step #define AUTOTUNE_LEVEL_ANGLE_CD 300 // angle which qualifies as level -#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the copter to be level +#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch +#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw +#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level #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 @@ -57,16 +59,16 @@ #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_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing -#define AUTOTUNE_RD_MIN 0.002f // minimum Rate D value +#define AUTOTUNE_RD_MIN 0.004f // minimum Rate D value #define AUTOTUNE_RD_MAX 0.050f // maximum Rate D value #define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value #define AUTOTUNE_RLPF_MAX 10.0f // maximum Rate Yaw filter value #define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value -#define AUTOTUNE_RP_MAX 5.0f // maximum Rate P value +#define AUTOTUNE_RP_MAX 1.0f // maximum Rate P value #define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value -#define AUTOTUNE_SP_MIN 1.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_ACCEL_Y_BACKOFF 0.75f // back off from maximum acceleration #define AUTOTUNE_RP_ACCEL_MIN 75000.0f // Minimum acceleration for Roll and Pitch #define AUTOTUNE_Y_ACCEL_MIN 18000.0f // Minimum acceleration for Roll and Pitch #define AUTOTUNE_SUCCESS_COUNT 4 // how many successful iterations we need to freeze at current gains @@ -130,6 +132,7 @@ struct autotune_state_struct { uint8_t positive_direction : 1; // 0 = tuning in negative direction (i.e. left for roll), 1 = positive direction (i.e. right for roll) AutoTuneStepType step : 2; // see AutoTuneStepType for what steps are performed AutoTuneTuneType tune_type : 3; // see AutoTuneTuneType + uint8_t ignore_next : 1; // 1 = ignore the next test } autotune_state; // variables @@ -141,8 +144,11 @@ static uint32_t autotune_step_stop_time; // start time of static int8_t autotune_counter; // counter for tuning gains static float autotune_target_rate, autotune_start_rate; // target and start rate static float autotune_target_angle, autotune_start_angle; // target and start angles +static float autotune_desired_yaw; // yaw heading during tune static float rate_max, autotune_test_accel_max; // maximum acceleration variables +LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second + // backup of currently being tuned parameter values static float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp; static float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp; @@ -308,6 +314,7 @@ static void autotune_run() // set gains to their intra-test values (which are very close to the original gains) // autotune_load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; // set tuning step back from beginning + autotune_desired_yaw = ahrs.yaw_sensor; } } @@ -341,15 +348,20 @@ static void autotune_attitude_control() attitude_control.limit_angle_to_rate_request(true); // hold level attitude - attitude_control.angle_ef_roll_pitch_rate_ef_yaw( 0.0f, 0.0f, 0.0f); + attitude_control.angle_ef_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true); - // hold the copter level for 0.25 seconds before we begin a twitch + // hold the copter level for 0.5 seconds before we begin a twitch // reset counter if we are no longer level - if ((labs(ahrs.roll_sensor) > AUTOTUNE_LEVEL_ANGLE_CD) || (labs(ahrs.pitch_sensor) > AUTOTUNE_LEVEL_ANGLE_CD)) { + if ((labs(ahrs.roll_sensor) > AUTOTUNE_LEVEL_ANGLE_CD) || + (labs(ahrs.pitch_sensor) > AUTOTUNE_LEVEL_ANGLE_CD) || + (labs(wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_desired_yaw)) > AUTOTUNE_LEVEL_ANGLE_CD) || + ((ToDeg(ahrs.get_gyro().x) * 100.0f) > AUTOTUNE_LEVEL_RATE_RP_CD) || + ((ToDeg(ahrs.get_gyro().y) * 100.0f) > AUTOTUNE_LEVEL_RATE_RP_CD) || + ((ToDeg(ahrs.get_gyro().z) * 100.0f) > AUTOTUNE_LEVEL_RATE_Y_CD) ) { autotune_step_start_time = millis(); } - // if we have been level for a sufficient amount of time (0.25 seconds) move onto tuning step + // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step if (millis() - autotune_step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { // initiate variables for next step autotune_state.step = AUTOTUNE_STEP_TWITCHING; @@ -357,7 +369,7 @@ static void autotune_attitude_control() autotune_step_stop_time = autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; autotune_test_max = 0.0f; autotune_test_min = 0.0f; - rotation_rate = 0.0f; + rotation_rate_filt.reset(0.0f); rate_max = 0.0f; // set gains to their to-be-tested values autotune_load_twitch_gains(); @@ -369,18 +381,36 @@ static void autotune_attitude_control() autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_roll(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f; autotune_start_angle = ahrs.roll_sensor; + rotation_rate_filt.set_cutoff_frequency(MAIN_LOOP_SECONDS,g.pid_rate_roll.filt_hz()*4.0f); + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate_filt.reset(autotune_start_rate); + } else { + rotation_rate_filt.reset(0); + } break; case AUTOTUNE_AXIS_PITCH: autotune_target_rate = constrain_float(attitude_control.max_rate_step_bf_pitch(), AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_pitch(), AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f; autotune_start_angle = ahrs.pitch_sensor; + rotation_rate_filt.set_cutoff_frequency(MAIN_LOOP_SECONDS,g.pid_rate_pitch.filt_hz()*4.0f); + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate_filt.reset(autotune_start_rate); + } else { + rotation_rate_filt.reset(0); + } break; case AUTOTUNE_AXIS_YAW: autotune_target_rate = constrain_float(attitude_control.max_rate_step_bf_yaw()/1.5f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS); autotune_target_angle = constrain_float(attitude_control.max_angle_step_bf_yaw(), AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); autotune_start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f; autotune_start_angle = ahrs.yaw_sensor; + rotation_rate_filt.set_cutoff_frequency(MAIN_LOOP_SECONDS,orig_yaw_rLPF*4.0f); + if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + rotation_rate_filt.reset(autotune_start_rate); + } else { + rotation_rate_filt.reset(0); + } break; } break; @@ -434,25 +464,25 @@ static void autotune_attitude_control() switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f)); } else { - rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate)); } lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)autotune_start_angle); break; case AUTOTUNE_AXIS_PITCH: if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f)); } else { - rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate)); } lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)autotune_start_angle); break; case AUTOTUNE_AXIS_YAW: if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f)); } else { - rotation_rate = direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate)); } lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_start_angle); break; @@ -717,7 +747,10 @@ static void autotune_backup_gains_and_initialise() autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; autotune_step_start_time = millis(); autotune_state.tune_type = AUTOTUNE_TYPE_RD_UP; - autotune_start_angle = ahrs.yaw_sensor; + + autotune_desired_yaw = ahrs.yaw_sensor; + + g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05, 0.1); // backup original pids and initialise tuned pid values if (autotune_roll_enabled()) { @@ -1090,22 +1123,26 @@ void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, f }else{ // we have a good measurement of bounce back if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; // bounce back is bigger than our threshold so increment the success counter autotune_counter++; - // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; }else{ - // bounce back is smaller than our threshold so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; - } - // increase D gain (which should increase bounce back) - tune_d += tune_d*tune_d_step_ratio*2.0f; - // stop tuning if we hit maximum D - if (tune_d >= tune_d_max) { - tune_d = tune_d_max; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + if (autotune_state.ignore_next == 0){ + // bounce back is smaller than our threshold so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // increase D gain (which should increase bounce back) + tune_d += tune_d*tune_d_step_ratio*2.0f; + // stop tuning if we hit maximum D + if (tune_d >= tune_d_max) { + tune_d = tune_d_max; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } else { + autotune_state.ignore_next = 0; } } } @@ -1137,15 +1174,19 @@ void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step }else{ // we have a good measurement of bounce back if (measurement_max-measurement_min < measurement_max*g.autotune_aggressiveness) { - // bounce back is less than our threshold so increment the success counter - autotune_counter++; + if (autotune_state.ignore_next == 0){ + // bounce back is less than our threshold so increment the success counter + autotune_counter++; + } else { + autotune_state.ignore_next = 0; + } }else{ + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; // bounce back is larger than our threshold so decrement the success counter if (autotune_counter > 0 ) { autotune_counter--; } - // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; // decrease D gain (which should decrease bounce back) tune_d -= tune_d*tune_d_step_ratio; // stop tuning if we hit minimum D @@ -1163,15 +1204,19 @@ void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max < target) { - // if maximum measurement was lower than target so increment the success counter - autotune_counter++; + if (autotune_state.ignore_next == 0){ + // if maximum measurement was lower than target so increment the success counter + autotune_counter++; + } else { + autotune_state.ignore_next = 0; + } }else{ + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; // if maximum measurement was higher than target so decrement the success counter if (autotune_counter > 0 ) { autotune_counter--; } - // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; // decrease P gain (which should decrease the maximum) tune_p -= tune_p*tune_p_step_ratio; // stop tuning if we hit maximum P @@ -1188,22 +1233,26 @@ void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max > target) { + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; // if maximum measurement was greater than target so increment the success counter autotune_counter++; - // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; }else{ - // if maximum measurement was lower than target so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; - } - // increase P gain (which should increase the maximum) - tune_p += tune_p*tune_p_step_ratio; - // stop tuning if we hit maximum P - if (tune_p >= tune_p_max) { - tune_p = tune_p_max; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + if (autotune_state.ignore_next == 0){ + // if maximum measurement was lower than target so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // increase P gain (which should increase the maximum) + tune_p += tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } else { + autotune_state.ignore_next = 0; } } } @@ -1213,17 +1262,15 @@ void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_r void autotune_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) { if (measurement_max > target) { + // ignore the next result unless it is the same as this one + autotune_state.ignore_next = 1; // if maximum measurement was greater than target so increment the success counter autotune_counter++; - // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; }else if ((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 (autotune_counter > 0 ) { autotune_counter--; } - // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; // decrease D gain (which should decrease bounce back) tune_d -= tune_d*tune_d_step_ratio; // stop tuning if we hit minimum D @@ -1238,18 +1285,24 @@ void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d tune_p = tune_p_min; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } + // cancel change in direction + autotune_state.positive_direction = !autotune_state.positive_direction; }else{ - // if maximum measurement was lower than target so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; - } - // increase P gain (which should increase the maximum) - tune_p += tune_p*tune_p_step_ratio; - // stop tuning if we hit maximum P - if (tune_p >= tune_p_max) { - tune_p = tune_p_max; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + if (autotune_state.ignore_next == 0){ + // if maximum measurement was lower than target so decrement the success counter + if (autotune_counter > 0 ) { + autotune_counter--; + } + // increase P gain (which should increase the maximum) + tune_p += tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } else { + autotune_state.ignore_next = 0; } } }