magfit_gps.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer offsets
5 '''
6 from __future__ import print_function
7 from builtins import object
8 
9 from argparse import ArgumentParser
10 parser = ArgumentParser(description=__doc__)
11 parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
12 parser.add_argument("--minspeed", type=float, default=5.0, help="minimum ground speed to use")
13 parser.add_argument("--condition", default=None, help="select packets by condition")
14 parser.add_argument("--declination", type=float, default=None, help="force declination")
15 parser.add_argument("logs", metavar="LOG", nargs="+")
16 
17 args = parser.parse_args()
18 
19 from pymavlink import mavutil
20 
21 
22 class vec3(object):
23  def __init__(self, x, y, z):
24  self.x = x
25  self.y = y
26  self.z = z
27  def __str__(self):
28  return "%.1f %.1f %.1f" % (self.x, self.y, self.z)
29 
30 def heading_error1(parm, data):
31  from math import sin, cos, atan2, degrees
32  xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
33 
34  if args.declination is not None:
35  declination = args.declination
36 
37  ret = []
38  for d in data:
39  x = d[0] + xofs
40  y = d[1] + yofs
41  z = d[2] + zofs
42  r = d[3]
43  p = d[4]
44  h = d[5]
45 
46  headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
47  headY = y*cos(r) - z*sin(r)
48  heading = degrees(atan2(-headY,headX)) + declination
49  if heading < 0:
50  heading += 360
51  herror = h - heading
52  if herror > 180:
53  herror -= 360
54  if herror < -180:
55  herror += 360
56  ret.append(herror)
57  return ret
58 
59 def heading_error(parm, data):
60  from math import sin, cos, atan2, degrees
61  from numpy import dot
62  xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm
63 
64  if args.declination is not None:
65  declination = args.declination
66 
67  a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]]
68 
69  ret = []
70  for d in data:
71  x = d[0] + xofs
72  y = d[1] + yofs
73  z = d[2] + zofs
74  r = d[3]
75  p = d[4]
76  h = d[5]
77  mv = [x, y, z]
78  mv2 = dot(a, mv)
79  x = mv2[0]
80  y = mv2[1]
81  z = mv2[2]
82 
83  headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p)
84  headY = y*cos(r) - z*sin(r)
85  heading = degrees(atan2(-headY,headX)) + declination
86  if heading < 0:
87  heading += 360
88  herror = h - heading
89  if herror > 180:
90  herror -= 360
91  if herror < -180:
92  herror += 360
93  ret.append(herror)
94  return ret
95 
96 def fit_data(data):
97  from scipy import optimize
98 
99  p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0]
100  if args.declination is not None:
101  p0[-1] = args.declination
102  p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data))
103 
104 # p0 = p1[:]
105 # p1, ier = optimize.leastsq(heading_error, p0[:], args=(data))
106 
107  print(p1)
108  if not ier in [1, 2, 3, 4]:
109  raise RuntimeError("Unable to find solution")
110  return p1
111 
112 def magfit(logfile):
113  '''find best magnetometer offset fit to a log file'''
114  print("Processing log %s" % filename)
115  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
116 
117  flying = False
118  gps_heading = 0.0
119 
120  data = []
121 
122  # get the current mag offsets
123  m = mlog.recv_match(type='SENSOR_OFFSETS',condition=args.condition)
124  offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
125 
126  attitude = mlog.recv_match(type='ATTITUDE',condition=args.condition)
127 
128  # now gather all the data
129  while True:
130  m = mlog.recv_match(condition=args.condition)
131  if m is None:
132  break
133  if m.get_type() == "GPS_RAW":
134  # flying if groundspeed more than 5 m/s
135  flying = (m.v > args.minspeed and m.fix_type == 2)
136  gps_heading = m.hdg
137  if m.get_type() == "GPS_RAW_INT":
138  # flying if groundspeed more than 5 m/s
139  flying = (m.vel/100 > args.minspeed and m.fix_type == 3)
140  gps_heading = m.cog/100
141  if m.get_type() == "ATTITUDE":
142  attitude = m
143  if m.get_type() == "SENSOR_OFFSETS":
144  # update current offsets
145  offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
146  if not flying:
147  continue
148  if m.get_type() == "RAW_IMU":
149  data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading))
150  print("Extracted %u data points" % len(data))
151  print("Current offsets: %s" % offsets)
152  ofs2 = fit_data(data)
153  print("Declination estimate: %.1f" % ofs2[-1])
154  new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2])
155  a = [[ofs2[3], ofs2[4], ofs2[5]],
156  [ofs2[6], ofs2[7], ofs2[8]],
157  [ofs2[9], ofs2[10], ofs2[11]]]
158  print(a)
159  print("New offsets : %s" % new_offsets)
160 
161 total = 0.0
162 for filename in args.logs:
163  magfit(filename)


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