magfit_rotation_gyro.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer rotation to gyro data
5 '''
6 from __future__ import print_function
7 
8 from builtins import range
9 from builtins import object
10 
11 from argparse import ArgumentParser
12 parser = ArgumentParser(description=__doc__)
13 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
14 parser.add_argument("--verbose", action='store_true', help="verbose output")
15 parser.add_argument("--min-rotation", default=5.0, type=float, help="min rotation to add point")
16 parser.add_argument("logs", metavar="LOG", nargs="+")
17 
18 args = parser.parse_args()
19 
20 from pymavlink import mavutil
21 from pymavlink.rotmat import Vector3, Matrix3
22 from math import radians
23 
24 
25 class Rotation(object):
26  def __init__(self, name, roll, pitch, yaw):
27  self.name = name
28  self.roll = roll
29  self.pitch = pitch
30  self.yaw = yaw
31  self.r = Matrix3()
32  self.r.from_euler(self.roll, self.pitch, self.yaw)
33 
34  def is_90_degrees(self):
35  return (self.roll % 90 == 0) and (self.pitch % 90 == 0) and (self.yaw % 90 == 0)
36 
37  def __str__(self):
38  return self.name
39 
40 # the rotations used in APM
41 rotations = [
42  Rotation("ROTATION_NONE", 0, 0, 0),
43  Rotation("ROTATION_YAW_45", 0, 0, 45),
44  Rotation("ROTATION_YAW_90", 0, 0, 90),
45  Rotation("ROTATION_YAW_135", 0, 0, 135),
46  Rotation("ROTATION_YAW_180", 0, 0, 180),
47  Rotation("ROTATION_YAW_225", 0, 0, 225),
48  Rotation("ROTATION_YAW_270", 0, 0, 270),
49  Rotation("ROTATION_YAW_315", 0, 0, 315),
50  Rotation("ROTATION_ROLL_180", 180, 0, 0),
51  Rotation("ROTATION_ROLL_180_YAW_45", 180, 0, 45),
52  Rotation("ROTATION_ROLL_180_YAW_90", 180, 0, 90),
53  Rotation("ROTATION_ROLL_180_YAW_135", 180, 0, 135),
54  Rotation("ROTATION_PITCH_180", 0, 180, 0),
55  Rotation("ROTATION_ROLL_180_YAW_225", 180, 0, 225),
56  Rotation("ROTATION_ROLL_180_YAW_270", 180, 0, 270),
57  Rotation("ROTATION_ROLL_180_YAW_315", 180, 0, 315),
58  Rotation("ROTATION_ROLL_90", 90, 0, 0),
59  Rotation("ROTATION_ROLL_90_YAW_45", 90, 0, 45),
60  Rotation("ROTATION_ROLL_90_YAW_90", 90, 0, 90),
61  Rotation("ROTATION_ROLL_90_YAW_135", 90, 0, 135),
62  Rotation("ROTATION_ROLL_270", 270, 0, 0),
63  Rotation("ROTATION_ROLL_270_YAW_45", 270, 0, 45),
64  Rotation("ROTATION_ROLL_270_YAW_90", 270, 0, 90),
65  Rotation("ROTATION_ROLL_270_YAW_135", 270, 0, 135),
66  Rotation("ROTATION_PITCH_90", 0, 90, 0),
67  Rotation("ROTATION_PITCH_270", 0, 270, 0),
68  Rotation("ROTATION_PITCH_180_YAW_90", 0, 180, 90),
69  Rotation("ROTATION_PITCH_180_YAW_270", 0, 180, 270),
70  Rotation("ROTATION_ROLL_90_PITCH_90", 90, 90, 0),
71  Rotation("ROTATION_ROLL_180_PITCH_90", 180, 90, 0),
72  Rotation("ROTATION_ROLL_270_PITCH_90", 270, 90, 0),
73  Rotation("ROTATION_ROLL_90_PITCH_180", 90, 180, 0),
74  Rotation("ROTATION_ROLL_270_PITCH_180", 270, 180, 0),
75  Rotation("ROTATION_ROLL_90_PITCH_270", 90, 270, 0),
76  Rotation("ROTATION_ROLL_180_PITCH_270", 180, 270, 0),
77  Rotation("ROTATION_ROLL_270_PITCH_270", 270, 270, 0),
78  Rotation("ROTATION_ROLL_90_PITCH_180_YAW_90", 90, 180, 90),
79  Rotation("ROTATION_ROLL_90_YAW_270", 90, 0, 270)
80  ]
81 
82 def mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL):
83  '''fixup a mag vector back to original value using AHRS and Compass orientation parameters'''
84  if COMPASS_EXTERNAL == 0 and AHRS_ORIENTATION != 0:
85  # undo any board orientation
86  mag = rotations[AHRS_ORIENTATION].r.transposed() * mag
87  # undo any compass orientation
88  if COMPASS_ORIENT != 0:
89  mag = rotations[COMPASS_ORIENT].r.transposed() * mag
90  return mag
91 
92 def add_errors(mag, gyr, last_mag, deltat, total_error, rotations):
93  for i in range(len(rotations)):
94  if not rotations[i].is_90_degrees():
95  continue
96  r = rotations[i].r
97  m = Matrix3()
98  m.rotate(gyr * deltat)
99  rmag1 = r * last_mag
100  rmag2 = r * mag
101  rmag3 = m.transposed() * rmag1
102  err = rmag3 - rmag2
103  total_error[i] += err.length()
104 
105 
106 def magfit(logfile):
107  '''find best magnetometer rotation fit to a log file'''
108 
109  print("Processing log %s" % filename)
110  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
111 
112  last_mag = None
113  last_usec = 0
114  count = 0
115  total_error = [0]*len(rotations)
116 
117  AHRS_ORIENTATION = 0
118  COMPASS_ORIENT = 0
119  COMPASS_EXTERNAL = 0
120  last_gyr = None
121 
122  # now gather all the data
123  while True:
124  m = mlog.recv_match()
125  if m is None:
126  break
127  if m.get_type() in ["PARAM_VALUE", "PARM"]:
128  if m.get_type() == "PARM":
129  name = str(m.Name)
130  value = int(m.Value)
131  else:
132  name = str(m.param_id)
133  value = int(m.param_value)
134  if name == "AHRS_ORIENTATION":
135  AHRS_ORIENTATION = value
136  if name == 'COMPASS_ORIENT':
137  COMPASS_ORIENT = value
138  if name == 'COMPASS_EXTERNAL':
139  COMPASS_EXTERNAL = value
140  if m.get_type() in ["RAW_IMU", "MAG","IMU"]:
141  if m.get_type() == "RAW_IMU":
142  mag = Vector3(m.xmag, m.ymag, m.zmag)
143  gyr = Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001
144  usec = m.time_usec
145  elif m.get_type() == "IMU":
146  last_gyr = Vector3(m.GyrX,m.GyrY,m.GyrZ)
147  continue
148  elif last_gyr is not None:
149  mag = Vector3(m.MagX,m.MagY,m.MagZ)
150  gyr = last_gyr
151  usec = m.TimeMS * 1000
152  mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL)
153  if last_mag is not None and gyr.length() > radians(args.min_rotation):
154  add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations)
155  count += 1
156  last_mag = mag
157  last_usec = usec
158 
159  best_i = 0
160  best_err = total_error[0]
161  for i in range(len(rotations)):
162  r = rotations[i]
163  if not r.is_90_degrees():
164  continue
165  if args.verbose:
166  print("%s err=%.2f" % (r, total_error[i]/count))
167  if total_error[i] < best_err:
168  best_i = i
169  best_err = total_error[i]
170  r = rotations[best_i]
171  print("Current rotation is AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=%u" % (
172  rotations[AHRS_ORIENTATION],
173  rotations[COMPASS_ORIENT],
174  COMPASS_EXTERNAL))
175  print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count))
176  print("Please set AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=1" % (
177  rotations[AHRS_ORIENTATION],
178  r))
179 
180 for filename in args.logs:
181  magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:06