00001
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
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
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)