diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
index 42ed29348491fd..b55fb721b6f8da 100644
--- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
+++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
@@ -1040,7 +1040,7 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
     if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP){
         // if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
         if (!is_zero(sweep_tgt.maxgain.freq)) {
-            next_test_freq = sweep_tgt.maxgain.freq - 0.25f * 3.14159f * 2.0f;
+            next_test_freq = sweep_tgt.maxgain.freq;
         } else {
             next_test_freq = min_sweep_freq;            
         }
@@ -1069,7 +1069,7 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
     if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
         // if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
         if (!is_zero(sweep_mtr.ph180.freq)) {
-            next_test_freq = sweep_mtr.ph180.freq - 0.25f * M_2PI;
+            next_test_freq = sweep_mtr.ph180.freq;
             reset_maxgains_update_gain_variables();
         } else {
             next_test_freq = min_sweep_freq;