magfit_rotation_gps.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer rotation to GPS data
5 '''
6 from __future__ import print_function
7 from builtins import range
8 from builtins import object
9 
10 from argparse import ArgumentParser
11 parser = ArgumentParser(description=__doc__)
12 parser.add_argument("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
13 parser.add_argument("--declination", default=0.0, type=float, help="magnetic declination")
14 parser.add_argument("--min-speed", default=4.0, type=float, help="minimum GPS speed")
15 parser.add_argument("logs", metavar="LOG", nargs="+")
16 
17 args = parser.parse_args()
18 
19 from pymavlink import mavutil
20 from pymavlink.rotmat import Vector3, Matrix3
21 from math import radians, degrees, sin, cos, atan2
22 
23 
24 class Rotation(object):
25  def __init__(self, roll, pitch, yaw, r):
26  self.roll = roll
27  self.pitch = pitch
28  self.yaw = yaw
29  self.r = r
30 
31 def in_rotations_list(rotations, m):
32  for r in rotations:
33  m2 = m.transposed() * r.r
34  (r, p, y) = m2.to_euler()
35  if (abs(r) < radians(1) and
36  abs(p) < radians(1) and
37  abs(y) < radians(1)):
38  return True
39  return False
40 
42  '''generate all 90 degree rotations'''
43  rotations = []
44  for yaw in [0, 90, 180, 270]:
45  for pitch in [0, 90, 180, 270]:
46  for roll in [0, 90, 180, 270]:
47  m = Matrix3()
48  m.from_euler(radians(roll), radians(pitch), radians(yaw))
49  if not in_rotations_list(rotations, m):
50  rotations.append(Rotation(roll, pitch, yaw, m))
51  return rotations
52 
53 def angle_diff(angle1, angle2):
54  '''give the difference between two angles in degrees'''
55  ret = angle1 - angle2
56  if ret > 180:
57  ret -= 360;
58  if ret < -180:
59  ret += 360
60  return ret
61 
62 def heading_difference(mag, attitude, declination):
63  r = attitude.roll
64  p = attitude.pitch
65  headX = mag.x*cos(p) + mag.y*sin(r)*sin(p) + mag.z*cos(r)*sin(p)
66  headY = mag.y*cos(r) - mag.z*sin(r)
67  heading = degrees(atan2(-headY,headX)) + declination
68  heading2 = degrees(attitude.yaw)
69  return abs(angle_diff(heading, heading2))
70 
71 def add_errors(mag, attitude, total_error, rotations):
72  for i in range(len(rotations)):
73  r = rotations[i].r
74  rmag = r * mag
75  total_error[i] += heading_difference(rmag, attitude, args.declination)
76 
77 
78 def magfit(logfile):
79  '''find best magnetometer rotation fit to a log file'''
80 
81  print("Processing log %s" % filename)
82  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
83 
84  # generate 90 degree rotations
85  rotations = generate_rotations()
86  print("Generated %u rotations" % len(rotations))
87 
88  count = 0
89  total_error = [0]*len(rotations)
90  attitude = None
91  gps = None
92 
93  # now gather all the data
94  while True:
95  m = mlog.recv_match()
96  if m is None:
97  break
98  if m.get_type() == "ATTITUDE":
99  attitude = m
100  if m.get_type() == "GPS_RAW_INT":
101  gps = m
102  if m.get_type() == "RAW_IMU":
103  mag = Vector3(m.xmag, m.ymag, m.zmag)
104  if attitude is not None and gps is not None and gps.vel > args.min_speed*100 and gps.fix_type>=3:
105  add_errors(mag, attitude, total_error, rotations)
106  count += 1
107 
108  best_i = 0
109  best_err = total_error[0]
110  for i in range(len(rotations)):
111  r = rotations[i]
112  print("(%u,%u,%u) err=%.2f" % (
113  r.roll,
114  r.pitch,
115  r.yaw,
116  total_error[i]/count))
117  if total_error[i] < best_err:
118  best_i = i
119  best_err = total_error[i]
120  r = rotations[best_i]
121  print("Best rotation (%u,%u,%u) err=%.2f" % (
122  r.roll,
123  r.pitch,
124  r.yaw,
125  best_err/count))
126 
127 for filename in args.logs:
128  magfit(filename)


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