def fly_gyro_fft_harmonic(self): """Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic.""" # 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() # magic tridge EKF type that dramatically speeds up the test self.set_parameter("AHRS_EKF_TYPE", 10) self.set_parameter("EK2_ENABLE", 0) self.set_parameter("INS_LOG_BAT_MASK", 3) self.set_parameter("INS_LOG_BAT_OPT", 0) self.set_parameter("INS_GYRO_FILTER", 100) self.set_parameter("INS_FAST_SAMPLE", 0) self.set_parameter("LOG_BITMASK", 958) self.set_parameter("LOG_DISARMED", 0) self.set_parameter("SIM_DRIFT_SPEED", 0) self.set_parameter("SIM_DRIFT_TIME", 0) self.set_parameter("FFT_THR_REF", self.get_parameter("MOT_THST_HOVER")) # enable a noisy motor peak self.set_parameter("SIM_GYR_RND", 20) # enabling FFT will also enable the arming check, self-testing the functionality self.set_parameter("FFT_ENABLE", 1) self.set_parameter("FFT_MINHZ", 50) self.set_parameter("FFT_MAXHZ", 450) self.set_parameter("FFT_SNR_REF", 10) # Step 1: inject actual motor noise and use the FFT to track it self.set_parameter("SIM_VIB_MOT_MAX", 250) # gives a motor peak at about 175Hz self.set_parameter("FFT_WINDOW_SIZE", 64) self.set_parameter("FFT_WINDOW_OLAP", 0.75) self.reboot_sitl() freq = self.hover_and_check_matched_frequency(-15, 100, 250, 64) # Step 2: add a second harmonic and check the first is still tracked self.start_subtest("Add a fixed frequency harmonic at twice the hover frequency and check the right harmonic is found") self.set_parameter("SIM_VIB_FREQ_X", freq * 2) self.set_parameter("SIM_VIB_FREQ_Y", freq * 2) self.set_parameter("SIM_VIB_FREQ_Z", freq * 2) self.set_parameter("SIM_VIB_MOT_MULT", 0.25) # halve the motor noise so that the higher harmonic dominates self.reboot_sitl() self.hover_and_check_matched_frequency(-15, 100, 250, 64, None) # Step 3: switch harmonics mid flight and check for tracking self.start_subtest("Switch harmonics mid flight and check the right harmonic is found") 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)) # Step 4: dynamic harmonic self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated") # find a motor peak freq, vfr_hud, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350) # now add a dynamic notch and check that the peak is squashed self.set_parameter("INS_LOG_BAT_OPT", 2) self.set_parameter("INS_HNTCH_ENABLE", 1) self.set_parameter("INS_HNTCH_HMNCS", 3) self.set_parameter("INS_HNTCH_MODE", 4) self.set_parameter("INS_HNTCH_FREQ", freq) #self.set_parameter("INS_HNTCH_REF", 1.0) self.set_parameter("INS_HNTCH_REF", vfr_hud.throttle/100.) self.set_parameter("INS_HNTCH_ATT", 100) self.set_parameter("INS_HNTCH_BW", freq/2) self.set_parameter("INS_HNTCH_OPTS", 3) self.reboot_sitl() # 5db is far in excess of the attenuation that the double dynamic-harmonic notch is able # to provide (-7dB on average), but without the notch the peak is around 20dB so still a safe test self.hover_and_check_matched_frequency_with_fft(5, 100, 350, reverse=True) self.set_parameter("SIM_VIB_FREQ_X", 0) self.set_parameter("SIM_VIB_FREQ_Y", 0) self.set_parameter("SIM_VIB_FREQ_Z", 0) self.set_parameter("SIM_VIB_MOT_MULT", 1.) # prevent update parameters from messing with the settings when we pop the context self.set_parameter("FFT_ENABLE", 0) self.reboot_sitl() except Exception as e: self.progress("Exception caught in %s loop: %s" % (loop, self.get_exception_stacktrace(e))) if loop != "second": continue ex = e break self.context_pop() # need a final reboot because weird things happen to your # vehicle state when switching back from EKF type 10! self.reboot_sitl()