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, 0 insertions, 159 deletions
diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py b/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py deleted file mode 100644 index 30ba45806..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py +++ /dev/null @@ -1,159 +0,0 @@ -#!/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) |