AC_AutoTune: general cleanup and protect against freq exceedences

libraries/AC_AutoTune/AC_AutoTune_Heli.cpp

@@ -749,9 +749,11 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float fi
     filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq);
     filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq);
 
+    curr_test_mtr = {};
+    curr_test_tgt = {};
     cycle_complete_tgt = false;
     cycle_complete_mtr = false;
-
+    sweep_complete = false;
 
 }
 
@@ -877,9 +879,13 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
 
         if (freqresp_mtr.is_cycle_complete()) {
             if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
+                if (is_zero(curr_test_mtr.freq) && freqresp_mtr.get_freq() < test_start_freq) {
+                    // don't set data since captured frequency is below the start frequency
+                } else {
                     curr_test_mtr.freq = freqresp_mtr.get_freq();
                     curr_test_mtr.gain = freqresp_mtr.get_gain();
                     curr_test_mtr.phase = freqresp_mtr.get_phase();
+                }
                 // reset cycle_complete to allow indication of next cycle
                 freqresp_mtr.reset_cycle_complete();
 #if HAL_LOGGING_ENABLED
@@ -895,10 +901,14 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
 
         if (freqresp_tgt.is_cycle_complete()) {
             if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
+                if (is_zero(curr_test_tgt.freq) && freqresp_tgt.get_freq() < test_start_freq) {
+                    // don't set data since captured frequency is below the start frequency
+                } else {
                     curr_test_tgt.freq = freqresp_tgt.get_freq();
                     curr_test_tgt.gain = freqresp_tgt.get_gain();
                     curr_test_tgt.phase = freqresp_tgt.get_phase();
                     if (test_calc_type == DRB) {test_accel_max = freqresp_tgt.get_accel_max();}
+                }
                 // reset cycle_complete to allow indication of next cycle
                 freqresp_tgt.reset_cycle_complete();
 #if HAL_LOGGING_ENABLED
@@ -967,6 +977,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
 
         if (now - step_start_time_ms >= sweep_time_ms + 200) {
             // we have passed the maximum stop time
+            sweep_complete = true;
             step = UPDATE_GAINS;
         }
     } else {
@@ -1047,7 +1058,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 = constrain_float(sweep_tgt.maxgain.freq, min_sweep_freq, max_sweep_freq);
         } else {
             next_test_freq = min_sweep_freq;            
         }
@@ -1076,7 +1087,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 = constrain_float(sweep_mtr.ph180.freq, min_sweep_freq, max_sweep_freq);
             reset_maxgains_update_gain_variables();
         } else {
             next_test_freq = min_sweep_freq;
@@ -1165,9 +1176,9 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data
     float test_freq_incr = 0.05f * M_2PI;
     next_freq = test_data.freq;
     if (test_data.phase > 15.0f) {
-        next_freq -= test_freq_incr;
+        next_freq = constrain_float(next_freq - test_freq_incr, 0.1 * M_2PI, max_sweep_freq);
     } else if (test_data.phase < 0.0f) {
-        next_freq += test_freq_incr;
+        next_freq = constrain_float(next_freq + test_freq_incr, 0.1 * M_2PI, max_sweep_freq);
     } else {
         if ((test_data.gain > 0.1 && test_data.gain < 0.93) || test_data.gain > 0.98) {
             if (tune_ff > 0.0f) {
@@ -1468,7 +1479,7 @@ void AC_AutoTune_Heli::Log_AutoTune()
 // log autotune time history results for command, angular rate, and attitude
 void AC_AutoTune_Heli::Log_AutoTuneDetails()
 {
-    if (tune_type == SP_UP) {
+    if (tune_type == SP_UP || tune_type == TUNE_CHECK) {
         Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate);
     } else {
         Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f);

libraries/AC_AutoTune/AC_AutoTune_Heli.h

@@ -45,13 +45,6 @@ protected:
     //
     // methods to load and save gains
     //
-    // sweep_info contains information about a specific test's sweep results
-    struct sweep_info {
-        float freq;
-        float gain;
-        float phase;
-    };
-
 
     // backup original gains and prepare for start of tuning
     void backup_gains_and_initialise() override;
@@ -143,6 +136,13 @@ protected:
     uint32_t get_testing_step_timeout_ms() const override;
 
 private:
+    // sweep_info contains information about a specific test's sweep results
+    struct sweep_info {
+        float freq;
+        float gain;
+        float phase;
+    };
+
     // max_gain_data type stores information from the max gain test
     struct max_gain_data {
         float freq;