mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 14:08:45 -04:00
Tools: add motor vibration test and support for post-test fft
perform FFT analysis on vibration output
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
449e9ee13d
commit
39e948a40a
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user