mavfft.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 fit best estimate of magnetometer offsets
00005 '''
00006 
00007 import sys, time, os, math, numpy
00008 import matplotlib.pyplot as plt
00009 import pylab
00010 
00011 from argparse import ArgumentParser
00012 parser = ArgumentParser(description=__doc__)
00013 parser.add_argument("--condition", default=None, help="select packets by condition")
00014 parser.add_argument("--sample-length", type=int, default=0, help="number of samples to run FFT over")
00015 parser.add_argument("logs", metavar="LOG", nargs="+")
00016 
00017 args = parser.parse_args()
00018 
00019 from pymavlink import mavutil
00020 
00021 def fft(logfile):
00022     '''display fft for raw ACC data in logfile'''
00023 
00024     print("Processing log %s" % filename)
00025     mlog = mavutil.mavlink_connection(filename)
00026 
00027     data = {'ACC1.rate' : 1000,
00028             'ACC2.rate' : 1600,
00029             'ACC3.rate' : 1000,
00030             'GYR1.rate' : 1000,
00031             'GYR2.rate' :  800,
00032             'GYR3.rate' : 1000}
00033     for acc in ['ACC1','ACC2','ACC3']:
00034         for ax in ['AccX', 'AccY', 'AccZ']:
00035             data[acc+'.'+ax] = []
00036     for gyr in ['GYR1','GYR2','GYR3']:
00037         for ax in ['GyrX', 'GyrY', 'GyrZ']:
00038             data[gyr+'.'+ax] = []
00039 
00040     # now gather all the data
00041     while True:
00042         m = mlog.recv_match(condition=args.condition)
00043         if m is None:
00044             break
00045         type = m.get_type()
00046         if type.startswith("ACC"):
00047             data[type+'.AccX'].append(m.AccX)
00048             data[type+'.AccY'].append(m.AccY)
00049             data[type+'.AccZ'].append(m.AccZ)
00050         if type.startswith("GYR"):
00051             data[type+'.GyrX'].append(m.GyrX)
00052             data[type+'.GyrY'].append(m.GyrY)
00053             data[type+'.GyrZ'].append(m.GyrZ)
00054 
00055     print("Extracted %u data points" % len(data['ACC1.AccX']))
00056 
00057     for msg in ['ACC1', 'ACC2', 'ACC3', 'GYR1', 'GYR2', 'GYR3']:
00058         pylab.figure()
00059 
00060         if msg.startswith('ACC'):
00061             prefix = 'Acc'
00062         else:
00063             prefix = 'Gyr'
00064         for axis in ['X', 'Y', 'Z']:
00065             field = msg + '.' + prefix + axis
00066             d = data[field]
00067             if args.sample_length != 0:
00068                 d = d[0:args.sample_length]
00069             d = numpy.array(d)
00070             if len(d) == 0:
00071                 continue
00072             avg = numpy.sum(d) / len(d)
00073             d -= avg
00074             d_fft = numpy.fft.rfft(d)
00075             freq  = numpy.fft.rfftfreq(len(d), 1.0 / data[msg+'.rate'])
00076             pylab.plot( freq, numpy.abs(d_fft), label=field )
00077         pylab.legend(loc='upper right')
00078 
00079 for filename in args.logs:
00080     fft(filename)
00081 
00082 pylab.show()


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17