|
|
|
@ -411,9 +411,9 @@ class AutoTestQuadPlane(AutoTest):
@@ -411,9 +411,9 @@ class AutoTestQuadPlane(AutoTest):
|
|
|
|
|
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz] |
|
|
|
|
peakdb = numpy.amax(psd["X"][sminhz:smaxhz]) |
|
|
|
|
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)) |
|
|
|
|
raise NotAchievedException("No motor peak, found %fHz at %fdB" % (freq, peakdb)) |
|
|
|
|
else: |
|
|
|
|
self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb)) |
|
|
|
|
self.progress("motor peak %fHz, thr %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb)) |
|
|
|
|
|
|
|
|
|
# we have a peak make sure that the FFT detected something close |
|
|
|
|
# logging is at 10Hz |
|
|
|
@ -512,10 +512,11 @@ class AutoTestQuadPlane(AutoTest):
@@ -512,10 +512,11 @@ class AutoTestQuadPlane(AutoTest):
|
|
|
|
|
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:]) < -10: |
|
|
|
|
self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:]))) |
|
|
|
|
peakdB = numpy.amax(psd["X"][ignore_bins:]) |
|
|
|
|
if peakdB < -10: |
|
|
|
|
self.progress("No motor peak, %f at %f dB" % (freq, peakdB)) |
|
|
|
|
else: |
|
|
|
|
raise NotAchievedException("Detected motor peak at %f Hz" % (freq)) |
|
|
|
|
raise NotAchievedException("Detected peak at %f Hz of %.2f dB" % (freq, peakdB)) |
|
|
|
|
|
|
|
|
|
# Step 4: take off as a copter land as a plane, make sure we track |
|
|
|
|
self.progress("Flying with gyro FFT - vtol to plane") |
|
|
|
|