magfit_delta.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer offsets using the algorithm from
5 Bill Premerlani
6 '''
7 from __future__ import print_function
8 from builtins import range
9 
10 import sys
11 
12 # command line option handling
13 from argparse import ArgumentParser
14 parser = ArgumentParser(description=__doc__)
15 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
16 parser.add_argument("--condition", default=None, help="select packets by condition")
17 parser.add_argument("--verbose", action='store_true', default=False, help="verbose offset output")
18 parser.add_argument("--gain", type=float, default=0.01, help="algorithm gain")
19 parser.add_argument("--noise", type=float, default=0, help="noise to add")
20 parser.add_argument("--max-change", type=float, default=10, help="max step change")
21 parser.add_argument("--min-diff", type=float, default=50, help="min mag vector delta")
22 parser.add_argument("--history", type=int, default=20, help="how many points to keep")
23 parser.add_argument("--repeat", type=int, default=1, help="number of repeats through the data")
24 parser.add_argument("logs", metavar="LOG", nargs="+")
25 
26 args = parser.parse_args()
27 
28 from pymavlink import mavutil
29 from pymavlink.rotmat import Vector3
30 
31 
32 def noise():
33  '''a noise vector'''
34  from random import gauss
35  v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
36  v.normalize()
37  return v * args.noise
38 
39 def find_offsets(data, ofs):
40  '''find mag offsets by applying Bills "offsets revisited" algorithm
41  on the data
42 
43  This is an implementation of the algorithm from:
44  http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
45  '''
46 
47  # a limit on the maximum change in each step
48  max_change = args.max_change
49 
50  # the gain factor for the algorithm
51  gain = args.gain
52 
53  data2 = []
54  for d in data:
55  d = d.copy() + noise()
56  d.x = float(int(d.x + 0.5))
57  d.y = float(int(d.y + 0.5))
58  d.z = float(int(d.z + 0.5))
59  data2.append(d)
60  data = data2
61 
62  history_idx = 0
63  mag_history = data[0:args.history]
64 
65  for i in range(args.history, len(data)):
66  B1 = mag_history[history_idx] + ofs
67  B2 = data[i] + ofs
68 
69  diff = B2 - B1
70  diff_length = diff.length()
71  if diff_length <= args.min_diff:
72  # the mag vector hasn't changed enough - we don't get any
73  # information from this
74  history_idx = (history_idx+1) % args.history
75  continue
76 
77  mag_history[history_idx] = data[i]
78  history_idx = (history_idx+1) % args.history
79 
80  # equation 6 of Bills paper
81  delta = diff * (gain * (B2.length() - B1.length()) / diff_length)
82 
83  # limit the change from any one reading. This is to prevent
84  # single crazy readings from throwing off the offsets for a long
85  # time
86  delta_length = delta.length()
87  if max_change != 0 and delta_length > max_change:
88  delta *= max_change / delta_length
89 
90  # set the new offsets
91  ofs = ofs - delta
92 
93  if args.verbose:
94  print(ofs)
95  return ofs
96 
97 
98 def magfit(logfile):
99  '''find best magnetometer offset fit to a log file'''
100 
101  print("Processing log %s" % filename)
102 
103  # open the log file
104  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
105 
106  data = []
107  mag = None
108  offsets = Vector3(0,0,0)
109 
110  # now gather all the data
111  while True:
112  # get the next MAVLink message in the log
113  m = mlog.recv_match(condition=args.condition)
114  if m is None:
115  break
116  if m.get_type() == "SENSOR_OFFSETS":
117  # update offsets that were used during this flight
118  offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
119  if m.get_type() == "RAW_IMU" and offsets is not None:
120  # extract one mag vector, removing the offsets that were
121  # used during that flight to get the raw sensor values
122  mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets
123  data.append(mag)
124 
125  print("Extracted %u data points" % len(data))
126  print("Current offsets: %s" % offsets)
127 
128  # run the fitting algorithm
129  ofs = offsets
130  ofs = Vector3(0,0,0)
131  for r in range(args.repeat):
132  ofs = find_offsets(data, ofs)
133  print('Loop %u offsets %s' % (r, ofs))
134  sys.stdout.flush()
135  print("New offsets: %s" % ofs)
136 
137 total = 0.0
138 for filename in args.logs:
139  magfit(filename)


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