4 fit best estimate of magnetometer offsets using the algorithm from 7 from __future__
import print_function
8 from builtins
import range
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=
"+")
26 args = parser.parse_args()
28 from pymavlink
import mavutil
34 from random
import gauss
35 v =
Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
40 '''find mag offsets by applying Bills "offsets revisited" algorithm 43 This is an implementation of the algorithm from: 44 http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf 48 max_change = args.max_change
55 d = d.copy() +
noise()
63 mag_history = data[0:args.history]
65 for i
in range(args.history, len(data)):
66 B1 = mag_history[history_idx] + ofs
70 diff_length = diff.length()
71 if diff_length <= args.min_diff:
74 history_idx = (history_idx+1) % args.history
77 mag_history[history_idx] = data[i]
78 history_idx = (history_idx+1) % args.history
81 delta = diff * (gain * (B2.length() - B1.length()) / diff_length)
86 delta_length = delta.length()
87 if max_change != 0
and delta_length > max_change:
88 delta *= max_change / delta_length
99 '''find best magnetometer offset fit to a log file''' 101 print(
"Processing log %s" % filename)
104 mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
113 m = mlog.recv_match(condition=args.condition)
116 if m.get_type() ==
"SENSOR_OFFSETS":
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:
122 mag =
Vector3(m.xmag, m.ymag, m.zmag) - offsets
125 print(
"Extracted %u data points" % len(data))
126 print(
"Current offsets: %s" % offsets)
131 for r
in range(args.repeat):
133 print(
'Loop %u offsets %s' % (r, ofs))
135 print(
"New offsets: %s" % ofs)
138 for filename
in args.logs: