mavfft_int.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 interactively select accel and gyro data for FFT analysis
5 '''
6 from __future__ import print_function
7 
8 import numpy
9 import pylab
10 import matplotlib
11 import matplotlib.pyplot as pyplot
12 
13 from argparse import ArgumentParser
14 parser = ArgumentParser(description=__doc__)
15 parser.add_argument("--condition", default=None, help="select packets by condition")
16 parser.add_argument("logs", metavar="LOG", nargs="+")
17 
18 args = parser.parse_args()
19 
20 from pymavlink import mavutil
21 
22 try:
23  raw_input # Python 2
24 except NameError:
25  raw_input = input # Python 3
26 
27 def plot_input(data, msg, prefix, start, end):
28  preview = pylab.figure()
29  preview.set_size_inches(12, 3, forward=True)
30  for axis in ['X', 'Y', 'Z']:
31  field = msg + '.' + prefix + axis
32  d = numpy.array(data[field][start:end])
33  pylab.plot( d, marker='.', label=field )
34  pylab.legend(loc='upper right')
35 # pylab.ylabel('m/sec/sec')
36  pylab.subplots_adjust(left=0.06, right=0.95, top=0.95, bottom=0.16)
37  preview.canvas.set_window_title('FFT input: ' + msg)
38  pylab.show()
39 
40 def check_drops(data, msg, start, end):
41  ts = 1e-6 * numpy.array(data[msg + '.TimeUS'])
42  seqcnt = numpy.array(data[msg + '.SampleC'])
43 
44  deltas = numpy.diff(seqcnt[start:end])
45 # print('ndeltas: ', len(deltas))
46  duration = ts[end] - ts[start]
47  print(msg + ' duration: {0:.3f} seconds'.format(duration))
48  avg_rate = float(end - start - 1) / duration
49  print('average logging rate: {0:.0f} Hz'.format(avg_rate))
50  ts_mean = numpy.mean(deltas)
51  dmin = numpy.min(deltas)
52  dmax = numpy.max(deltas)
53  print('sample count delta min: {0}, max: {1}'.format(dmin, dmax))
54  if (dmin != dmax):
55  print('sample count delta mean: ', '{0:.2f}, std: {0:.2f}'.format(ts_mean, numpy.std(deltas)))
56  print('sensor sample rate: {0:.0f} Hz'.format(ts_mean * avg_rate))
57 
58  drop_lens = []
59  drop_times = []
60  intvl_count = [0]
61  for i in range(0, len(deltas)):
62  if (deltas[i] > 1.5 * ts_mean):
63  drop_lens.append(deltas[i])
64  drop_times.append(ts[start+i])
65  print('dropout at sample {0}: length {1}'.format(start+i, deltas[i]))
66 
67  print('{0:d} sample intervals > {1:.3f}'.format(len(drop_lens), 1.5 * ts_mean))
68  return avg_rate
69 
70 def fft(logfile):
71  '''display fft for raw ACC data in logfile'''
72 
73  print("Processing log %s" % filename)
74  mlog = mavutil.mavlink_connection(filename)
75 
76  data = {'ACC1.rate' : 1000,
77  'ACC2.rate' : 1600,
78  'ACC3.rate' : 1000,
79  'GYR1.rate' : 1000,
80  'GYR2.rate' : 800,
81  'GYR3.rate' : 1000}
82  for acc in ['ACC1','ACC2','ACC3']:
83  for ax in ['AccX', 'AccY', 'AccZ', 'SampleC', 'TimeUS']:
84  data[acc+'.'+ax] = []
85  for gyr in ['GYR1','GYR2','GYR3']:
86  for ax in ['GyrX', 'GyrY', 'GyrZ', 'SampleC', 'TimeUS']:
87  data[gyr+'.'+ax] = []
88 
89  # now gather all the data
90  while True:
91  m = mlog.recv_match(condition=args.condition)
92  if m is None:
93  break
94  type = m.get_type()
95  if type.startswith("ACC"):
96  data[type+'.AccX'].append(m.AccX)
97  data[type+'.AccY'].append(m.AccY)
98  data[type+'.AccZ'].append(m.AccZ)
99  data[type+'.SampleC'].append(m.SampleC)
100  data[type+'.TimeUS'].append(m.TimeUS)
101  if type.startswith("GYR"):
102  data[type+'.GyrX'].append(m.GyrX)
103  data[type+'.GyrY'].append(m.GyrY)
104  data[type+'.GyrZ'].append(m.GyrZ)
105  data[type+'.SampleC'].append(m.SampleC)
106  data[type+'.TimeUS'].append(m.TimeUS)
107 
108  # SampleC is just a sample counter
109  ts = 1e-6 * numpy.array(data['ACC1.TimeUS'])
110  seqcnt = numpy.array(data['ACC1.SampleC'])
111 
112  print("Extracted %u data points" % len(data['ACC1.AccX']))
113 
114  # interactive selection of analysis window
115  preview = pylab.figure()
116  preview.set_size_inches(12, 3, forward=True)
117  msg = 'ACC1'
118  for axis in ['X', 'Y', 'Z']:
119  field = msg + '.Acc' + axis
120  d = numpy.array(data[field])
121  pylab.plot( d, marker='.', label=field )
122  pylab.legend(loc='upper right')
123  pylab.ylabel('m/sec/sec')
124  pylab.subplots_adjust(left=0.06, right=0.95, top=0.95, bottom=0.16)
125  pylab.show()
126  currentAxes = preview.gca()
127  s_start = 0
128  s_end = len(ts)-1
129  n_samp = s_end - s_start
130  currentAxes.set_xlim(s_start, s_end)
131 
132  # outer loop for repeating time window selection
133  while True:
134 
135  while True:
136  print('select sample range for fft analysis')
137  preview.canvas.set_window_title('select sample range')
138  try:
139  s_start = input('start sample: ')
140  s_end = input('end sample: ')
141  currentAxes.set_xlim(s_start, s_end)
142  except:
143  break
144 
145  # process selected samples
146  s_start = int(currentAxes.get_xlim()[0])
147  s_end = int(currentAxes.get_xlim()[1])
148  n_samp = s_end - s_start
149  print('sample range: ', s_start, s_end)
150  print('N samples: ', n_samp)
151 
152  # check for dropouts: (delta > 1)
153  avg_rate = check_drops(data, 'ACC1', s_start, s_end)
154 
155  title = 'FFT input: {0:s} ACC1[{1:d}:{2:d}], {3:d} samples'.format(logfile, s_start, s_end, n_samp)
156  currentAxes.set_xlabel('sample index : nsamples: {0:d}, avg rate: {1:.0f} Hz'.format(n_samp, avg_rate))
157  preview.canvas.set_window_title(title)
158  preview.savefig('acc1z.png')
159 
160  for msg in ['ACC1', 'GYR1', 'ACC2', 'GYR2']:
161  if msg.startswith('ACC'):
162  prefix = 'Acc'
163  title = '{2} FFT [{0:d}:{1:d}]'.format(s_start, s_end, msg)
164  else:
165  prefix = 'Gyr'
166  title = '{2} FFT [{0:d}:{1:d}]'.format(s_start, s_end, msg)
167 
168  # check for dropouts
169  data[msg+'.rate'] = check_drops(data, msg, s_start, s_end)
170  plot_input(data, msg, prefix, s_start, s_end)
171 
172  fftwin = pylab.figure()
173  fftwin.set_size_inches(12, 3, forward=True)
174  f_res = float(data[msg+'.rate']) / n_samp
175 
176  max_fft = 0
177  abs_fft = {}
178  index = 0
179  avg = {'X':0, 'Y':0, 'Z':0}
180  for axis in ['X', 'Y', 'Z']:
181  field = msg + '.' + prefix + axis
182  d = data[field][s_start:s_end]
183  if len(d) == 0:
184  continue
185 
186  d = numpy.array(d)
187  freq = numpy.fft.rfftfreq(len(d), 1.0 / data[msg+'.rate'])
188  # remove mean
189  avg[axis] = numpy.mean(d)
190  d -= avg[axis]
191  print('{1} DC component: {0:.3f}'.format(avg[axis], field))
192  # transform
193  d_fft = numpy.fft.rfft(d)
194  abs_fft[axis] = numpy.abs(d_fft)
195  # remember the max over all axes
196  thismax = numpy.max(abs_fft[axis])
197  if (max_fft < thismax):
198  max_fft = thismax
199  index += 1
200 
201  for axis in ['X', 'Y', 'Z']:
202  # scale to 0dB = max
203  field = msg + '.' + prefix + axis
204  db_fft = 20 * numpy.log10(abs_fft[axis] / max_fft)
205  pylab.plot( freq, db_fft, label=field )
206 
207  fftwin.canvas.set_window_title(title)
208  fftwin.gca().set_ylim(-90, 0)
209  pylab.legend(loc='upper right')
210  pylab.xlabel('Hz : res: ' + '{0:.3f}'.format(f_res))
211  pylab.ylabel('dB X {0:.3f} Y {1:.3f} Z {2:.3f}\n'.format(avg['X'], avg['Y'], avg['Z']))
212  pylab.subplots_adjust(left=0.1, right=0.95, top=0.95, bottom=0.16)
213  fftwin.savefig(msg + '_fft.png')
214 
215  try:
216  selection = raw_input('q to proceed to next file, anything else to select a new range: ')
217  print(selection)
218  except:
219  continue
220 
221  if (selection == 'q'):
222  break
223 
224 pylab.ion()
225 for filename in args.logs:
226  fft(filename)
227 
228 print('type ctrl-c to close windows and exit')
229 pylab.ioff()
230 pylab.show()
231 
pymavlink.tools.mavfft_int.raw_input
raw_input
Definition: mavfft_int.py:25
pymavlink.tools.mavfft_int.plot_input
def plot_input(data, msg, prefix, start, end)
Definition: mavfft_int.py:27
pymavlink.tools.mavfft_int.fft
def fft(logfile)
Definition: mavfft_int.py:70
pymavlink.tools.mavfft_int.check_drops
def check_drops(data, msg, start, end)
Definition: mavfft_int.py:40

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