Copter: Autotune - Fix low angle P issue

ArduCopter/mode_autotune.cpp

@@ -1255,7 +1255,7 @@ void Copter::ModeAutoTune::twitching_test_angle(float angle, float rate, float a
 
     // 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
+        // the measurement not reached the 75% 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);
     }
@@ -1449,7 +1449,7 @@ void Copter::ModeAutoTune::updating_rate_p_up_d_down(float &tune_d, float tune_d
 // 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 ((meas_angle_max < angle_target*(1+0.5f*g.autotune_aggressiveness)) && (meas_rate_min > -meas_rate_max*g.autotune_aggressiveness)) {
+    if (meas_angle_max < angle_target*(1+0.5f*g.autotune_aggressiveness)) {
         if (ignore_next == false) {
             // if maximum measurement was lower than target so increment the success counter
             counter++;
@@ -1478,7 +1478,8 @@ void Copter::ModeAutoTune::updating_angle_p_down(float &tune_p, float tune_p_min
 // 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)) {
+    if ((meas_angle_max > angle_target*(1+0.5f*g.autotune_aggressiveness)) ||
+            ((meas_angle_max > angle_target) && (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