diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 572ce1d45c..8e53e1bac5 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -395,7 +395,7 @@ static void autotune_attitude_control() } // capture min rotation rate after the rotation rate has peaked (aka "bounce back rate") - if (rotation_rate < autotune_test_min && autotune_test_max > AUTOTUNE_TARGET_RATE_CDS*(1-2*AUTOTUNE_AGGRESSIVENESS)) { + if (rotation_rate < autotune_test_min && autotune_test_max > AUTOTUNE_TARGET_RATE_CDS*0.5) { autotune_test_min = rotation_rate; } } @@ -446,14 +446,20 @@ static void autotune_attitude_control() // if max rotation rate was higher than target, reduce rate P if (autotune_state.axis == AUTOTUNE_AXIS_ROLL) { tune_roll_rp -= AUTOTUNE_RP_STEP; + // abandon tuning if rate P falls below 0.01 + if(tune_roll_rp < AUTOTUNE_RP_MIN) { + tune_roll_rp = AUTOTUNE_RP_MIN; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } }else{ tune_pitch_rp -= AUTOTUNE_RP_STEP; - } - // abandon tuning if rate P falls below 0.01 - if(((autotune_state.axis == AUTOTUNE_AXIS_ROLL && tune_roll_rp < AUTOTUNE_RP_MIN) || - (autotune_state.axis == AUTOTUNE_AXIS_PITCH && tune_pitch_rp < AUTOTUNE_RP_MIN)) ) { - autotune_failed(); - return; + // abandon tuning if rate P falls below 0.01 + if( tune_pitch_rp < AUTOTUNE_RP_MIN ) { + tune_pitch_rp = AUTOTUNE_RP_MIN; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } } // if maximum rotation rate was less than 80% of requested rate increase rate P }else if(autotune_test_max < AUTOTUNE_TARGET_RATE_CDS*(1.0f-AUTOTUNE_AGGRESSIVENESS*2.0f) && @@ -499,14 +505,30 @@ static void autotune_attitude_control() // if max rotation rate was higher than target, reduce rate P if (autotune_state.axis == AUTOTUNE_AXIS_ROLL) { tune_roll_rp -= AUTOTUNE_RP_STEP; + // reduce rate D if tuning if rate P falls below 0.01 + if(tune_roll_rp < AUTOTUNE_RP_MIN) { + tune_roll_rp = AUTOTUNE_RP_MIN; + tune_roll_rd -= AUTOTUNE_RD_STEP; + // stop tuning if we hit min D + if (tune_roll_rd <= AUTOTUNE_RD_MIN) { + tune_roll_rd = AUTOTUNE_RD_MIN; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } }else{ tune_pitch_rp -= AUTOTUNE_RP_STEP; - } - // abandon tuning if rate P falls below 0.01 - if(((autotune_state.axis == AUTOTUNE_AXIS_ROLL && tune_roll_rp < AUTOTUNE_RP_MIN) || - (autotune_state.axis == AUTOTUNE_AXIS_PITCH && tune_pitch_rp < AUTOTUNE_RP_MIN)) ) { - autotune_failed(); - return; + // reduce rate D if tuning if rate P falls below 0.01 + if( tune_pitch_rp < AUTOTUNE_RP_MIN ) { + tune_pitch_rp = AUTOTUNE_RP_MIN; + tune_pitch_rd -= AUTOTUNE_RD_STEP; + // stop tuning if we hit min D + if (tune_pitch_rd <= AUTOTUNE_RD_MIN) { + tune_pitch_rd = AUTOTUNE_RD_MIN; + autotune_counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); + } + } } // if maximum rotation rate was less than 80% of requested rate increase rate P }else if(autotune_test_max < AUTOTUNE_TARGET_RATE_CDS*(1-AUTOTUNE_AGGRESSIVENESS*2.0f) &&