From 7204d46ccbb69fc556f44085bf9ab9bd0119476e Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 11 Apr 2018 18:43:41 +0930 Subject: [PATCH] Copter: Autotune - Fix low angle P issue --- ArduCopter/mode_autotune.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 762ab063f5..d424200ca8 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/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