diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index a71e309cae..e98d49ea10 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -96,6 +96,7 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ 'AP_Hott_Telem', 'AP_ESC_Telem', 'AP_Stats', + 'AP_GyroFFT', ] def get_legacy_defines(sketch_name): diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index fb76c38275..5d6fcb3383 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -490,6 +490,7 @@ class chibios(Board): '--specs=nano.specs', '-specs=nosys.specs', '-DCHIBIOS_BOARD_NAME="%s"' % self.name, + '-D__USE_CMSIS', ] env.CXXFLAGS += env.CFLAGS + [ '-fno-rtti', @@ -525,6 +526,7 @@ class chibios(Board): '-L%s' % env.BUILDROOT, '-L%s' % cfg.srcnode.make_node('modules/ChibiOS/os/common/startup/ARMCMx/compilers/GCC/ld/').abspath(), '-L%s' % cfg.srcnode.make_node('libraries/AP_HAL_ChibiOS/hwdef/common/').abspath(), + '-L%s' % cfg.srcnode.make_node('libraries/AP_GyroFFT/CMSIS_5/lib/').abspath(), '-Wl,--gc-sections,--no-warn-mismatch,--library-path=/ld,--script=ldscript.ld,--defsym=__process_stack_size__=%s,--defsym=__main_stack_size__=%s' % (cfg.env.PROCESS_STACK, cfg.env.MAIN_STACK) ] @@ -551,6 +553,10 @@ class chibios(Board): 'ChibiOS', ] + env.INCLUDES += [ + cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath() + ] + try: import intelhex env.HAVE_INTEL_HEX = True diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 06f5638964..2aa2ea667b 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -374,7 +374,6 @@ def configure(cfg): cfg.fatal("Failed to process hwdef.dat") if ret != 0: cfg.fatal("Failed to process hwdef.dat ret=%d" % ret) - load_env_vars(cfg.env) if env.HAL_WITH_UAVCAN: setup_can_build(cfg) @@ -421,8 +420,12 @@ def build(bld): target=bld.bldnode.find_or_declare('modules/ChibiOS/libch.a') ) ch_task.name = "ChibiOS_lib" - - bld.env.LIB += ['ch'] + if bld.env.CORTEX == 'cortex-m4': + bld.env.LIB += ['ch', 'arm_cortexM4lf_math'] + elif bld.env.CORTEX == 'cortex-m7': + bld.env.LIB += ['ch', 'arm_cortexM7lfdp_math'] + else: + bld.env.LIB += ['ch'] bld.env.LIBPATH += ['modules/ChibiOS/'] # list of functions that will be wrapped to move them out of libc into our # own code note that we also include functions that we deliberately don't diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 6133af2864..45bdc83e47 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1836,6 +1836,8 @@ class AutoTestCopter(AutoTest): ex = None try: self.set_rc_default() + # magic tridge EKF type that dramatically speeds up the test + self.set_parameter("AHRS_EKF_TYPE", 10) self.set_parameter("INS_LOG_BAT_MASK", 3) self.set_parameter("INS_LOG_BAT_OPT", 0) self.set_parameter("LOG_BITMASK", 958) @@ -3625,6 +3627,8 @@ class AutoTestCopter(AutoTest): def fly_dynamic_notches(self): self.progress("Flying with dynamic notches") + # magic tridge EKF type that dramatically speeds up the test + self.set_parameter("AHRS_EKF_TYPE", 10) self.set_parameter("INS_HNTCH_ENABLE", 1) self.set_parameter("INS_HNTCH_FREQ", 80) self.set_parameter("INS_HNTCH_REF", 0.35) @@ -3635,6 +3639,7 @@ class AutoTestCopter(AutoTest): self.reboot_sitl() self.set_parameter("SIM_GYR_RND", 10) + self.takeoff(10, mode="LOITER") self.change_mode("ALT_HOLD") @@ -3647,6 +3652,186 @@ class AutoTestCopter(AutoTest): self.do_RTL() + def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None): + + # find a motor peak + self.takeoff(10, mode="ALT_HOLD") + + hover_time = 15 + 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) + tend = self.get_sim_time() + + self.do_RTL() + psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + + # batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin + freq = psd["F"][numpy.argmax(psd["X"][minhz:maxhz]) + minhz] * (1000. / 1024.) + peakdb = numpy.amax(psd["X"][minhz:maxhz]) + if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05): + raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb)) + else: + self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb)) + + # we have a peak make sure that the FFT detected something close + # logging is at 10Hz + mlog = self.dfreader_for_current_onboard_log() + pkAvg = freq + nmessages = 1 + + m = mlog.recv_match(type='FTN1', blocking=False, + condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6)) + + while m is not None: + nmessages = nmessages + 1 + pkAvg = pkAvg + (0.1 * (m.PkAvg - pkAvg)) + m = mlog.recv_match(type='FTN1', blocking=False, + condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6)) + + self.progress("Detected motor peak at %fHz processing %d messages" % (pkAvg, nmessages)) + + # peak within 5% + if abs(pkAvg - freq) / freq > 0.1: + raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq)) + + return freq + + def fly_gyro_fft(self): + """Use dynamic harmonic notch to control motor noise.""" + # basic gyro sample rate test + self.progress("Flying with gyro FFT - Gyro sample rate") + self.context_push() + + ex = None + try: + 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) + # 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) + self.set_parameter("FFT_WINDOW_SIZE", 128) + self.set_parameter("FFT_WINDOW_OLAP", 0.75) + + # 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 + # a 250Hz peak should be detectable within 5% + self.set_parameter("SIM_VIB_FREQ_X", 250) + self.set_parameter("SIM_VIB_FREQ_Y", 250) + self.set_parameter("SIM_VIB_FREQ_Z", 250) + + self.reboot_sitl() + + # find a motor peak + self.hover_and_check_matched_frequency(-15, 100, 350, 250) + + # Step 2: inject actual motor noise and use the standard length FFT to track it + 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_MAX", 250) # gives a motor peak at about 175Hz + self.set_parameter("FFT_WINDOW_SIZE", 32) + self.set_parameter("FFT_WINDOW_OLAP", 0.5) + + self.reboot_sitl() + freq = self.hover_and_check_matched_frequency(-15, 100, 250) + + # Step 2a: add a second harmonic and check the first is still tracked + 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) + 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.) + + # Step 3: add a FFT 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_FREQ", freq) + self.set_parameter("INS_HNTCH_REF", 1.0) + self.set_parameter("INS_HNTCH_ATT", 50) + self.set_parameter("INS_HNTCH_BW", freq/2) + self.set_parameter("INS_HNTCH_MODE", 4) + self.reboot_sitl() + + self.takeoff(10, mode="ALT_HOLD") + hover_time = 15 + ignore_bins = 20 + self.progress("Hovering for %u seconds" % hover_time) + tstart = self.get_sim_time() + while self.get_sim_time_cached() < tstart + hover_time: + attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) + tend = self.get_sim_time() + + # fly fast forrest! + self.set_rc(3, 1900) + self.set_rc(2, 1200) + self.wait_groundspeed(5, 1000) + self.set_rc(3, 1500) + self.set_rc(2, 1500) + + 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 < -10: + self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, dblevel)) + else: + raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, dblevel)) + + # Step 4: loop sample rate test with larger window + self.progress("Flying with gyro FFT - Fast loop rate") + # we are limited to half the loop rate for frequency detection + self.set_parameter("FFT_MAXHZ", 185) + self.set_parameter("INS_LOG_BAT_OPT", 0) + self.set_parameter("SIM_VIB_MOT_MAX", 220) + self.set_parameter("FFT_WINDOW_SIZE", 64) + self.set_parameter("FFT_WINDOW_OLAP", 0.75) + self.set_parameter("FFT_SAMPLE_MODE", 1) + self.reboot_sitl() + + self.takeoff(10, mode="ALT_HOLD") + + self.progress("Hovering for %u seconds" % hover_time) + tstart = self.get_sim_time() + while self.get_sim_time_cached() < tstart + hover_time: + attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) + tend = self.get_sim_time() + + self.do_RTL() + # 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: + ex = e + + self.context_pop() + + if ex is not None: + raise ex + def test_onboard_compass_calibration(self, timeout=300): params = [ ("SIM_MAG_DIA_X", "COMPASS_DIA_X", 1.0), @@ -4631,6 +4816,10 @@ class AutoTestCopter(AutoTest): "Fly Dynamic Notches", self.fly_dynamic_notches), + ("GyroFFT", + "Fly Gyro FFT", + self.fly_gyro_fft), + ("RangeFinderDrivers", "Test rangefinder drivers", self.fly_rangefinder_drivers),