1 files changed, 159 insertions, 0 deletions
diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py b/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py
new file mode 100644
index 000000000..30ba45806
--- /dev/null
+++ b/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py
@@ -0,0 +1,159 @@
+#!/usr/bin/env python
+
+'''
+fit best estimate of magnetometer offsets
+'''
+
+import sys, time, os, math
+
+# allow import from the parent directory, where mavlink.py is
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
+
+from optparse import OptionParser
+parser = OptionParser("magfit.py [options]")
+parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
+parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
+parser.add_option("--minspeed", type='float', default=5.0, help="minimum ground speed to use")
+
+(opts, args) = parser.parse_args()
+
+if opts.mav10:
+    os.environ['MAVLINK10'] = '1'
+import mavutil
+
+if len(args) < 1:
+    print("Usage: magfit.py [options] <LOGFILE...>")
+    sys.exit(1)
+
+class vec3(object):
+    def __init__(self, x, y, z):
+        self.x = x
+        self.y = y
+        self.z = z
+    def __str__(self):
+        return "%.1f %.1f %.1f" % (self.x, self.y, self.z)
+
+def heading_error1(parm, data):
+    from math import sin, cos, atan2, degrees
+    from numpy import dot
+    xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
+
+    ret = []
+    for d in data:
+        x = d[0] + xofs
+        y = d[1] + yofs
+        z = d[2] + zofs
+        r = d[3]
+        p = d[4]
+        h = d[5]
+
+        headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
+        headY = y*cos(r) - z*sin(r)
+        heading = degrees(atan2(-headY,headX)) + declination
+        if heading < 0:
+            heading += 360
+        herror = h - heading
+        if herror > 180:
+            herror -= 360
+        if herror < -180:
+            herror += 360
+        ret.append(herror)
+    return ret
+
+def heading_error(parm, data):
+    from math import sin, cos, atan2, degrees
+    from numpy import dot
+    xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
+
+    a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]]
+
+    ret = []
+    for d in data:
+        x = d[0] + xofs
+        y = d[1] + yofs
+        z = d[2] + zofs
+        r = d[3]
+        p = d[4]
+        h = d[5]
+        mv = [x, y, z]
+        mv2 = dot(a, mv)
+        x = mv2[0]
+        y = mv2[1]
+        z = mv2[2]
+
+        headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
+        headY = y*cos(r) - z*sin(r)
+        heading = degrees(atan2(-headY,headX)) + declination
+        if heading < 0:
+            heading += 360
+        herror = h - heading
+        if herror > 180:
+            herror -= 360
+        if herror < -180:
+            herror += 360
+        ret.append(herror)
+    return ret
+
+def fit_data(data):
+    import numpy, scipy
+    from scipy import optimize
+
+    p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0]
+    p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data))
+
+#    p0 = p1[:]
+#    p1, ier = optimize.leastsq(heading_error, p0[:], args=(data))
+
+    print(p1)
+    if not ier in [1, 2, 3, 4]:
+        raise RuntimeError("Unable to find solution")
+    return p1
+
+def magfit(logfile):
+    '''find best magnetometer offset fit to a log file'''
+    print("Processing log %s" % filename)
+    mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps)
+
+    flying = False
+    gps_heading = 0.0
+
+    data = []
+
+    # get the current mag offsets
+    m = mlog.recv_match(type='SENSOR_OFFSETS')
+    offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
+
+    attitude = mlog.recv_match(type='ATTITUDE')
+
+    # now gather all the data
+    while True:
+        m = mlog.recv_match()
+        if m is None:
+            break
+        if m.get_type() == "GPS_RAW":
+            # flying if groundspeed more than 5 m/s
+            flying = (m.v > opts.minspeed and m.fix_type == 2)
+            gps_heading = m.hdg
+        if m.get_type() == "ATTITUDE":
+            attitude = m
+        if m.get_type() == "SENSOR_OFFSETS":
+            # update current offsets
+            offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
+        if not flying:
+            continue
+        if m.get_type() == "RAW_IMU":
+            data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading))
+    print("Extracted %u data points" % len(data))
+    print("Current offsets: %s" % offsets)
+    ofs2 = fit_data(data)
+    print("Declination estimate: %.1f" % ofs2[-1])
+    new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2])
+    a = [[ofs2[3], ofs2[4], ofs2[5]],
+         [ofs2[6], ofs2[7], ofs2[8]],
+         [ofs2[9], ofs2[10], ofs2[11]]]
+    print(a)
+    print("New offsets    : %s" % new_offsets)
+
+total = 0.0
+for filename in args:
+    magfit(filename)