magfit_gps.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 fit best estimate of magnetometer offsets
00005 '''
00006 
00007 import sys, time, os, math
00008 
00009 from argparse import ArgumentParser
00010 parser = ArgumentParser(description=__doc__)
00011 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
00012 parser.add_argument("--minspeed", type=float, default=5.0, help="minimum ground speed to use")
00013 parser.add_argument("--condition", default=None, help="select packets by condition")
00014 parser.add_argument("--declination", type=float, default=None, help="force declination")
00015 parser.add_argument("logs", metavar="LOG", nargs="+")
00016 
00017 args = parser.parse_args()
00018 
00019 from pymavlink import mavutil
00020 
00021 
00022 class vec3(object):
00023     def __init__(self, x, y, z):
00024         self.x = x
00025         self.y = y
00026         self.z = z
00027     def __str__(self):
00028         return "%.1f %.1f %.1f" % (self.x, self.y, self.z)
00029 
00030 def heading_error1(parm, data):
00031     from math import sin, cos, atan2, degrees
00032     from numpy import dot
00033     xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
00034 
00035     if args.declination is not None:
00036         declination = args.declination
00037 
00038     ret = []
00039     for d in data:
00040         x = d[0] + xofs
00041         y = d[1] + yofs
00042         z = d[2] + zofs
00043         r = d[3]
00044         p = d[4]
00045         h = d[5]
00046 
00047         headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
00048         headY = y*cos(r) - z*sin(r)
00049         heading = degrees(atan2(-headY,headX)) + declination
00050         if heading < 0:
00051             heading += 360
00052         herror = h - heading
00053         if herror > 180:
00054             herror -= 360
00055         if herror < -180:
00056             herror += 360
00057         ret.append(herror)
00058     return ret
00059 
00060 def heading_error(parm, data):
00061     from math import sin, cos, atan2, degrees
00062     from numpy import dot
00063     xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
00064 
00065     if args.declination is not None:
00066         declination = args.declination
00067 
00068     a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]]
00069 
00070     ret = []
00071     for d in data:
00072         x = d[0] + xofs
00073         y = d[1] + yofs
00074         z = d[2] + zofs
00075         r = d[3]
00076         p = d[4]
00077         h = d[5]
00078         mv = [x, y, z]
00079         mv2 = dot(a, mv)
00080         x = mv2[0]
00081         y = mv2[1]
00082         z = mv2[2]
00083 
00084         headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
00085         headY = y*cos(r) - z*sin(r)
00086         heading = degrees(atan2(-headY,headX)) + declination
00087         if heading < 0:
00088             heading += 360
00089         herror = h - heading
00090         if herror > 180:
00091             herror -= 360
00092         if herror < -180:
00093             herror += 360
00094         ret.append(herror)
00095     return ret
00096 
00097 def fit_data(data):
00098     import numpy, scipy
00099     from scipy import optimize
00100 
00101     p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0]
00102     if args.declination is not None:
00103         p0[-1] = args.declination
00104     p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data))
00105 
00106 #    p0 = p1[:]
00107 #    p1, ier = optimize.leastsq(heading_error, p0[:], args=(data))
00108 
00109     print(p1)
00110     if not ier in [1, 2, 3, 4]:
00111         raise RuntimeError("Unable to find solution")
00112     return p1
00113 
00114 def magfit(logfile):
00115     '''find best magnetometer offset fit to a log file'''
00116     print("Processing log %s" % filename)
00117     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00118 
00119     flying = False
00120     gps_heading = 0.0
00121 
00122     data = []
00123 
00124     # get the current mag offsets
00125     m = mlog.recv_match(type='SENSOR_OFFSETS',condition=args.condition)
00126     offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
00127 
00128     attitude = mlog.recv_match(type='ATTITUDE',condition=args.condition)
00129 
00130     # now gather all the data
00131     while True:
00132         m = mlog.recv_match(condition=args.condition)
00133         if m is None:
00134             break
00135         if m.get_type() == "GPS_RAW":
00136             # flying if groundspeed more than 5 m/s
00137             flying = (m.v > args.minspeed and m.fix_type == 2)
00138             gps_heading = m.hdg
00139         if m.get_type() == "GPS_RAW_INT":
00140             # flying if groundspeed more than 5 m/s
00141             flying = (m.vel/100 > args.minspeed and m.fix_type == 3)
00142             gps_heading = m.cog/100
00143         if m.get_type() == "ATTITUDE":
00144             attitude = m
00145         if m.get_type() == "SENSOR_OFFSETS":
00146             # update current offsets
00147             offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
00148         if not flying:
00149             continue
00150         if m.get_type() == "RAW_IMU":
00151             data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading))
00152     print("Extracted %u data points" % len(data))
00153     print("Current offsets: %s" % offsets)
00154     ofs2 = fit_data(data)
00155     print("Declination estimate: %.1f" % ofs2[-1])
00156     new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2])
00157     a = [[ofs2[3], ofs2[4], ofs2[5]],
00158          [ofs2[6], ofs2[7], ofs2[8]],
00159          [ofs2[9], ofs2[10], ofs2[11]]]
00160     print(a)
00161     print("New offsets    : %s" % new_offsets)
00162 
00163 total = 0.0
00164 for filename in args.logs:
00165     magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17