469 lines
22 KiB
Python
469 lines
22 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
# -*- coding: utf-8 -*-
|
||
|
|
|
||
|
|
""" ArduPilot IMU Filter Test Class
|
||
|
|
|
||
|
|
This program 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 3 of the License, or (at your option) any later
|
||
|
|
version.
|
||
|
|
This program 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
|
||
|
|
this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
"""
|
||
|
|
|
||
|
|
__author__ = "Guglielmo Cassinelli"
|
||
|
|
__contact__ = "gdguglie@gmail.com"
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
from matplotlib.widgets import Slider
|
||
|
|
from matplotlib.animation import FuncAnimation
|
||
|
|
from scipy import signal
|
||
|
|
from BiquadFilter import BiquadFilterType, BiquadFilter
|
||
|
|
|
||
|
|
sliders = [] # matplotlib sliders must be global
|
||
|
|
anim = None # matplotlib animations must be global
|
||
|
|
|
||
|
|
|
||
|
|
class FilterTest:
|
||
|
|
FILTER_DEBOUNCE = 10 # ms
|
||
|
|
|
||
|
|
FILT_SHAPE_DT_FACTOR = 1 # increase to reduce filter shape size
|
||
|
|
|
||
|
|
FFT_N = 512
|
||
|
|
|
||
|
|
filters = {}
|
||
|
|
|
||
|
|
def __init__(self, acc_t, acc_x, acc_y, acc_z, gyr_t, gyr_x, gyr_y, gyr_z, acc_freq, gyr_freq,
|
||
|
|
acc_lpf_cutoff, gyr_lpf_cutoff,
|
||
|
|
acc_notch_freq, acc_notch_att, acc_notch_band,
|
||
|
|
gyr_notch_freq, gyr_notch_att, gyr_notch_band,
|
||
|
|
log_name, accel_notch=False, second_notch=False):
|
||
|
|
|
||
|
|
self.filter_color_map = plt.get_cmap('summer')
|
||
|
|
|
||
|
|
self.filters["acc"] = [
|
||
|
|
BiquadFilter(acc_lpf_cutoff, acc_freq)
|
||
|
|
]
|
||
|
|
|
||
|
|
if accel_notch:
|
||
|
|
self.filters["acc"].append(
|
||
|
|
BiquadFilter(acc_notch_freq, acc_freq, BiquadFilterType.PEAK, acc_notch_att, acc_notch_band),
|
||
|
|
)
|
||
|
|
|
||
|
|
self.filters["gyr"] = [
|
||
|
|
BiquadFilter(gyr_lpf_cutoff, gyr_freq),
|
||
|
|
BiquadFilter(gyr_notch_freq, gyr_freq, BiquadFilterType.PEAK, gyr_notch_att, gyr_notch_band)
|
||
|
|
]
|
||
|
|
|
||
|
|
if second_notch:
|
||
|
|
self.filters["acc"].append(
|
||
|
|
BiquadFilter(acc_notch_freq * 2, acc_freq, BiquadFilterType.PEAK, acc_notch_att, acc_notch_band)
|
||
|
|
)
|
||
|
|
self.filters["gyr"].append(
|
||
|
|
BiquadFilter(gyr_notch_freq * 2, gyr_freq, BiquadFilterType.PEAK, gyr_notch_att, gyr_notch_band)
|
||
|
|
)
|
||
|
|
|
||
|
|
self.ACC_t = acc_t
|
||
|
|
self.ACC_x = acc_x
|
||
|
|
self.ACC_y = acc_y
|
||
|
|
self.ACC_z = acc_z
|
||
|
|
|
||
|
|
self.GYR_t = gyr_t
|
||
|
|
self.GYR_x = gyr_x
|
||
|
|
self.GYR_y = gyr_y
|
||
|
|
self.GYR_z = gyr_z
|
||
|
|
|
||
|
|
self.GYR_freq = gyr_freq
|
||
|
|
self.ACC_freq = acc_freq
|
||
|
|
|
||
|
|
self.gyr_dt = 1. / gyr_freq
|
||
|
|
self.acc_dt = 1. / acc_freq
|
||
|
|
|
||
|
|
self.timer = None
|
||
|
|
|
||
|
|
self.updated_artists = []
|
||
|
|
|
||
|
|
# INIT
|
||
|
|
self.init_plot(log_name)
|
||
|
|
|
||
|
|
def test_acc_filters(self):
|
||
|
|
filt_xs = self.test_filters(self.filters["acc"], self.ACC_t, self.ACC_x)
|
||
|
|
filt_ys = self.test_filters(self.filters["acc"], self.ACC_t, self.ACC_y)
|
||
|
|
filt_zs = self.test_filters(self.filters["acc"], self.ACC_t, self.ACC_z)
|
||
|
|
return filt_xs, filt_ys, filt_zs
|
||
|
|
|
||
|
|
def test_gyr_filters(self):
|
||
|
|
filt_xs = self.test_filters(self.filters["gyr"], self.GYR_t, self.GYR_x)
|
||
|
|
filt_ys = self.test_filters(self.filters["gyr"], self.GYR_t, self.GYR_y)
|
||
|
|
filt_zs = self.test_filters(self.filters["gyr"], self.GYR_t, self.GYR_z)
|
||
|
|
return filt_xs, filt_ys, filt_zs
|
||
|
|
|
||
|
|
def test_filters(self, filters, Ts, Xs):
|
||
|
|
for f in filters:
|
||
|
|
f.reset()
|
||
|
|
|
||
|
|
x_filtered = []
|
||
|
|
|
||
|
|
for i, t in enumerate(Ts):
|
||
|
|
x = Xs[i]
|
||
|
|
|
||
|
|
x_f = x
|
||
|
|
for filt in filters:
|
||
|
|
x_f = filt.apply(x_f)
|
||
|
|
|
||
|
|
x_filtered.append(x_f)
|
||
|
|
|
||
|
|
return x_filtered
|
||
|
|
|
||
|
|
def get_filter_shape(self, filter):
|
||
|
|
samples = int(filter.get_sample_freq()) # resolution of filter shape based on sample rate
|
||
|
|
x_space = np.linspace(0.0, samples // 2, samples // int(2 * self.FILT_SHAPE_DT_FACTOR))
|
||
|
|
return x_space, filter.freq_response(x_space)
|
||
|
|
|
||
|
|
def init_signal_plot(self, ax, Ts, Xs, Ys, Zs, Xs_filtered, Ys_filtered, Zs_filtered, label):
|
||
|
|
ax.plot(Ts, Xs, linewidth=1, label="{}X".format(label), alpha=0.5)
|
||
|
|
ax.plot(Ts, Ys, linewidth=1, label="{}Y".format(label), alpha=0.5)
|
||
|
|
ax.plot(Ts, Zs, linewidth=1, label="{}Z".format(label), alpha=0.5)
|
||
|
|
filtered_x_ax, = ax.plot(Ts, Xs_filtered, linewidth=1, label="{}X filtered".format(label), alpha=1)
|
||
|
|
filtered_y_ax, = ax.plot(Ts, Ys_filtered, linewidth=1, label="{}Y filtered".format(label), alpha=1)
|
||
|
|
filtered_z_ax, = ax.plot(Ts, Zs_filtered, linewidth=1, label="{}Z filtered".format(label), alpha=1)
|
||
|
|
ax.legend(prop={'size': 8})
|
||
|
|
return filtered_x_ax, filtered_y_ax, filtered_z_ax
|
||
|
|
|
||
|
|
def fft_to_xdata(self, fft):
|
||
|
|
n = len(fft)
|
||
|
|
norm_factor = 2. / n
|
||
|
|
return norm_factor * np.abs(fft[:n // 2])
|
||
|
|
|
||
|
|
def plot_fft(self, ax, x, fft, label):
|
||
|
|
fft_ax, = ax.plot(x, self.fft_to_xdata(fft), label=label)
|
||
|
|
return fft_ax
|
||
|
|
|
||
|
|
def init_fft(self, ax, Ts, Xs, Ys, Zs, sample_rate, dt, Xs_filtered, Ys_filtered, Zs_filtered, label):
|
||
|
|
|
||
|
|
_freqs_raw_x, _times_raw_x, _stft_raw_x = signal.stft(Xs, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
raw_fft_x = np.average(np.abs(_stft_raw_x), axis=1)
|
||
|
|
|
||
|
|
_freqs_raw_y, _times_raw_y, _stft_raw_y = signal.stft(Ys, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
raw_fft_y = np.average(np.abs(_stft_raw_y), axis=1)
|
||
|
|
|
||
|
|
_freqs_raw_z, _times_raw_z, _stft_raw_z = signal.stft(Zs, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
raw_fft_z = np.average(np.abs(_stft_raw_z), axis=1)
|
||
|
|
|
||
|
|
_freqs_x, _times_x, _stft_x = signal.stft(Xs_filtered, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
filtered_fft_x = np.average(np.abs(_stft_x), axis=1)
|
||
|
|
|
||
|
|
_freqs_y, _times_y, _stft_y = signal.stft(Ys_filtered, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
filtered_fft_y = np.average(np.abs(_stft_y), axis=1)
|
||
|
|
|
||
|
|
_freqs_z, _times_z, _stft_z = signal.stft(Zs_filtered, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
filtered_fft_z = np.average(np.abs(_stft_z), axis=1)
|
||
|
|
|
||
|
|
ax.plot(_freqs_raw_x, raw_fft_x, alpha=0.5, linewidth=1, label="{}x FFT".format(label))
|
||
|
|
ax.plot(_freqs_raw_y, raw_fft_y, alpha=0.5, linewidth=1, label="{}y FFT".format(label))
|
||
|
|
ax.plot(_freqs_raw_z, raw_fft_z, alpha=0.5, linewidth=1, label="{}z FFT".format(label))
|
||
|
|
|
||
|
|
filtered_fft_ax_x, = ax.plot(_freqs_x, filtered_fft_x, label="filt. {}x FFT".format(label))
|
||
|
|
filtered_fft_ax_y, = ax.plot(_freqs_y, filtered_fft_y, label="filt. {}y FFT".format(label))
|
||
|
|
filtered_fft_ax_z, = ax.plot(_freqs_z, filtered_fft_z, label="filt. {}z FFT".format(label))
|
||
|
|
|
||
|
|
# FFT
|
||
|
|
# samples = len(Ts)
|
||
|
|
# x_space = np.linspace(0.0, 1.0 / (2.0 * dt), samples // 2)
|
||
|
|
# filtered_data = np.hanning(len(Xs_filtered)) * Xs_filtered
|
||
|
|
# raw_fft = np.fft.fft(np.hanning(len(Xs)) * Xs)
|
||
|
|
# filtered_fft = np.fft.fft(filtered_data, n=self.FFT_N)
|
||
|
|
# self.plot_fft(ax, x_space, raw_fft, "{} FFT".format(label))
|
||
|
|
# fft_freq = np.fft.fftfreq(self.FFT_N, d=dt)
|
||
|
|
# x_space
|
||
|
|
# filtered_fft_ax = self.plot_fft(ax, fft_freq[:self.FFT_N // 2], filtered_fft, "filtered {} FFT".format(label))
|
||
|
|
|
||
|
|
ax.set_xlabel("frequency")
|
||
|
|
# ax.set_xscale("log")
|
||
|
|
# ax.xaxis.set_major_formatter(ScalarFormatter())
|
||
|
|
ax.legend(prop={'size': 8})
|
||
|
|
|
||
|
|
return filtered_fft_ax_x, filtered_fft_ax_y, filtered_fft_ax_z
|
||
|
|
|
||
|
|
def init_filter_shape(self, ax, filter, color):
|
||
|
|
center = filter.get_center_freq()
|
||
|
|
x_space, lpf_shape = self.get_filter_shape(filter)
|
||
|
|
|
||
|
|
plot_slpf_shape, = ax.plot(x_space, lpf_shape, c=color, label="LPF shape")
|
||
|
|
xvline_lpf_cutoff = ax.axvline(x=center, linestyle="--", c=color) # LPF cutoff freq
|
||
|
|
|
||
|
|
return plot_slpf_shape, xvline_lpf_cutoff
|
||
|
|
|
||
|
|
def create_slider(self, name, rect, max, value, color, callback):
|
||
|
|
global sliders
|
||
|
|
ax_slider = self.fig.add_axes(rect, facecolor='lightgoldenrodyellow')
|
||
|
|
slider = Slider(ax_slider, name, 0, max, valinit=np.sqrt(max * value), valstep=1, color=color)
|
||
|
|
slider.valtext.set_text(value)
|
||
|
|
|
||
|
|
# slider.drawon = False
|
||
|
|
|
||
|
|
def changed(val, cbk, max, slider):
|
||
|
|
# non linear slider to better control small values
|
||
|
|
val = int(val ** 2 / max)
|
||
|
|
slider.valtext.set_text(val)
|
||
|
|
cbk(val)
|
||
|
|
|
||
|
|
slider.on_changed(lambda val, cbk=callback, max=max, s=slider: changed(val, cbk, max, s))
|
||
|
|
sliders.append(slider)
|
||
|
|
|
||
|
|
def delay_update(self, update_cbk):
|
||
|
|
def _delayed_update(self, cbk):
|
||
|
|
self.timer.stop()
|
||
|
|
cbk()
|
||
|
|
|
||
|
|
# delay actual filtering
|
||
|
|
if self.fig:
|
||
|
|
if self.timer:
|
||
|
|
self.timer.stop()
|
||
|
|
self.timer = self.fig.canvas.new_timer(interval=self.FILTER_DEBOUNCE)
|
||
|
|
self.timer.add_callback(lambda self=self: _delayed_update(self, update_cbk))
|
||
|
|
self.timer.start()
|
||
|
|
|
||
|
|
def update_filter_shape(self, filter, shape, center_line):
|
||
|
|
x_data, new_shape = self.get_filter_shape(filter)
|
||
|
|
|
||
|
|
shape.set_ydata(new_shape)
|
||
|
|
center_line.set_xdata(filter.get_center_freq())
|
||
|
|
|
||
|
|
self.updated_artists.extend([
|
||
|
|
shape,
|
||
|
|
center_line,
|
||
|
|
])
|
||
|
|
|
||
|
|
def update_signal_and_fft_plot(self, filters_key, time_list, sample_lists, signal_shapes, fft_shapes, shape,
|
||
|
|
center_line, sample_rate):
|
||
|
|
# print("update_signal_and_fft_plot", self.filters[filters_key][0].get_center_freq())
|
||
|
|
Xs, Ys, Zs = sample_lists
|
||
|
|
signal_shape_x, signal_shape_y, signal_shape_z = signal_shapes
|
||
|
|
fft_shape_x, fft_shape_y, fft_shape_z = fft_shapes
|
||
|
|
|
||
|
|
Xs_filtered = self.test_filters(self.filters[filters_key], time_list, Xs)
|
||
|
|
Ys_filtered = self.test_filters(self.filters[filters_key], time_list, Ys)
|
||
|
|
Zs_filtered = self.test_filters(self.filters[filters_key], time_list, Zs)
|
||
|
|
|
||
|
|
signal_shape_x.set_ydata(Xs_filtered)
|
||
|
|
signal_shape_y.set_ydata(Ys_filtered)
|
||
|
|
signal_shape_z.set_ydata(Zs_filtered)
|
||
|
|
|
||
|
|
self.updated_artists.extend([signal_shape_x, signal_shape_y, signal_shape_z])
|
||
|
|
|
||
|
|
_freqs_x, _times_x, _stft_x = signal.stft(Xs_filtered, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
filtered_fft_x = np.average(np.abs(_stft_x), axis=1)
|
||
|
|
|
||
|
|
_freqs_y, _times_y, _stft_y = signal.stft(Ys_filtered, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
filtered_fft_y = np.average(np.abs(_stft_y), axis=1)
|
||
|
|
|
||
|
|
_freqs_z, _times_z, _stft_z = signal.stft(Zs_filtered, sample_rate, window='hann', nperseg=self.FFT_N)
|
||
|
|
filtered_fft_z = np.average(np.abs(_stft_z), axis=1)
|
||
|
|
|
||
|
|
fft_shape_x.set_ydata(filtered_fft_x)
|
||
|
|
fft_shape_y.set_ydata(filtered_fft_y)
|
||
|
|
fft_shape_z.set_ydata(filtered_fft_z)
|
||
|
|
|
||
|
|
self.updated_artists.extend([
|
||
|
|
fft_shape_x, fft_shape_y, fft_shape_z,
|
||
|
|
shape, center_line,
|
||
|
|
])
|
||
|
|
|
||
|
|
# self.fig.canvas.draw()
|
||
|
|
|
||
|
|
def animation_update(self):
|
||
|
|
updated_artists = self.updated_artists.copy()
|
||
|
|
|
||
|
|
# if updated_artists:
|
||
|
|
# print("animation update")
|
||
|
|
|
||
|
|
# reset updated artists
|
||
|
|
self.updated_artists = []
|
||
|
|
|
||
|
|
return updated_artists
|
||
|
|
|
||
|
|
def update_filter(self, val, cbk, filter, shape, center_line, filters_key, time_list, sample_lists, signal_shapes,
|
||
|
|
fft_shapes):
|
||
|
|
# this callback sets the parameter controlled by the slider
|
||
|
|
cbk(val)
|
||
|
|
# print("filter update",val)
|
||
|
|
# update filter shape and delay fft update
|
||
|
|
self.update_filter_shape(filter, shape, center_line)
|
||
|
|
sample_freq = filter.get_sample_freq()
|
||
|
|
self.delay_update(
|
||
|
|
lambda self=self: self.update_signal_and_fft_plot(filters_key, time_list, sample_lists, signal_shapes,
|
||
|
|
fft_shapes, shape, center_line, sample_freq))
|
||
|
|
|
||
|
|
def create_filter_control(self, name, filter, rect, max, default, shape, center_line, cbk, filters_key, time_list,
|
||
|
|
sample_lists, signal_shapes, fft_shapes, filt_color):
|
||
|
|
self.create_slider(name, rect, max, default, filt_color, lambda val, cbk=cbk, self=self, filter=filter, shape=shape,
|
||
|
|
center_line=center_line, filters_key=filters_key,
|
||
|
|
time_list=time_list, sample_list=sample_lists,
|
||
|
|
signal_shape=signal_shapes, fft_shape=fft_shapes:
|
||
|
|
self.update_filter(val, cbk, filter, shape, center_line, filters_key,
|
||
|
|
time_list, sample_list, signal_shape, fft_shape))
|
||
|
|
|
||
|
|
def create_controls(self, filters_key, base_rect, padding, ax_fft, time_list, sample_lists, signal_shapes,
|
||
|
|
fft_shapes):
|
||
|
|
ax_filter = ax_fft.twinx()
|
||
|
|
ax_filter.set_navigate(False)
|
||
|
|
ax_filter.set_yticks([])
|
||
|
|
|
||
|
|
num_filters = len(self.filters[filters_key])
|
||
|
|
|
||
|
|
for i, filter in enumerate(self.filters[filters_key]):
|
||
|
|
filt_type = filter.get_type()
|
||
|
|
filt_color = self.filter_color_map(i / num_filters)
|
||
|
|
filt_shape, filt_cutoff = self.init_filter_shape(ax_filter, filter, filt_color)
|
||
|
|
|
||
|
|
if filt_type == BiquadFilterType.PEAK:
|
||
|
|
name = "Notch"
|
||
|
|
else:
|
||
|
|
name = "LPF"
|
||
|
|
|
||
|
|
# control for center freq is common to all filters
|
||
|
|
self.create_filter_control("{} freq".format(name), filter, base_rect, 500, filter.get_center_freq(),
|
||
|
|
filt_shape, filt_cutoff,
|
||
|
|
lambda val, filter=filter: filter.set_center_freq(val),
|
||
|
|
filters_key, time_list, sample_lists, signal_shapes, fft_shapes, filt_color)
|
||
|
|
# move down of control height + padding
|
||
|
|
base_rect[1] -= (base_rect[3] + padding)
|
||
|
|
|
||
|
|
if filt_type == BiquadFilterType.PEAK:
|
||
|
|
self.create_filter_control("{} att (db)".format(name), filter, base_rect, 100, filter.get_attenuation(),
|
||
|
|
filt_shape, filt_cutoff,
|
||
|
|
lambda val, filter=filter: filter.set_attenuation(val),
|
||
|
|
filters_key, time_list, sample_lists, signal_shapes, fft_shapes, filt_color)
|
||
|
|
base_rect[1] -= (base_rect[3] + padding)
|
||
|
|
self.create_filter_control("{} band".format(name), filter, base_rect, 300, filter.get_bandwidth(),
|
||
|
|
filt_shape, filt_cutoff,
|
||
|
|
lambda val, filter=filter: filter.set_bandwidth(val),
|
||
|
|
filters_key, time_list, sample_lists, signal_shapes, fft_shapes, filt_color)
|
||
|
|
base_rect[1] -= (base_rect[3] + padding)
|
||
|
|
|
||
|
|
def create_spectrogram(self, data, name, sample_rate):
|
||
|
|
freqs, times, Sx = signal.spectrogram(np.array(data), fs=sample_rate, window='hanning',
|
||
|
|
nperseg=self.FFT_N, noverlap=self.FFT_N - self.FFT_N // 10,
|
||
|
|
detrend=False, scaling='spectrum')
|
||
|
|
|
||
|
|
f, ax = plt.subplots(figsize=(4.8, 2.4))
|
||
|
|
ax.pcolormesh(times, freqs, 10 * np.log10(Sx), cmap='viridis')
|
||
|
|
ax.set_title(name)
|
||
|
|
ax.set_ylabel('Frequency (Hz)')
|
||
|
|
ax.set_xlabel('Time (s)')
|
||
|
|
|
||
|
|
def init_plot(self, log_name):
|
||
|
|
|
||
|
|
self.fig = plt.figure(figsize=(14, 9))
|
||
|
|
self.fig.canvas.set_window_title("ArduPilot Filter Test Tool - {}".format(log_name))
|
||
|
|
self.fig.canvas.draw()
|
||
|
|
|
||
|
|
rows = 2
|
||
|
|
cols = 3
|
||
|
|
raw_acc_index = 1
|
||
|
|
fft_acc_index = raw_acc_index + 1
|
||
|
|
raw_gyr_index = cols + 1
|
||
|
|
fft_gyr_index = raw_gyr_index + 1
|
||
|
|
|
||
|
|
# signal
|
||
|
|
self.ax_acc = self.fig.add_subplot(rows, cols, raw_acc_index)
|
||
|
|
self.ax_gyr = self.fig.add_subplot(rows, cols, raw_gyr_index, sharex=self.ax_acc)
|
||
|
|
|
||
|
|
accx_filtered, accy_filtered, accz_filtered = self.test_acc_filters()
|
||
|
|
self.ax_filtered_accx, self.ax_filtered_accy, self.ax_filtered_accz = self.init_signal_plot(self.ax_acc,
|
||
|
|
self.ACC_t,
|
||
|
|
self.ACC_x,
|
||
|
|
self.ACC_y,
|
||
|
|
self.ACC_z,
|
||
|
|
accx_filtered,
|
||
|
|
accy_filtered,
|
||
|
|
accz_filtered,
|
||
|
|
"AccX")
|
||
|
|
|
||
|
|
gyrx_filtered, gyry_filtered, gyrz_filtered = self.test_gyr_filters()
|
||
|
|
self.ax_filtered_gyrx, self.ax_filtered_gyry, self.ax_filtered_gyrz = self.init_signal_plot(self.ax_gyr,
|
||
|
|
self.GYR_t,
|
||
|
|
self.GYR_x,
|
||
|
|
self.GYR_y,
|
||
|
|
self.GYR_z,
|
||
|
|
gyrx_filtered,
|
||
|
|
gyry_filtered,
|
||
|
|
gyrz_filtered,
|
||
|
|
"GyrX")
|
||
|
|
|
||
|
|
# FFT
|
||
|
|
self.ax_acc_fft = self.fig.add_subplot(rows, cols, fft_acc_index)
|
||
|
|
self.ax_gyr_fft = self.fig.add_subplot(rows, cols, fft_gyr_index)
|
||
|
|
|
||
|
|
self.acc_filtered_fft_ax_x, self.acc_filtered_fft_ax_y, self.acc_filtered_fft_ax_z = self.init_fft(
|
||
|
|
self.ax_acc_fft, self.ACC_t, self.ACC_x, self.ACC_y, self.ACC_z, self.ACC_freq, self.acc_dt, accx_filtered,
|
||
|
|
accy_filtered, accz_filtered, "AccX")
|
||
|
|
self.gyr_filtered_fft_ax_x, self.gyr_filtered_fft_ax_y, self.gyr_filtered_fft_ax_z = self.init_fft(
|
||
|
|
self.ax_gyr_fft, self.GYR_t, self.GYR_x, self.GYR_y, self.GYR_z, self.GYR_freq, self.gyr_dt, gyrx_filtered,
|
||
|
|
gyry_filtered, gyrz_filtered, "GyrX")
|
||
|
|
|
||
|
|
self.fig.tight_layout()
|
||
|
|
|
||
|
|
# TODO add y z
|
||
|
|
self.create_controls("acc", [0.75, 0.95, 0.2, 0.02], 0.01, self.ax_acc_fft, self.ACC_t,
|
||
|
|
(self.ACC_x, self.ACC_y, self.ACC_z),
|
||
|
|
(self.ax_filtered_accx, self.ax_filtered_accy, self.ax_filtered_accz),
|
||
|
|
(self.acc_filtered_fft_ax_x, self.acc_filtered_fft_ax_y, self.acc_filtered_fft_ax_z))
|
||
|
|
self.create_controls("gyr", [0.75, 0.45, 0.2, 0.02], 0.01, self.ax_gyr_fft, self.GYR_t,
|
||
|
|
(self.GYR_x, self.GYR_y, self.GYR_z),
|
||
|
|
(self.ax_filtered_gyrx, self.ax_filtered_gyry, self.ax_filtered_gyrz),
|
||
|
|
(self.gyr_filtered_fft_ax_x, self.gyr_filtered_fft_ax_y, self.gyr_filtered_fft_ax_z))
|
||
|
|
|
||
|
|
# setup animation for continuous update
|
||
|
|
global anim
|
||
|
|
anim = FuncAnimation(self.fig, lambda frame, self=self: self.animation_update(), interval=1, blit=False)
|
||
|
|
|
||
|
|
# Work in progress here...
|
||
|
|
# self.create_spectrogram(self.GYR_x, "GyrX", self.GYR_freq)
|
||
|
|
# self.create_spectrogram(gyrx_filtered, "GyrX filtered", self.GYR_freq)
|
||
|
|
# self.create_spectrogram(self.ACC_x, "AccX", self.ACC_freq)
|
||
|
|
# self.create_spectrogram(accx_filtered, "AccX filtered", self.ACC_freq)
|
||
|
|
|
||
|
|
plt.show()
|
||
|
|
|
||
|
|
self.print_filter_param_info()
|
||
|
|
|
||
|
|
def print_filter_param_info(self):
|
||
|
|
if len(self.filters["acc"]) > 2 or len(self.filters["gyr"]) > 2:
|
||
|
|
print("Testing too many filters unsupported from firmware, cannot calculate parameters to set them")
|
||
|
|
return
|
||
|
|
|
||
|
|
print("To have the last filter settings in the graphs set the following parameters:\n")
|
||
|
|
|
||
|
|
for f in self.filters["acc"]:
|
||
|
|
filt_type = f.get_type()
|
||
|
|
|
||
|
|
if filt_type == BiquadFilterType.PEAK: # NOTCH
|
||
|
|
print("INS_NOTCA_ENABLE,", 1)
|
||
|
|
print("INS_NOTCA_FREQ,", f.get_center_freq())
|
||
|
|
print("INS_NOTCA_BW,", f.get_bandwidth())
|
||
|
|
print("INS_NOTCA_ATT,", f.get_attenuation())
|
||
|
|
else: # LPF
|
||
|
|
print("INS_ACCEL_FILTER,", f.get_center_freq())
|
||
|
|
|
||
|
|
for f in self.filters["gyr"]:
|
||
|
|
filt_type = f.get_type()
|
||
|
|
|
||
|
|
if filt_type == BiquadFilterType.PEAK: # NOTCH
|
||
|
|
print("INS_NOTCH_ENABLE,", 1)
|
||
|
|
print("INS_NOTCH_FREQ,", f.get_center_freq())
|
||
|
|
print("INS_NOTCH_BW,", f.get_bandwidth())
|
||
|
|
print("INS_NOTCH_ATT,", f.get_attenuation())
|
||
|
|
else: # LPF
|
||
|
|
print("INS_GYRO_FILTER,", f.get_center_freq())
|
||
|
|
|
||
|
|
print("\n+---------+")
|
||
|
|
print("| WARNING |")
|
||
|
|
print("+---------+")
|
||
|
|
print("Always check the onboard FFT to setup filters, this tool only simulate effects of filtering.")
|