magfit_delta.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 fit best estimate of magnetometer offsets using the algorithm from
00005 Bill Premerlani
00006 '''
00007 
00008 import sys, time, os, math
00009 
00010 # command line option handling
00011 from argparse import ArgumentParser
00012 parser = ArgumentParser(description=__doc__)
00013 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
00014 parser.add_argument("--condition", default=None, help="select packets by condition")
00015 parser.add_argument("--verbose", action='store_true', default=False, help="verbose offset output")
00016 parser.add_argument("--gain", type=float, default=0.01, help="algorithm gain")
00017 parser.add_argument("--noise", type=float, default=0, help="noise to add")
00018 parser.add_argument("--max-change", type=float, default=10, help="max step change")
00019 parser.add_argument("--min-diff", type=float, default=50, help="min mag vector delta")
00020 parser.add_argument("--history", type=int, default=20, help="how many points to keep")
00021 parser.add_argument("--repeat", type=int, default=1, help="number of repeats through the data")
00022 parser.add_argument("logs", metavar="LOG", nargs="+")
00023 
00024 args = parser.parse_args()
00025 
00026 from pymavlink import mavutil
00027 from pymavlink.rotmat import Vector3, Matrix3
00028 
00029 
00030 def noise():
00031     '''a noise vector'''
00032     from random import gauss
00033     v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
00034     v.normalize()
00035     return v * args.noise
00036 
00037 def find_offsets(data, ofs):
00038     '''find mag offsets by applying Bills "offsets revisited" algorithm
00039        on the data
00040 
00041        This is an implementation of the algorithm from:
00042           http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
00043        '''
00044 
00045     # a limit on the maximum change in each step
00046     max_change = args.max_change
00047 
00048     # the gain factor for the algorithm
00049     gain = args.gain
00050 
00051     data2 = []
00052     for d in data:
00053         d = d.copy() + noise()
00054         d.x = float(int(d.x + 0.5))
00055         d.y = float(int(d.y + 0.5))
00056         d.z = float(int(d.z + 0.5))
00057         data2.append(d)
00058     data = data2
00059 
00060     history_idx = 0
00061     mag_history = data[0:args.history]
00062 
00063     for i in range(args.history, len(data)):
00064         B1 = mag_history[history_idx] + ofs
00065         B2 = data[i] + ofs
00066 
00067         diff = B2 - B1
00068         diff_length = diff.length()
00069         if diff_length <= args.min_diff:
00070             # the mag vector hasn't changed enough - we don't get any
00071             # information from this
00072             history_idx = (history_idx+1) % args.history
00073             continue
00074 
00075         mag_history[history_idx] = data[i]
00076         history_idx = (history_idx+1) % args.history
00077 
00078         # equation 6 of Bills paper
00079         delta = diff * (gain * (B2.length() - B1.length()) / diff_length)
00080 
00081         # limit the change from any one reading. This is to prevent
00082         # single crazy readings from throwing off the offsets for a long
00083         # time
00084         delta_length = delta.length()
00085         if max_change != 0 and delta_length > max_change:
00086             delta *= max_change / delta_length
00087 
00088         # set the new offsets
00089         ofs = ofs - delta
00090 
00091         if args.verbose:
00092             print(ofs)
00093     return ofs
00094 
00095 
00096 def magfit(logfile):
00097     '''find best magnetometer offset fit to a log file'''
00098 
00099     print("Processing log %s" % filename)
00100 
00101     # open the log file
00102     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00103 
00104     data = []
00105     mag = None
00106     offsets = Vector3(0,0,0)
00107 
00108     # now gather all the data
00109     while True:
00110         # get the next MAVLink message in the log
00111         m = mlog.recv_match(condition=args.condition)
00112         if m is None:
00113             break
00114         if m.get_type() == "SENSOR_OFFSETS":
00115             # update offsets that were used during this flight
00116             offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
00117         if m.get_type() == "RAW_IMU" and offsets != None:
00118             # extract one mag vector, removing the offsets that were
00119             # used during that flight to get the raw sensor values
00120             mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets
00121             data.append(mag)
00122 
00123     print("Extracted %u data points" % len(data))
00124     print("Current offsets: %s" % offsets)
00125 
00126     # run the fitting algorithm
00127     ofs = offsets
00128     ofs = Vector3(0,0,0)
00129     for r in range(args.repeat):
00130         ofs = find_offsets(data, ofs)
00131         print('Loop %u offsets %s' % (r, ofs))
00132         sys.stdout.flush()
00133     print("New offsets: %s" % ofs)
00134 
00135 total = 0.0
00136 for filename in args.logs:
00137     magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57