mavfft.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer offsets
5 '''
6 from __future__ import print_function
7 
8 import numpy
9 import pylab
10 
11 from argparse import ArgumentParser
12 parser = ArgumentParser(description=__doc__)
13 parser.add_argument("--condition", default=None, help="select packets by condition")
14 parser.add_argument("--sample-length", type=int, default=0, help="number of samples to run FFT over")
15 parser.add_argument("logs", metavar="LOG", nargs="+")
16 
17 args = parser.parse_args()
18 
19 from pymavlink import mavutil
20 
21 def fft(logfile):
22  '''display fft for raw ACC data in logfile'''
23 
24  print("Processing log %s" % filename)
25  mlog = mavutil.mavlink_connection(filename)
26 
27  data = {'ACC1.rate' : 1000,
28  'ACC2.rate' : 1600,
29  'ACC3.rate' : 1000,
30  'GYR1.rate' : 1000,
31  'GYR2.rate' : 800,
32  'GYR3.rate' : 1000}
33  for acc in ['ACC1','ACC2','ACC3']:
34  for ax in ['AccX', 'AccY', 'AccZ']:
35  data[acc+'.'+ax] = []
36  for gyr in ['GYR1','GYR2','GYR3']:
37  for ax in ['GyrX', 'GyrY', 'GyrZ']:
38  data[gyr+'.'+ax] = []
39 
40  # now gather all the data
41  while True:
42  m = mlog.recv_match(condition=args.condition)
43  if m is None:
44  break
45  type = m.get_type()
46  if type.startswith("ACC"):
47  data[type+'.AccX'].append(m.AccX)
48  data[type+'.AccY'].append(m.AccY)
49  data[type+'.AccZ'].append(m.AccZ)
50  if type.startswith("GYR"):
51  data[type+'.GyrX'].append(m.GyrX)
52  data[type+'.GyrY'].append(m.GyrY)
53  data[type+'.GyrZ'].append(m.GyrZ)
54 
55  print("Extracted %u data points" % len(data['ACC1.AccX']))
56 
57  for msg in ['ACC1', 'ACC2', 'ACC3', 'GYR1', 'GYR2', 'GYR3']:
58  pylab.figure()
59 
60  if msg.startswith('ACC'):
61  prefix = 'Acc'
62  else:
63  prefix = 'Gyr'
64  for axis in ['X', 'Y', 'Z']:
65  field = msg + '.' + prefix + axis
66  d = data[field]
67  if args.sample_length != 0:
68  d = d[0:args.sample_length]
69  d = numpy.array(d)
70  if len(d) == 0:
71  continue
72  avg = numpy.sum(d) / len(d)
73  d -= avg
74  d_fft = numpy.fft.rfft(d)
75  freq = numpy.fft.rfftfreq(len(d), 1.0 / data[msg+'.rate'])
76  pylab.plot( freq, numpy.abs(d_fft), label=field )
77  pylab.legend(loc='upper right')
78 
79 for filename in args.logs:
80  fft(filename)
81 
82 pylab.show()


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