magfit_rotation_gyro.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 # the rotations used in APM
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         # undo any board orientation
00084         mag = rotations[AHRS_ORIENTATION].r.transposed() * mag
00085     # undo any compass orientation
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     # now gather all the data
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)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43