magfit_rotation_gps.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 fit best estimate of magnetometer rotation to GPS data
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("--declination", default=0.0, type=float, help="magnetic declination")
00013 parser.add_argument("--min-speed", default=4.0, type=float, help="minimum GPS speed")
00014 parser.add_argument("logs", metavar="LOG", nargs="+")
00015 
00016 args = parser.parse_args()
00017 
00018 from pymavlink import mavutil
00019 from pymavlink.rotmat import Vector3, Matrix3
00020 from math import radians, degrees, sin, cos, atan2
00021 
00022 
00023 class Rotation(object):
00024     def __init__(self, roll, pitch, yaw, r):
00025         self.roll = roll
00026         self.pitch = pitch
00027         self.yaw = yaw
00028         self.r = r
00029 
00030 def in_rotations_list(rotations, m):
00031     for r in rotations:
00032         m2 = m.transposed() * r.r
00033         (r, p, y) = m2.to_euler()
00034         if (abs(r) < radians(1) and
00035             abs(p) < radians(1) and
00036             abs(y) < radians(1)):
00037             return True
00038     return False
00039 
00040 def generate_rotations():
00041     '''generate all 90 degree rotations'''
00042     rotations = []
00043     for yaw in [0, 90, 180, 270]:
00044         for pitch in [0, 90, 180, 270]:
00045             for roll in [0, 90, 180, 270]:
00046                 m = Matrix3()
00047                 m.from_euler(radians(roll), radians(pitch), radians(yaw))
00048                 if not in_rotations_list(rotations, m):
00049                     rotations.append(Rotation(roll, pitch, yaw, m))
00050     return rotations
00051 
00052 def angle_diff(angle1, angle2):
00053     '''give the difference between two angles in degrees'''
00054     ret = angle1 - angle2
00055     if ret > 180:
00056         ret -= 360;
00057     if ret < -180:
00058         ret += 360
00059     return ret
00060 
00061 def heading_difference(mag, attitude, declination):
00062     r = attitude.roll
00063     p = attitude.pitch
00064     headX = mag.x*cos(p) + mag.y*sin(r)*sin(p) + mag.z*cos(r)*sin(p)
00065     headY = mag.y*cos(r) - mag.z*sin(r)
00066     heading = degrees(atan2(-headY,headX)) + declination
00067     heading2 = degrees(attitude.yaw)
00068     return abs(angle_diff(heading, heading2))
00069 
00070 def add_errors(mag, attitude, total_error, rotations):
00071     for i in range(len(rotations)):
00072         r = rotations[i].r
00073         rmag = r * mag
00074         total_error[i] += heading_difference(rmag, attitude, args.declination)
00075 
00076 
00077 def magfit(logfile):
00078     '''find best magnetometer rotation fit to a log file'''
00079 
00080     print("Processing log %s" % filename)
00081     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00082 
00083     # generate 90 degree rotations
00084     rotations = generate_rotations()
00085     print("Generated %u rotations" % len(rotations))
00086 
00087     count = 0
00088     total_error = [0]*len(rotations)
00089     attitude = None
00090     gps = None
00091 
00092     # now gather all the data
00093     while True:
00094         m = mlog.recv_match()
00095         if m is None:
00096             break
00097         if m.get_type() == "ATTITUDE":
00098             attitude = m
00099         if m.get_type() == "GPS_RAW_INT":
00100             gps = m
00101         if m.get_type() == "RAW_IMU":
00102             mag = Vector3(m.xmag, m.ymag, m.zmag)
00103             if attitude is not None and gps is not None and gps.vel > args.min_speed*100 and gps.fix_type>=3:
00104                 add_errors(mag, attitude, total_error, rotations)
00105             count += 1
00106 
00107     best_i = 0
00108     best_err = total_error[0]
00109     for i in range(len(rotations)):
00110         r = rotations[i]
00111         print("(%u,%u,%u) err=%.2f" % (
00112             r.roll,
00113             r.pitch,
00114             r.yaw,
00115             total_error[i]/count))
00116         if total_error[i] < best_err:
00117             best_i = i
00118             best_err = total_error[i]
00119     r = rotations[best_i]
00120     print("Best rotation (%u,%u,%u) err=%.2f" % (
00121         r.roll,
00122         r.pitch,
00123         r.yaw,
00124         best_err/count))
00125 
00126 for filename in args.logs:
00127     magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57