0,0 → 1,270 |
|
# Copyright (C) 2010 Antoine Drouin |
# |
# This file is part of Paparazzi. |
# |
# Paparazzi is free software; you can redistribute it and/or modify |
# it under the terms of the GNU General Public License as published by |
# the Free Software Foundation; either version 2, or (at your option) |
# any later version. |
# |
# Paparazzi is distributed in the hope that it will be useful, |
# but WITHOUT ANY WARRANTY; without even the implied warranty of |
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
# GNU General Public License for more details. |
# |
# You should have received a copy of the GNU General Public License |
# along with Paparazzi; see the file COPYING. If not, write to |
# the Free Software Foundation, 59 Temple Place - Suite 330, |
# Boston, MA 02111-1307, USA. |
# |
|
from __future__ import print_function, division |
|
import re |
import numpy as np |
from numpy import sin, cos |
from scipy import linalg, stats |
|
import matplotlib |
import matplotlib.pyplot as plt |
from mpl_toolkits.mplot3d import Axes3D |
|
|
def get_ids_in_log(filename): |
"""Returns available ac_id from a log.""" |
f = open(filename, 'r') |
ids = [] |
pattern = re.compile("\S+ (\S+)") |
while True: |
line = f.readline().strip() |
if line == '': |
break |
m = re.match(pattern, line) |
if m: |
ac_id = m.group(1) |
if not ac_id in ids: |
ids.append(ac_id) |
return ids |
|
|
def read_log(ac_id, filename, sensor): |
"""Extracts raw sensor measurements from a log.""" |
f = open(filename, 'r') |
pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW (\S+) (\S+) (\S+)") |
list_meas = [] |
while True: |
line = f.readline().strip() |
if line == '': |
break |
m = re.match(pattern, line) |
if m: |
list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4))]) |
return np.array(list_meas) |
|
|
def read_log_mag_current(ac_id, filename): |
"""Extracts raw magnetometer and current measurements from a log.""" |
f = open(filename, 'r') |
pattern = re.compile("(\S+) "+ac_id+" IMU_MAG_CURRENT_CALIBRATION (\S+) (\S+) (\S+) (\S+)") |
list_meas = [] |
while True: |
line = f.readline().strip() |
if line == '': |
break |
m = re.match(pattern, line) |
if m: |
list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4)), float(m.group(5))]) |
return np.array(list_meas) |
|
|
def filter_meas(meas, window_size, noise_threshold): |
"""Select only non-noisy data.""" |
filtered_meas = [] |
filtered_idx = [] |
for i in range(window_size, len(meas)-window_size): |
noise = meas[i-window_size:i+window_size, :].std(axis=0) |
if linalg.norm(noise) < noise_threshold: |
filtered_meas.append(meas[i, :]) |
filtered_idx.append(i) |
return np.array(filtered_meas), filtered_idx |
|
|
def get_min_max_guess(meas, scale): |
"""Initial boundary based calibration.""" |
max_meas = meas[:, :].max(axis=0) |
min_meas = meas[:, :].min(axis=0) |
n = (max_meas + min_meas) / 2 |
sf = 2*scale/(max_meas - min_meas) |
return np.array([n[0], n[1], n[2], sf[0], sf[1], sf[2]]) |
|
|
def scale_measurements(meas, p): |
"""Scale the set of measurements.""" |
l_comp = [] |
l_norm = [] |
for m in meas[:, ]: |
sm = (m - p[0:3])*p[3:6] |
l_comp.append(sm) |
l_norm.append(linalg.norm(sm)) |
return np.array(l_comp), np.array(l_norm) |
|
|
def estimate_mag_current_relation(meas): |
"""Calculate linear coefficient of magnetometer-current relation.""" |
coefficient = [] |
for i in range(0, 3): |
gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:, 3], meas[:, i]) |
coefficient.append(gradient) |
return coefficient |
|
|
def print_xml(p, sensor, res): |
"""Print xml for airframe file.""" |
print("") |
print("<define name=\""+sensor+"_X_NEUTRAL\" value=\""+str(int(round(p[0])))+"\"/>") |
print("<define name=\""+sensor+"_Y_NEUTRAL\" value=\""+str(int(round(p[1])))+"\"/>") |
print("<define name=\""+sensor+"_Z_NEUTRAL\" value=\""+str(int(round(p[2])))+"\"/>") |
print("<define name=\""+sensor+"_X_SENS\" value=\""+str(p[3]*2**res)+"\" integer=\"16\"/>") |
print("<define name=\""+sensor+"_Y_SENS\" value=\""+str(p[4]*2**res)+"\" integer=\"16\"/>") |
print("<define name=\""+sensor+"_Z_SENS\" value=\""+str(p[5]*2**res)+"\" integer=\"16\"/>") |
|
|
def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sensor_ref): |
"""Plot calibration results.""" |
plt.subplot(3, 1, 1) |
plt.plot(measurements[:, 0]) |
plt.plot(measurements[:, 1]) |
plt.plot(measurements[:, 2]) |
plt.plot(flt_idx, flt_meas[:, 0], 'ro') |
plt.plot(flt_idx, flt_meas[:, 1], 'ro') |
plt.plot(flt_idx, flt_meas[:, 2], 'ro') |
plt.xlabel('time (s)') |
plt.ylabel('ADC') |
plt.title('Raw sensors') |
|
plt.subplot(3, 2, 3) |
plt.plot(cp0[:, 0]) |
plt.plot(cp0[:, 1]) |
plt.plot(cp0[:, 2]) |
plt.plot(-sensor_ref*np.ones(len(flt_meas))) |
plt.plot(sensor_ref*np.ones(len(flt_meas))) |
|
plt.subplot(3, 2, 4) |
plt.plot(np0) |
plt.plot(sensor_ref*np.ones(len(flt_meas))) |
|
plt.subplot(3, 2, 5) |
plt.plot(cp1[:, 0]) |
plt.plot(cp1[:, 1]) |
plt.plot(cp1[:, 2]) |
plt.plot(-sensor_ref*np.ones(len(flt_meas))) |
plt.plot(sensor_ref*np.ones(len(flt_meas))) |
|
plt.subplot(3, 2, 6) |
plt.plot(np1) |
plt.plot(sensor_ref*np.ones(len(flt_meas))) |
|
# if we want to have another plot we only draw the figure (non-blocking) |
# also in matplotlib before 1.0.0 there is only one call to show possible |
if block: |
plt.show() |
else: |
plt.draw() |
|
|
def plot_mag_3d(measured, calibrated, p): |
"""Plot magnetometer measurements on 3D sphere.""" |
# set up points for sphere and ellipsoid wireframes |
u = np.r_[0:2 * np.pi:20j] |
v = np.r_[0:np.pi:20j] |
wx = np.outer(cos(u), sin(v)) |
wy = np.outer(sin(u), sin(v)) |
wz = np.outer(np.ones(np.size(u)), cos(v)) |
ex = p[0] * np.ones(np.size(u)) + np.outer(cos(u), sin(v)) / p[3] |
ey = p[1] * np.ones(np.size(u)) + np.outer(sin(u), sin(v)) / p[4] |
ez = p[2] * np.ones(np.size(u)) + np.outer(np.ones(np.size(u)), cos(v)) / p[5] |
|
# measurements |
mx = measured[:, 0] |
my = measured[:, 1] |
mz = measured[:, 2] |
|
# calibrated values |
cx = calibrated[:, 0] |
cy = calibrated[:, 1] |
cz = calibrated[:, 2] |
|
# axes size |
left = 0.02 |
bottom = 0.05 |
width = 0.46 |
height = 0.9 |
rect_l = [left, bottom, width, height] |
rect_r = [left/2+0.5, bottom, width, height] |
|
fig = plt.figure(figsize=plt.figaspect(0.5)) |
if matplotlib.__version__.startswith('0'): |
ax = Axes3D(fig, rect=rect_l) |
else: |
ax = fig.add_subplot(1, 2, 1, position=rect_l, projection='3d') |
# plot measurements |
ax.scatter(mx, my, mz) |
plt.hold(True) |
# plot line from center to ellipsoid center |
ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+', markersize=10) |
# plot ellipsoid |
ax.plot_wireframe(ex, ey, ez, color='grey', alpha=0.5) |
|
# Create cubic bounding box to simulate equal aspect ratio |
max_range = np.array([mx.max() - mx.min(), my.max() - my.min(), mz.max() - mz.min()]).max() |
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (mx.max() + mx.min()) |
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (my.max() + my.min()) |
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (mz.max() + mz.min()) |
# add the fake bounding box: |
for xb, yb, zb in zip(Xb, Yb, Zb): |
ax.plot([xb], [yb], [zb], 'w') |
|
ax.set_title('MAG raw with fitted ellipsoid and center offset') |
ax.set_xlabel('x') |
ax.set_ylabel('y') |
ax.set_zlabel('z') |
|
if matplotlib.__version__.startswith('0'): |
ax = Axes3D(fig, rect=rect_r) |
else: |
ax = fig.add_subplot(1, 2, 2, position=rect_r, projection='3d') |
ax.plot_wireframe(wx, wy, wz, color='grey', alpha=0.5) |
plt.hold(True) |
ax.scatter(cx, cy, cz) |
|
ax.set_title('MAG calibrated on unit sphere') |
ax.set_xlabel('x') |
ax.set_ylabel('y') |
ax.set_zlabel('z') |
ax.set_xlim3d(-1, 1) |
ax.set_ylim3d(-1, 1) |
ax.set_zlim3d(-1, 1) |
plt.show() |
|
|
def read_turntable_log(ac_id, tt_id, filename, _min, _max): |
""" Read a turntable log. |
return an array which first column is turnatble and next 3 are gyro |
""" |
f = open(filename, 'r') |
pattern_g = re.compile("(\S+) "+str(ac_id)+" IMU_GYRO_RAW (\S+) (\S+) (\S+)") |
pattern_t = re.compile("(\S+) "+str(tt_id)+" IMU_TURNTABLE (\S+)") |
last_tt = None |
list_tt = [] |
while True: |
line = f.readline().strip() |
if line == '': |
break |
m = re.match(pattern_t, line) |
if m: |
last_tt = float(m.group(2)) |
m = re.match(pattern_g, line) |
if m and last_tt and _min < last_tt < _max: |
list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), float(m.group(4))]) |
return np.array(list_tt) |