Go to the documentation of this file.00001
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
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()