magfit_motors.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 fit best estimate of magnetometer offsets, trying to take into account motor interference
5 '''
6 from __future__ import print_function
7 from builtins import range
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("--condition",dest="condition", default=None, help="select packets by condition")
13 parser.add_argument("--noise", type=float, default=0, help="noise to add")
14 parser.add_argument("logs", metavar="LOG", nargs="+")
15 
16 args = parser.parse_args()
17 
18 from pymavlink import mavutil
19 from pymavlink.rotmat import Vector3
20 
21 
22 def noise():
23  '''a noise vector'''
24  from random import gauss
25  v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
26  v.normalize()
27  return v * args.noise
28 
29 def select_data(data):
30  ret = []
31  counts = {}
32  for d in data:
33  (mag,motor) = d
34  key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20)
35  if key in counts:
36  counts[key] += 1
37  else:
38  counts[key] = 1
39  if counts[key] < 3:
40  ret.append(d)
41  print(len(data), len(ret))
42  return ret
43 
44 def radius(d, offsets, motor_ofs):
45  '''return radius give data point and offsets'''
46  (mag, motor) = d
47  return (mag + offsets + motor*motor_ofs).length()
48 
49 def radius_cmp(a, b, offsets, motor_ofs):
50  '''return radius give data point and offsets'''
51  diff = radius(a, offsets, motor_ofs) - radius(b, offsets, motor_ofs)
52  if diff > 0:
53  return 1
54  if diff < 0:
55  return -1
56  return 0
57 
58 def sphere_error(p, data):
59  x,y,z,mx,my,mz,r = p
60  ofs = Vector3(x,y,z)
61  motor_ofs = Vector3(mx,my,mz)
62  ret = []
63  for d in data:
64  (mag,motor) = d
65  err = r - radius((mag,motor), ofs, motor_ofs)
66  ret.append(err)
67  return ret
68 
69 def fit_data(data):
70  from scipy import optimize
71 
72  p0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
73  p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
74  if not ier in [1, 2, 3, 4]:
75  raise RuntimeError("Unable to find solution")
76  return (Vector3(p1[0], p1[1], p1[2]), Vector3(p1[3], p1[4], p1[5]), p1[6])
77 
78 def magfit(logfile):
79  '''find best magnetometer offset fit to a log file'''
80 
81  print("Processing log %s" % filename)
82  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
83 
84  data = []
85 
86  last_t = 0
87  offsets = Vector3(0,0,0)
88  motor_ofs = Vector3(0,0,0)
89  motor = 0.0
90 
91  # now gather all the data
92  while True:
93  m = mlog.recv_match(condition=args.condition)
94  if m is None:
95  break
96  if m.get_type() == "PARAM_VALUE" and m.param_id == "RC3_MIN":
97  rc3_min = float(m.param_value)
98  if m.get_type() == "SENSOR_OFFSETS":
99  # update current offsets
100  offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
101  if m.get_type() == "SERVO_OUTPUT_RAW":
102  motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw
103  motor_pwm *= 0.25
104  rc3_min = mlog.param('RC3_MIN', 1100)
105  rc3_max = mlog.param('RC3_MAX', 1900)
106  motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min)
107  if motor > 1.0:
108  motor = 1.0
109  if motor < 0.0:
110  motor = 0.0
111  if m.get_type() == "RAW_IMU":
112  mag = Vector3(m.xmag, m.ymag, m.zmag)
113  # add data point after subtracting the current offsets
114  data.append((mag - offsets + noise(), motor))
115 
116  print("Extracted %u data points" % len(data))
117  print("Current offsets: %s" % offsets)
118 
119  data = select_data(data)
120 
121  # do an initial fit with all data
122  (offsets, motor_ofs, field_strength) = fit_data(data)
123 
124  for count in range(3):
125  # sort the data by the radius
126  data.sort(lambda a,b : radius_cmp(a,b,offsets,motor_ofs))
127 
128  print("Fit %u : %s %s field_strength=%6.1f to %6.1f" % (
129  count, offsets, motor_ofs,
130  radius(data[0], offsets, motor_ofs), radius(data[-1], offsets, motor_ofs)))
131 
132  # discard outliers, keep the middle 3/4
133  data = data[len(data)/8:-len(data)/8]
134 
135  # fit again
136  (offsets, motor_ofs, field_strength) = fit_data(data)
137 
138  print("Final : %s %s field_strength=%6.1f to %6.1f" % (
139  offsets, motor_ofs,
140  radius(data[0], offsets, motor_ofs), radius(data[-1], offsets, motor_ofs)))
141  print("mavgraph.py '%s' 'mag_field(RAW_IMU)' 'mag_field_motors(RAW_IMU,SENSOR_OFFSETS,(%f,%f,%f),SERVO_OUTPUT_RAW,(%f,%f,%f))'" % (
142  filename,
143  offsets.x,offsets.y,offsets.z,
144  motor_ofs.x, motor_ofs.y, motor_ofs.z))
145 
146 total = 0.0
147 for filename in args.logs:
148  magfit(filename)


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