mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Tools: add CMSIS DSP module to waf for M4 ChibiOS and control inclusion of FFT based on HAL_WITH_DSP and GYROFFT_ENABLED. target appropriate ARM cpus
sophisticated autotest for Gyro FFT
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
593ff6818d
commit
88f0c26636
@ -96,6 +96,7 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [
|
||||
'AP_Hott_Telem',
|
||||
'AP_ESC_Telem',
|
||||
'AP_Stats',
|
||||
'AP_GyroFFT',
|
||||
]
|
||||
|
||||
def get_legacy_defines(sketch_name):
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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),
|
||||
|
||||
Loading…
Reference in New Issue
Block a user