autotest: test 256 FT windows and fix calculation of log-based FFT

fix quadplane FFT reference calculation
re-enable harmonic test
use median for measuring in-flight FFT average as it's much more reliable
relax quadplane filter restriction
harmonic switching test
tighten frequency check and loop twice to avoid heisenbugs

Tools/autotest/arducopter.py

@@ -3764,8 +3764,10 @@ class AutoTestCopter(AutoTest):
         smaxhz = int(maxhz * scale)
         freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
         peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
-        if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
+        if peakdb < dblevel:
             raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
+        elif peakhz is not None and abs(freq - peakhz) / peakhz > 0.05:
+            raise NotAchievedException("Did not detect a motor peak at %fHz, found %fHz at %fdB" % (peakhz, freq, peakdb))
         else:
             self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb))
 
@@ -3801,8 +3803,9 @@ class AutoTestCopter(AutoTest):
         # basic gyro sample rate test
         self.progress("Flying with gyro FFT harmonic - Gyro sample rate")
         self.context_push()
-
         ex = None
+        # we are dealing with probabalistic scenarios involving threads, have two bites at the cherry
+        for loop in ["first", "second"]:
             try:
                 self.start_subtest("Hover to calculate approximate hover frequency")
                 self.set_rc_default()
@@ -3844,6 +3847,45 @@ class AutoTestCopter(AutoTest):
 
                 self.hover_and_check_matched_frequency(-15, 100, 250, 64, None)
 
+                # Step 3: switch harmonics mid flight and check for tracking
+                self.set_parameter("FFT_HMNC_PEAK", 0)
+                self.reboot_sitl()
+
+                self.takeoff(10, mode="ALT_HOLD")
+
+                hover_time = 10
+                tstart = self.get_sim_time()
+                self.progress("Hovering for %u seconds" % hover_time)
+                while self.get_sim_time_cached() < tstart + hover_time:
+                    attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
+                vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
+
+                self.set_parameter("SIM_VIB_MOT_MULT", 5.0)
+
+                self.progress("Hovering for %u seconds" % hover_time)
+                while self.get_sim_time_cached() < tstart + hover_time:
+                    attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
+                vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
+                tend = self.get_sim_time()
+
+                self.do_RTL()
+
+                mlog = self.dfreader_for_current_onboard_log()
+                m = mlog.recv_match(type='FTN1', blocking=False,
+                            condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
+                freqs = []
+                while m is not None:
+                    freqs.append(m.PkAvg)
+                    m = mlog.recv_match(type='FTN1', blocking=False,
+                            condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
+
+                # peak within resolution of FFT length, the highest energy peak switched but our detection should not
+                pkAvg = numpy.median(numpy.asarray(freqs))
+                freqDelta = 1000. / self.get_parameter("FFT_WINDOW_SIZE")
+
+                if abs(pkAvg - freq) > freqDelta:
+                    raise NotAchievedException("FFT did not detect a harmonic motor peak, found %f, wanted %f" % (pkAvg, freq))
+
                 self.set_parameter("SIM_VIB_FREQ_X", 0)
                 self.set_parameter("SIM_VIB_FREQ_Y", 0)
                 self.set_parameter("SIM_VIB_FREQ_Z", 0)
@@ -3853,8 +3895,11 @@ class AutoTestCopter(AutoTest):
                 self.reboot_sitl()
 
             except Exception as e:
-            self.progress("Exception caught: %s" % (self.get_exception_stacktrace(e)))
+                self.progress("Exception caught in %s loop: %s" % (loop, self.get_exception_stacktrace(e)))
+                if loop is not "second":
+                    continue
                 ex = e
+            break
 
         self.context_pop()
 
@@ -3868,6 +3913,8 @@ class AutoTestCopter(AutoTest):
         self.context_push()
 
         ex = None
+        # we are dealing with probabalistic scenarios involving threads, have two bites at the cherry
+        for loop in ["first" "second"]:
             try:
                 self.set_rc_default()
                 # magic tridge EKF type that dramatically speeds up the test
@@ -3890,6 +3937,7 @@ class AutoTestCopter(AutoTest):
                 self.set_parameter("FFT_SNR_REF", 10)
                 self.set_parameter("FFT_WINDOW_SIZE", 128)
                 self.set_parameter("FFT_WINDOW_OLAP", 0.75)
+                self.set_parameter("FFT_SAMPLE_MODE", 0)
 
                 # Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft
                 # can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so
@@ -3906,7 +3954,7 @@ class AutoTestCopter(AutoTest):
 
                 # Step 1b: run the same test with an FFT length of 256 which is needed to flush out a
                 # whole host of bugs related to uint8_t. This also tests very accurately the frequency resolution
-            self.set_parameter("FFT_WINDOW_SIZE", 128) # requires #13741 to work at 256
+                self.set_parameter("FFT_WINDOW_SIZE", 256)
                 self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")
 
                 self.reboot_sitl()
@@ -3958,13 +4006,17 @@ class AutoTestCopter(AutoTest):
 
                 self.do_RTL()
                 psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
-            freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
-            dblevel = numpy.amax(psd["X"][ignore_bins:])
 
-            if dblevel < -9:
+                # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
-                self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, dblevel))
+                scale = 1000. / 1024.
+                sminhz = int(100 * scale)
+                smaxhz = int(350 * scale)
+                freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
+                peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
+                if peakdb < -9:
+                    self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
                 else:
-                raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, dblevel))
+                    raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, peakdb))
 
                 # Step 4: loop sample rate test with larger window
                 self.start_subtest("Hover and check that the FFT can find the motor noise when running at fast loop rate")
@@ -3991,8 +4043,11 @@ class AutoTestCopter(AutoTest):
                 self.reboot_sitl()
 
             except Exception as e:
-            self.progress("Exception caught: %s" % (self.get_exception_stacktrace(e)))
+                self.progress("Exception caught in %s loop: %s" % (loop, self.get_exception_stacktrace(e)))
+                if loop is not "second":
+                    continue
                 ex = e
+            break
 
         self.context_pop()
 
@@ -5087,7 +5142,6 @@ class AutoTestCopter(AutoTest):
             "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",
             "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525",
             "BeaconPosition": "See https://github.com/ArduPilot/ardupilot/issues/11689",
-            "GyroFFT": "Temporarily disabled due to flapping test",
         }
 
 class AutoTestHeli(AutoTestCopter):