From 39e948a40afabd86b2ea4b333be3293b0035f96c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 3 Oct 2019 17:12:34 +0100 Subject: [PATCH] Tools: add motor vibration test and support for post-test fft perform FFT analysis on vibration output --- Tools/autotest/arducopter.py | 73 ++++++++++++++++++++++++++- Tools/autotest/common.py | 96 ++++++++++++++++++++++++++++++++++++ 2 files changed, 168 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d6e6f0861e..76568c0125 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7,6 +7,7 @@ import math import os import shutil import time +import numpy from pymavlink import mavutil from pymavlink import mavextra @@ -1847,6 +1848,73 @@ class AutoTestCopter(AutoTest): self.do_RTL() + def fly_motor_vibration(self): + """Test flight with motor vibration""" + self.context_push() + + ex = None + try: + self.set_rc_default() + self.set_parameter("INS_LOG_BAT_MASK", 3) + self.set_parameter("INS_LOG_BAT_OPT", 2) + self.set_parameter("LOG_BITMASK", 958) + self.set_parameter("LOG_DISARMED", 0) + self.set_parameter("SIM_VIB_MOT_MAX", 350) + self.set_parameter("SIM_GYR_RND", 100) + self.set_parameter("SIM_DRIFT_SPEED", 0) + self.set_parameter("SIM_DRIFT_TIME", 0) + self.reboot_sitl() + + self.takeoff(10, mode="LOITER") + self.change_alt(alt_min=15) + + hover_time = 5 + 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) + tend = self.get_sim_time() + + self.do_RTL() + psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + # ignore the first 20Hz + ignore_bins = 20 + freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] + if numpy.amax(psd["X"][ignore_bins:]) < -22 or freq < 200 or freq > 300: + raise NotAchievedException("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:]))) + else: + self.progress("Detected motor peak at %fHz" % freq) + + # now add a notch and check that the peak is squashed + self.set_parameter("INS_NOTCH_ENABLE", 1) + self.set_parameter("INS_NOTCH_FREQ", freq) + self.set_parameter("INS_NOTCH_ATT", 50) + self.set_parameter("INS_NOTCH_BW", freq/2) + self.reboot_sitl() + + self.takeoff(10, mode="LOITER") + self.change_alt(alt_min=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) + tend = self.get_sim_time() + 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] + if numpy.amax(psd["X"][ignore_bins:]) < -22: + self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:]))) + else: + raise NotAchievedException("Detected motor peak at %f Hz" % (freq)) + except Exception as e: + ex = e + + self.context_pop() + + if ex is not None: + raise ex + def fly_vision_position(self): """Disable GPS navigation, enable Vicon input.""" # scribble down a location we can set origin to: @@ -4407,12 +4475,15 @@ class AutoTestCopter(AutoTest): "Test rangefinder drivers", self.fly_rangefinder_drivers), + ("MotorVibration", + "Fly motor vibration test", + self.fly_motor_vibration), + ("LogDownLoad", "Log download", lambda: self.log_download( self.buildlogs_path("ArduCopter-log.bin"), upload_logs=len(self.fail_list) > 0)) - ]) return ret diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 5682b953bc..e27047745d 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -11,6 +11,7 @@ import traceback import pexpect import fnmatch import operator +import numpy from MAVProxy.modules.lib import mp_util @@ -3713,3 +3714,98 @@ switch value''' ret = self.run_tests(tests) self.post_tests_announcements() return ret + + def mavfft_fttd(self, sensor_type, sensor_instance, since, until): + '''display fft for raw ACC data in current logfile''' + + '''object to store data about a single FFT plot''' + class MessageData(object): + def __init__(self, ffth): + self.seqno = -1 + self.fftnum = ffth.N + self.sensor_type = ffth.type + self.instance = ffth.instance + self.sample_rate_hz = ffth.smp_rate + self.multiplier = ffth.mul + self.sample_us = ffth.SampleUS + self.data = {} + self.data["X"] = [] + self.data["Y"] = [] + self.data["Z"] = [] + self.holes = False + self.freq = None + + def add_fftd(self, fftd): + self.seqno += 1 + self.data["X"].extend(fftd.x) + self.data["Y"].extend(fftd.y) + self.data["Z"].extend(fftd.z) + + mlog = self.dfreader_for_current_onboard_log() + + # see https://holometer.fnal.gov/GH_FFT.pdf for a description of the techniques used here + messages = [] + messagedata = None + while True: + m = mlog.recv_match() + if m is None: + break + msg_type = m.get_type() + if msg_type == "ISBH": + if messagedata is not None: + if messagedata.sensor_type == sensor_type and messagedata.instance == sensor_instance and messagedata.sample_us > since and messagedata.sample_us < until: + messages.append(messagedata) + messagedata = MessageData(m) + continue + + if msg_type == "ISBD": + if messagedata is not None and messagedata.sensor_type == sensor_type and messagedata.instance == sensor_instance: + messagedata.add_fftd(m) + + fft_len = len(messages[0].data["X"]) + sum_fft = { + "X": numpy.zeros(fft_len/2+1), + "Y": numpy.zeros(fft_len/2+1), + "Z": numpy.zeros(fft_len/2+1), + } + sample_rate = 0 + counts = 0 + window = numpy.hanning(fft_len) + freqmap = numpy.fft.rfftfreq(fft_len, 1.0 / messages[0].sample_rate_hz) + + # calculate NEBW constant + S2 = numpy.inner(window, window) + + for message in messages: + for axis in [ "X","Y","Z" ]: + # normalize data and convert to dps in order to produce more meaningful magnitudes + if message.sensor_type == 1: + d = numpy.array(numpy.degrees(message.data[axis])) / float(message.multiplier) + else: + d = numpy.array(message.data[axis]) / float(message.multiplier) + + # apply window to the input + d *= window + # perform RFFT + d_fft = numpy.fft.rfft(d) + # convert to squared complex magnitude + d_fft = numpy.square(abs(d_fft)) + # remove DC component + d_fft[0] = 0 + d_fft[-1] = 0 + # accumulate the sums + sum_fft[axis] += d_fft + + sample_rate = message.sample_rate_hz + counts += 1 + + numpy.seterr(divide = 'ignore') + psd = {} + for axis in [ "X","Y","Z" ]: + # normalize output to averaged PSD + psd[axis] = 2 * (sum_fft[axis] / counts) / (sample_rate * S2) + psd[axis] = 10 * numpy.log10 (psd[axis]) + + psd["F"] = freqmap + + return psd