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