AC_AutoTune_Heli: protect against low freq

bnsgeyer · Apr 20, 2024 · f5da3f8 · f5da3f8
1 parent 017e316
commit f5da3f8
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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;