diff options
Diffstat (limited to 'mavlink/share/pyshared/pymavlink/examples/magfit_gps.py')
-rw-r--r-- | mavlink/share/pyshared/pymavlink/examples/magfit_gps.py | 159 |
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) |