mavfft_isb.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 extract ISBH and ISBD messages from AP_Logging files and produce FFT plots
5 '''
6 from __future__ import print_function
7 
8 import numpy
9 import os
10 import pylab
11 import sys
12 import time
13 
14 from argparse import ArgumentParser
15 parser = ArgumentParser(description=__doc__)
16 parser.add_argument("--condition", default=None, help="select packets by condition")
17 parser.add_argument("logs", metavar="LOG", nargs="+")
18 
19 args = parser.parse_args()
20 
21 from pymavlink import mavutil
22 
23 def mavfft_fttd(logfile):
24  '''display fft for raw ACC data in logfile'''
25 
26  '''object to store data about a single FFT plot'''
27  class PlotData(object):
28  def __init__(self, ffth):
29  self.seqno = -1
30  self.fftnum = ffth.N
31  self.sensor_type = ffth.type
32  self.instance = ffth.instance
33  self.sample_rate_hz = ffth.smp_rate
34  self.multiplier = ffth.mul
35  self.data = {}
36  self.data["X"] = []
37  self.data["Y"] = []
38  self.data["Z"] = []
39  self.holes = False
40  self.freq = None
41 
42  def add_fftd(self, fftd):
43  if fftd.N != self.fftnum:
44  print("Skipping ISBD with wrong fftnum (%u vs %u)\n" % (fftd.fftnum, self.fftnum))
45  return
46  if self.holes:
47  print("Skipping ISBD(%u) for ISBH(%u) with holes in it" % (fftd.seqno, self.fftnum))
48  return
49  if fftd.seqno != self.seqno+1:
50  print("ISBH(%u) has holes in it" % fftd.N)
51  self.holes = True
52  return
53  self.seqno += 1
54  self.data["X"].extend(fftd.x)
55  self.data["Y"].extend(fftd.y)
56  self.data["Z"].extend(fftd.z)
57 
58  def prefix(self):
59  if self.sensor_type == 0:
60  return "Accel"
61  elif self.sensor_type == 1:
62  return "Gyro"
63  else:
64  return "?Unknown Sensor Type?"
65 
66  def tag(self):
67  return str(self)
68 
69  def __str__(self):
70  return "%s[%u]" % (self.prefix(), self.instance)
71 
72  print("Processing log %s" % filename)
73  mlog = mavutil.mavlink_connection(filename)
74 
75  things_to_plot = []
76  plotdata = None
77  msgcount = 0
78  start_time = time.time()
79  while True:
80  m = mlog.recv_match(condition=args.condition)
81  if m is None:
82  break
83  msgcount += 1
84  if msgcount % 1000 == 0:
85  sys.stderr.write(".")
86  msg_type = m.get_type()
87  if msg_type == "ISBH":
88  if plotdata is not None:
89  # close off previous data collection
90  things_to_plot.append(plotdata)
91  # initialise plot-data collection object
92  plotdata = PlotData(m)
93  continue
94 
95  if msg_type == "ISBD":
96  if plotdata is None:
97  print("Skipping ISBD outside ISBH (fftnum=%u)\n" % m.N)
98  continue
99  plotdata.add_fftd(m)
100 
101  print("", file=sys.stderr)
102  time_delta = time.time() - start_time
103  print("%us messages %u messages/second %u kB/second" % (msgcount, msgcount/time_delta, os.stat(filename).st_size/time_delta))
104  print("Extracted %u fft data sets" % len(things_to_plot), file=sys.stderr)
105 
106  sum_fft = {}
107  freqmap = {}
108  count = 0
109 
110  first_freq = None
111  for thing_to_plot in things_to_plot:
112  for axis in [ "X","Y","Z" ]:
113  d = numpy.array(thing_to_plot.data[axis])/float(thing_to_plot.multiplier)
114  if len(d) == 0:
115  print("No data?!?!?!")
116  continue
117 
118  avg = numpy.sum(d) / len(d)
119  d -= avg
120  d_fft = numpy.fft.rfft(d)
121  if thing_to_plot.tag() not in sum_fft:
122  sum_fft[thing_to_plot.tag()] = {
123  "X": 0,
124  "Y": 0,
125  "Z": 0
126  }
127  sum_fft[thing_to_plot.tag()][axis] = numpy.add(sum_fft[thing_to_plot.tag()][axis], d_fft)
128  count += 1
129  freq = numpy.fft.rfftfreq(len(d), 1.0/thing_to_plot.sample_rate_hz)
130  freqmap[thing_to_plot.tag()] = freq
131 
132  for sensor in sum_fft:
133  print("Sensor: %s" % str(sensor))
134  pylab.figure(str(sensor))
135  for axis in [ "X","Y","Z" ]:
136  pylab.plot(freqmap[sensor], numpy.abs(sum_fft[sensor][axis]/count), label=axis)
137  pylab.legend(loc='upper right')
138  pylab.xlabel('Hz')
139 
140 for filename in args.logs:
141  mavfft_fttd(filename)
142 
143 pylab.show()


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:06