00001
00002
00003 '''
00004 fit best estimate of magnetometer rotation to gyro 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("--verbose", action='store_true', help="verbose output")
00013 parser.add_argument("--min-rotation", default=5.0, type=float, help="min rotation to add point")
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
00021
00022
00023 class Rotation(object):
00024 def __init__(self, name, roll, pitch, yaw):
00025 self.name = name
00026 self.roll = roll
00027 self.pitch = pitch
00028 self.yaw = yaw
00029 self.r = Matrix3()
00030 self.r.from_euler(self.roll, self.pitch, self.yaw)
00031
00032 def is_90_degrees(self):
00033 return (self.roll % 90 == 0) and (self.pitch % 90 == 0) and (self.yaw % 90 == 0)
00034
00035 def __str__(self):
00036 return self.name
00037
00038
00039 rotations = [
00040 Rotation("ROTATION_NONE", 0, 0, 0),
00041 Rotation("ROTATION_YAW_45", 0, 0, 45),
00042 Rotation("ROTATION_YAW_90", 0, 0, 90),
00043 Rotation("ROTATION_YAW_135", 0, 0, 135),
00044 Rotation("ROTATION_YAW_180", 0, 0, 180),
00045 Rotation("ROTATION_YAW_225", 0, 0, 225),
00046 Rotation("ROTATION_YAW_270", 0, 0, 270),
00047 Rotation("ROTATION_YAW_315", 0, 0, 315),
00048 Rotation("ROTATION_ROLL_180", 180, 0, 0),
00049 Rotation("ROTATION_ROLL_180_YAW_45", 180, 0, 45),
00050 Rotation("ROTATION_ROLL_180_YAW_90", 180, 0, 90),
00051 Rotation("ROTATION_ROLL_180_YAW_135", 180, 0, 135),
00052 Rotation("ROTATION_PITCH_180", 0, 180, 0),
00053 Rotation("ROTATION_ROLL_180_YAW_225", 180, 0, 225),
00054 Rotation("ROTATION_ROLL_180_YAW_270", 180, 0, 270),
00055 Rotation("ROTATION_ROLL_180_YAW_315", 180, 0, 315),
00056 Rotation("ROTATION_ROLL_90", 90, 0, 0),
00057 Rotation("ROTATION_ROLL_90_YAW_45", 90, 0, 45),
00058 Rotation("ROTATION_ROLL_90_YAW_90", 90, 0, 90),
00059 Rotation("ROTATION_ROLL_90_YAW_135", 90, 0, 135),
00060 Rotation("ROTATION_ROLL_270", 270, 0, 0),
00061 Rotation("ROTATION_ROLL_270_YAW_45", 270, 0, 45),
00062 Rotation("ROTATION_ROLL_270_YAW_90", 270, 0, 90),
00063 Rotation("ROTATION_ROLL_270_YAW_135", 270, 0, 135),
00064 Rotation("ROTATION_PITCH_90", 0, 90, 0),
00065 Rotation("ROTATION_PITCH_270", 0, 270, 0),
00066 Rotation("ROTATION_PITCH_180_YAW_90", 0, 180, 90),
00067 Rotation("ROTATION_PITCH_180_YAW_270", 0, 180, 270),
00068 Rotation("ROTATION_ROLL_90_PITCH_90", 90, 90, 0),
00069 Rotation("ROTATION_ROLL_180_PITCH_90", 180, 90, 0),
00070 Rotation("ROTATION_ROLL_270_PITCH_90", 270, 90, 0),
00071 Rotation("ROTATION_ROLL_90_PITCH_180", 90, 180, 0),
00072 Rotation("ROTATION_ROLL_270_PITCH_180", 270, 180, 0),
00073 Rotation("ROTATION_ROLL_90_PITCH_270", 90, 270, 0),
00074 Rotation("ROTATION_ROLL_180_PITCH_270", 180, 270, 0),
00075 Rotation("ROTATION_ROLL_270_PITCH_270", 270, 270, 0),
00076 Rotation("ROTATION_ROLL_90_PITCH_180_YAW_90", 90, 180, 90),
00077 Rotation("ROTATION_ROLL_90_YAW_270", 90, 0, 270)
00078 ]
00079
00080 def mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL):
00081 '''fixup a mag vector back to original value using AHRS and Compass orientation parameters'''
00082 if COMPASS_EXTERNAL == 0 and AHRS_ORIENTATION != 0:
00083
00084 mag = rotations[AHRS_ORIENTATION].r.transposed() * mag
00085
00086 if COMPASS_ORIENT != 0:
00087 mag = rotations[COMPASS_ORIENT].r.transposed() * mag
00088 return mag
00089
00090 def add_errors(mag, gyr, last_mag, deltat, total_error, rotations):
00091 for i in range(len(rotations)):
00092 if not rotations[i].is_90_degrees():
00093 continue
00094 r = rotations[i].r
00095 m = Matrix3()
00096 m.rotate(gyr * deltat)
00097 rmag1 = r * last_mag
00098 rmag2 = r * mag
00099 rmag3 = m.transposed() * rmag1
00100 err = rmag3 - rmag2
00101 total_error[i] += err.length()
00102
00103
00104 def magfit(logfile):
00105 '''find best magnetometer rotation fit to a log file'''
00106
00107 print("Processing log %s" % filename)
00108 mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00109
00110 last_mag = None
00111 last_usec = 0
00112 count = 0
00113 total_error = [0]*len(rotations)
00114
00115 AHRS_ORIENTATION = 0
00116 COMPASS_ORIENT = 0
00117 COMPASS_EXTERNAL = 0
00118 last_gyr = None
00119
00120
00121 while True:
00122 m = mlog.recv_match()
00123 if m is None:
00124 break
00125 if m.get_type() in ["PARAM_VALUE", "PARM"]:
00126 if m.get_type() == "PARM":
00127 name = str(m.Name)
00128 value = int(m.Value)
00129 else:
00130 name = str(m.param_id)
00131 value = int(m.param_value)
00132 if name == "AHRS_ORIENTATION":
00133 AHRS_ORIENTATION = value
00134 if name == 'COMPASS_ORIENT':
00135 COMPASS_ORIENT = value
00136 if name == 'COMPASS_EXTERNAL':
00137 COMPASS_EXTERNAL = value
00138 if m.get_type() in ["RAW_IMU", "MAG","IMU"]:
00139 if m.get_type() == "RAW_IMU":
00140 mag = Vector3(m.xmag, m.ymag, m.zmag)
00141 gyr = Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001
00142 usec = m.time_usec
00143 elif m.get_type() == "IMU":
00144 last_gyr = Vector3(m.GyrX,m.GyrY,m.GyrZ)
00145 continue
00146 elif last_gyr is not None:
00147 mag = Vector3(m.MagX,m.MagY,m.MagZ)
00148 gyr = last_gyr
00149 usec = m.TimeMS * 1000
00150 mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL)
00151 if last_mag is not None and gyr.length() > radians(args.min_rotation):
00152 add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations)
00153 count += 1
00154 last_mag = mag
00155 last_usec = usec
00156
00157 best_i = 0
00158 best_err = total_error[0]
00159 for i in range(len(rotations)):
00160 r = rotations[i]
00161 if not r.is_90_degrees():
00162 continue
00163 if args.verbose:
00164 print("%s err=%.2f" % (r, total_error[i]/count))
00165 if total_error[i] < best_err:
00166 best_i = i
00167 best_err = total_error[i]
00168 r = rotations[best_i]
00169 print("Current rotation is AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=%u" % (
00170 rotations[AHRS_ORIENTATION],
00171 rotations[COMPASS_ORIENT],
00172 COMPASS_EXTERNAL))
00173 print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count))
00174 print("Please set AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=1" % (
00175 rotations[AHRS_ORIENTATION],
00176 r))
00177
00178 for filename in args.logs:
00179 magfit(filename)