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):
134 """Plot calibration results."""
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')
142 plt.xlabel('time (s)')
143 plt.ylabel('ADC')
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)))
152  
153 plt.subplot(3, 2, 4)
154 plt.plot(np0)
155 plt.plot(sensor_ref*np.ones(len(flt_meas)))
156  
157 plt.subplot(3, 2, 5)
158 plt.plot(cp1[:, 0])
159 plt.plot(cp1[:, 1])
160 plt.plot(cp1[:, 2])
161 plt.plot(-sensor_ref*np.ones(len(flt_meas)))
162 plt.plot(sensor_ref*np.ones(len(flt_meas)))
163  
164 plt.subplot(3, 2, 6)
165 plt.plot(np1)
166 plt.plot(sensor_ref*np.ones(len(flt_meas)))
167  
168 # 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
170 if block:
171 plt.show()
172 else:
173 plt.draw()
174  
175  
176 def plot_mag_3d(measured, calibrated, p):
177 """Plot magnetometer measurements on 3D sphere."""
178 # set up points for sphere and ellipsoid wireframes
179 u = np.r_[0:2 * np.pi:20j]
180 v = np.r_[0:np.pi:20j]
181 wx = np.outer(cos(u), sin(v))
182 wy = np.outer(sin(u), sin(v))
183 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]
185 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]
187  
188 # measurements
189 mx = measured[:, 0]
190 my = measured[:, 1]
191 mz = measured[:, 2]
192  
193 # calibrated values
194 cx = calibrated[:, 0]
195 cy = calibrated[:, 1]
196 cz = calibrated[:, 2]
197  
198 # axes size
199 left = 0.02
200 bottom = 0.05
201 width = 0.46
202 height = 0.9
203 rect_l = [left, bottom, width, height]
204 rect_r = [left/2+0.5, bottom, width, height]
205  
206 fig = plt.figure(figsize=plt.figaspect(0.5))
207 if matplotlib.__version__.startswith('0'):
208 ax = Axes3D(fig, rect=rect_l)
209 else:
210 ax = fig.add_subplot(1, 2, 1, position=rect_l, projection='3d')
211 # plot measurements
212 ax.scatter(mx, my, mz)
213 plt.hold(True)
214 # 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)
216 # plot ellipsoid
217 ax.plot_wireframe(ex, ey, ez, color='grey', alpha=0.5)
218  
219 # 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()
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())
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())
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())
224 # add the fake bounding box:
225 for xb, yb, zb in zip(Xb, Yb, Zb):
226 ax.plot([xb], [yb], [zb], 'w')
227  
228 ax.set_title('MAG raw with fitted ellipsoid and center offset')
229 ax.set_xlabel('x')
230 ax.set_ylabel('y')
231 ax.set_zlabel('z')
232  
233 if matplotlib.__version__.startswith('0'):
234 ax = Axes3D(fig, rect=rect_r)
235 else:
236 ax = fig.add_subplot(1, 2, 2, position=rect_r, projection='3d')
237 ax.plot_wireframe(wx, wy, wz, color='grey', alpha=0.5)
238 plt.hold(True)
239 ax.scatter(cx, cy, cz)
240  
241 ax.set_title('MAG calibrated on unit sphere')
242 ax.set_xlabel('x')
243 ax.set_ylabel('y')
244 ax.set_zlabel('z')
245 ax.set_xlim3d(-1, 1)
246 ax.set_ylim3d(-1, 1)
247 ax.set_zlim3d(-1, 1)
248 plt.show()
249  
250  
251 def read_turntable_log(ac_id, tt_id, filename, _min, _max):
252 """ Read a turntable log.
253 return an array which first column is turnatble and next 3 are gyro
254 """
255 f = open(filename, 'r')
256 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+)")
258 last_tt = None
259 list_tt = []
260 while True:
261 line = f.readline().strip()
262 if line == '':
263 break
264 m = re.match(pattern_t, line)
265 if m:
266 last_tt = float(m.group(2))
267 m = re.match(pattern_g, line)
268 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))])
270 return np.array(list_tt)