Rev Author Line No. Line
3662 kaklik 1  
2 # Copyright (C) 2010 Antoine Drouin
3 #
4 # This file is part of Paparazzi.
5 #
6 # Paparazzi is free software; you can redistribute it and/or modify
7 # it under the terms of the GNU General Public License as published by
8 # the Free Software Foundation; either version 2, or (at your option)
9 # any later version.
10 #
11 # Paparazzi is distributed in the hope that it will be useful,
12 # but WITHOUT ANY WARRANTY; without even the implied warranty of
13 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 # GNU General Public License for more details.
15 #
16 # You should have received a copy of the GNU General Public License
17 # along with Paparazzi; see the file COPYING. If not, write to
18 # the Free Software Foundation, 59 Temple Place - Suite 330,
19 # Boston, MA 02111-1307, USA.
20 #
21  
22 from __future__ import print_function, division
23  
24 import re
25 import numpy as np
26 from numpy import sin, cos
27 from scipy import linalg, stats
28  
29 import matplotlib
30 import matplotlib.pyplot as plt
31 from mpl_toolkits.mplot3d import Axes3D
32  
33  
34 def get_ids_in_log(filename):
35 """Returns available ac_id from a log."""
36 f = open(filename, 'r')
37 ids = []
38 pattern = re.compile("\S+ (\S+)")
39 while True:
40 line = f.readline().strip()
41 if line == '':
42 break
43 m = re.match(pattern, line)
44 if m:
45 ac_id = m.group(1)
46 if not ac_id in ids:
47 ids.append(ac_id)
48 return ids
49  
50  
51 def read_log(ac_id, filename, sensor):
52 """Extracts raw sensor measurements from a log."""
53 f = open(filename, 'r')
54 pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW (\S+) (\S+) (\S+)")
55 list_meas = []
56 while True:
57 line = f.readline().strip()
58 if line == '':
59 break
60 m = re.match(pattern, line)
61 if m:
62 list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4))])
63 return np.array(list_meas)
64  
65  
66 def read_log_mag_current(ac_id, filename):
67 """Extracts raw magnetometer and current measurements from a log."""
68 f = open(filename, 'r')
69 pattern = re.compile("(\S+) "+ac_id+" IMU_MAG_CURRENT_CALIBRATION (\S+) (\S+) (\S+) (\S+)")
70 list_meas = []
71 while True:
72 line = f.readline().strip()
73 if line == '':
74 break
75 m = re.match(pattern, line)
76 if m:
77 list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4)), float(m.group(5))])
78 return np.array(list_meas)
79  
80  
81 def filter_meas(meas, window_size, noise_threshold):
82 """Select only non-noisy data."""
83 filtered_meas = []
84 filtered_idx = []
85 for i in range(window_size, len(meas)-window_size):
86 noise = meas[i-window_size:i+window_size, :].std(axis=0)
87 if linalg.norm(noise) < noise_threshold:
88 filtered_meas.append(meas[i, :])
89 filtered_idx.append(i)
90 return np.array(filtered_meas), filtered_idx
91  
92  
93 def get_min_max_guess(meas, scale):
94 """Initial boundary based calibration."""
95 max_meas = meas[:, :].max(axis=0)
96 min_meas = meas[:, :].min(axis=0)
97 n = (max_meas + min_meas) / 2
98 sf = 2*scale/(max_meas - min_meas)
99 return np.array([n[0], n[1], n[2], sf[0], sf[1], sf[2]])
100  
101  
102 def scale_measurements(meas, p):
103 """Scale the set of measurements."""
104 l_comp = []
105 l_norm = []
106 for m in meas[:, ]:
107 sm = (m - p[0:3])*p[3:6]
108 l_comp.append(sm)
109 l_norm.append(linalg.norm(sm))
110 return np.array(l_comp), np.array(l_norm)
111  
112  
113 def estimate_mag_current_relation(meas):
114 """Calculate linear coefficient of magnetometer-current relation."""
115 coefficient = []
116 for i in range(0, 3):
117 gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:, 3], meas[:, i])
118 coefficient.append(gradient)
119 return coefficient
120  
121  
122 def print_xml(p, sensor, res):
123 """Print xml for airframe file."""
124 print("")
125 print("<define name=\""+sensor+"_X_NEUTRAL\" value=\""+str(int(round(p[0])))+"\"/>")
126 print("<define name=\""+sensor+"_Y_NEUTRAL\" value=\""+str(int(round(p[1])))+"\"/>")
127 print("<define name=\""+sensor+"_Z_NEUTRAL\" value=\""+str(int(round(p[2])))+"\"/>")
128 print("<define name=\""+sensor+"_X_SENS\" value=\""+str(p[3]*2**res)+"\" integer=\"16\"/>")
129 print("<define name=\""+sensor+"_Y_SENS\" value=\""+str(p[4]*2**res)+"\" integer=\"16\"/>")
130 print("<define name=\""+sensor+"_Z_SENS\" value=\""+str(p[5]*2**res)+"\" integer=\"16\"/>")
131  
132  
133 def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref):
3663 kaklik 134 """Plot calibration results in 2D graphs."""
3662 kaklik 135 plt.subplot(3, 1, 1)
136 plt.plot(measurements[:, 0])
137 plt.plot(measurements[:, 1])
138 plt.plot(measurements[:, 2])
139 plt.plot(flt_idx, flt_meas[:, 0], 'ro')
140 plt.plot(flt_idx, flt_meas[:, 1], 'ro')
141 plt.plot(flt_idx, flt_meas[:, 2], 'ro')
3663 kaklik 142 plt.xlabel('sample number')
143 plt.ylabel('miligauss')
3662 kaklik 144 plt.title('Raw sensors')
145  
146 plt.subplot(3, 2, 3)
147 plt.plot(cp0[:, 0])
148 plt.plot(cp0[:, 1])
149 plt.plot(cp0[:, 2])
150 plt.plot(-sensor_ref*np.ones(len(flt_meas)))
151 plt.plot(sensor_ref*np.ones(len(flt_meas)))
3663 kaklik 152 plt.xlabel('sample number')
153 plt.ylabel('-')
154 plt.title('First approximation')
3662 kaklik 155  
156 plt.subplot(3, 2, 4)
157 plt.plot(np0)
158 plt.plot(sensor_ref*np.ones(len(flt_meas)))
3663 kaklik 159 plt.xlabel('sample number')
160 plt.ylabel('-')
161 plt.title('magnitude')
3662 kaklik 162  
163 plt.subplot(3, 2, 5)
164 plt.plot(cp1[:, 0])
165 plt.plot(cp1[:, 1])
166 plt.plot(cp1[:, 2])
167 plt.plot(-sensor_ref*np.ones(len(flt_meas)))
168 plt.plot(sensor_ref*np.ones(len(flt_meas)))
3663 kaklik 169 plt.xlabel('sample number')
170 plt.ylabel('-')
171 plt.title('separate axes')
3662 kaklik 172  
173 plt.subplot(3, 2, 6)
174 plt.plot(np1)
175 plt.plot(sensor_ref*np.ones(len(flt_meas)))
3663 kaklik 176 plt.xlabel('sample number')
177 plt.ylabel('-')
178 plt.title('magnitude')
3662 kaklik 179  
180 # if we want to have another plot we only draw the figure (non-blocking)
181 # also in matplotlib before 1.0.0 there is only one call to show possible
182 if block:
183 plt.show()
184 else:
185 plt.draw()
186  
187  
188 def plot_mag_3d(measured, calibrated, p):
189 """Plot magnetometer measurements on 3D sphere."""
190 # set up points for sphere and ellipsoid wireframes
191 u = np.r_[0:2 * np.pi:20j]
192 v = np.r_[0:np.pi:20j]
193 wx = np.outer(cos(u), sin(v))
194 wy = np.outer(sin(u), sin(v))
195 wz = np.outer(np.ones(np.size(u)), cos(v))
196 ex = p[0] * np.ones(np.size(u)) + np.outer(cos(u), sin(v)) / p[3]
197 ey = p[1] * np.ones(np.size(u)) + np.outer(sin(u), sin(v)) / p[4]
198 ez = p[2] * np.ones(np.size(u)) + np.outer(np.ones(np.size(u)), cos(v)) / p[5]
199  
200 # measurements
201 mx = measured[:, 0]
202 my = measured[:, 1]
203 mz = measured[:, 2]
204  
205 # calibrated values
206 cx = calibrated[:, 0]
207 cy = calibrated[:, 1]
208 cz = calibrated[:, 2]
209  
210 # axes size
211 left = 0.02
212 bottom = 0.05
213 width = 0.46
214 height = 0.9
215 rect_l = [left, bottom, width, height]
216 rect_r = [left/2+0.5, bottom, width, height]
217  
218 fig = plt.figure(figsize=plt.figaspect(0.5))
219 if matplotlib.__version__.startswith('0'):
220 ax = Axes3D(fig, rect=rect_l)
221 else:
222 ax = fig.add_subplot(1, 2, 1, position=rect_l, projection='3d')
223 # plot measurements
224 ax.scatter(mx, my, mz)
225 plt.hold(True)
226 # plot line from center to ellipsoid center
227 ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+', markersize=10)
228 # plot ellipsoid
229 ax.plot_wireframe(ex, ey, ez, color='grey', alpha=0.5)
230  
231 # Create cubic bounding box to simulate equal aspect ratio
232 max_range = np.array([mx.max() - mx.min(), my.max() - my.min(), mz.max() - mz.min()]).max()
233 Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (mx.max() + mx.min())
234 Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (my.max() + my.min())
235 Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (mz.max() + mz.min())
236 # add the fake bounding box:
237 for xb, yb, zb in zip(Xb, Yb, Zb):
238 ax.plot([xb], [yb], [zb], 'w')
239  
240 ax.set_title('MAG raw with fitted ellipsoid and center offset')
241 ax.set_xlabel('x')
242 ax.set_ylabel('y')
243 ax.set_zlabel('z')
244  
245 if matplotlib.__version__.startswith('0'):
246 ax = Axes3D(fig, rect=rect_r)
247 else:
248 ax = fig.add_subplot(1, 2, 2, position=rect_r, projection='3d')
249 ax.plot_wireframe(wx, wy, wz, color='grey', alpha=0.5)
250 plt.hold(True)
251 ax.scatter(cx, cy, cz)
252  
253 ax.set_title('MAG calibrated on unit sphere')
254 ax.set_xlabel('x')
255 ax.set_ylabel('y')
256 ax.set_zlabel('z')
257 ax.set_xlim3d(-1, 1)
258 ax.set_ylim3d(-1, 1)
259 ax.set_zlim3d(-1, 1)
260 plt.show()
261  
262  
263 def read_turntable_log(ac_id, tt_id, filename, _min, _max):
264 """ Read a turntable log.
265 return an array which first column is turnatble and next 3 are gyro
266 """
267 f = open(filename, 'r')
268 pattern_g = re.compile("(\S+) "+str(ac_id)+" IMU_GYRO_RAW (\S+) (\S+) (\S+)")
269 pattern_t = re.compile("(\S+) "+str(tt_id)+" IMU_TURNTABLE (\S+)")
270 last_tt = None
271 list_tt = []
272 while True:
273 line = f.readline().strip()
274 if line == '':
275 break
276 m = re.match(pattern_t, line)
277 if m:
278 last_tt = float(m.group(2))
279 m = re.match(pattern_g, line)
280 if m and last_tt and _min < last_tt < _max:
281 list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), float(m.group(4))])
282 return np.array(list_tt)