Skip to content

Commit d643084

Browse files
committed
autotune: add data selector for manual range selection
1 parent 3f9f2f6 commit d643084

3 files changed

Lines changed: 155 additions & 46 deletions

File tree

autotune/autotune.py

Lines changed: 11 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,8 @@
5151
import control as ctrl
5252
from scipy.signal import resample, detrend
5353

54+
from data_selection_window import DataSelectionWindow
55+
5456
class Window(QDialog):
5557
def __init__(self, parent=None):
5658
super(Window, self).__init__(parent)
@@ -95,21 +97,6 @@ def __init__(self, parent=None):
9597
left_menu = QVBoxLayout()
9698
left_menu.addWidget(self.btn_open_log)
9799

98-
xyz_group = QHBoxLayout()
99-
r_x = QRadioButton("x")
100-
r_x.setChecked(True)
101-
r_y = QRadioButton("y")
102-
r_z = QRadioButton("z")
103-
xyz_group.addWidget(QLabel("Axis"))
104-
xyz_group.addWidget(r_x)
105-
xyz_group.addWidget(r_y)
106-
xyz_group.addWidget(r_z)
107-
r_x.clicked.connect(self.loadXData)
108-
r_y.clicked.connect(self.loadYData)
109-
r_z.clicked.connect(self.loadZData)
110-
111-
left_menu.addLayout(xyz_group)
112-
113100
pz_group = QFormLayout()
114101
self.line_edit_zeros = QSpinBox()
115102
self.line_edit_zeros.setValue(self.sys_id_n_zeros)
@@ -601,37 +588,22 @@ def loadLog(self):
601588
options = QFileDialog.Options()
602589
options |= QFileDialog.DontUseNativeDialog
603590
self.file_name, _ = QFileDialog.getOpenFileName(self,"QFileDialog.getOpenFileName()", "","ULog (*.ulg)", options=options)
604-
if self.file_name:
605-
self.reset()
606-
self.refreshInputOutputData()
607-
self.runIdentification()
608-
self.computeController()
609591

610-
def loadXData(self):
611-
self.axis = 0
612592
if self.file_name:
613-
self.refreshInputOutputData()
614-
self.runIdentification()
615-
self.computeController()
593+
select = DataSelectionWindow(self.file_name)
616594

617-
def loadYData(self):
618-
self.axis = 1
619-
if self.file_name:
620-
self.refreshInputOutputData()
621-
self.runIdentification()
622-
self.computeController()
623-
624-
def loadZData(self):
625-
self.axis = 2
626-
if self.file_name:
627-
self.refreshInputOutputData()
628-
self.runIdentification()
629-
self.computeController()
595+
if select.exec_():
596+
self.reset()
597+
self.t = select.t - select.t[0]
598+
self.u = select.u
599+
self.y = select.y
600+
self.refreshInputOutputData()
601+
self.runIdentification()
602+
self.computeController()
630603

631604
def refreshInputOutputData(self):
632605
self.reset()
633606
if self.file_name:
634-
(self.t, self.u, self.y) = getInputOutputData(self.file_name, self.axis)
635607
dt = max(get_delta_mean(self.t), 0.008)
636608
self.resampleData(dt)
637609
self.plotInputOutput(redraw=True)

autotune/data_extractor.py

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
from pyulog import ULog
4242
from scipy.signal import resample
4343

44-
def getInputOutputData(logfile, axis, instance=0):
44+
def getInputOutputData(logfile, axis, t_start=0.0, t_stop=0.0, instance=0):
4545
log = ULog(logfile)
4646

4747
y_data = get_data(log, 'vehicle_angular_velocity', 'xyz[{}]'.format(axis))
@@ -51,9 +51,7 @@ def getInputOutputData(logfile, axis, instance=0):
5151
u_data = get_data(log, actuator_controls_n, 'control[{}]'.format(axis))
5252
t_u_data = us2s(get_data(log, actuator_controls_n, 'timestamp'))
5353

54-
(t_aligned, u_aligned, y_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis)
55-
56-
t_aligned -= t_aligned[0]
54+
(t_aligned, u_aligned, y_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_start, t_stop)
5755

5856
return (t_aligned, u_aligned, y_aligned)
5957

@@ -79,7 +77,7 @@ def get_delta_mean(data_list):
7977
dx = dx/(length-1)
8078
return dx
8179

82-
def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis):
80+
def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_start, t_stop):
8381
status_data = get_data(log, 'autotune_attitude_control_status', 'state')
8482
t_status = us2s(get_data(log, 'autotune_attitude_control_status', 'timestamp'))
8583

@@ -92,6 +90,12 @@ def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis):
9290
t_aligned = []
9391
axis_to_state = [2, 4, 6] # roll, pitch, yaw states
9492

93+
if t_start == 0.0:
94+
t_start = t_u_data[0]
95+
96+
if t_stop == 0.0:
97+
t_stop = t_u_data[-1]
98+
9599
for i_u in range(len(t_u_data)):
96100
t_u = t_u_data[i_u]
97101
while t_y_data[i_y] <= t_u and i_y < len_y-1:
@@ -103,12 +107,12 @@ def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis):
103107

104108
status_aligned = status_data[i_s-1]
105109

106-
if status_aligned == axis_to_state[axis]:
110+
if status_aligned == axis_to_state[axis] and t_u >= t_start and t_u <= t_stop:
107111
u_aligned.append(u_data[i_u])
108112
y_aligned.append(y_data[i_y-1])
109113
t_aligned.append(t_u)
110114

111-
else:
115+
elif t_u >= t_start and t_u <= t_stop:
112116
u_aligned.append(u_data[i_u])
113117
y_aligned.append(y_data[i_y-1])
114118
t_aligned.append(t_u)

autotune/data_selection_window.py

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
from PyQt5.QtWidgets import QDialog, QVBoxLayout, QHBoxLayout, QPushButton, QLabel, QFormLayout, QRadioButton, QMessageBox
2+
3+
import matplotlib.pyplot as plt
4+
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
5+
from matplotlib.widgets import SpanSelector
6+
7+
import numpy as np
8+
9+
from data_extractor import getInputOutputData
10+
11+
class DataSelectionWindow(QDialog):
12+
def __init__(self, filename):
13+
QDialog.__init__(self)
14+
15+
self.file_name = filename
16+
17+
self.figure = plt.figure(1)
18+
self.canvas = FigureCanvas(self.figure)
19+
20+
layout_v = QVBoxLayout()
21+
22+
layout_v.addWidget(self.canvas)
23+
24+
xyz_group = QHBoxLayout()
25+
r_x = QRadioButton("x")
26+
r_x.setChecked(True)
27+
r_y = QRadioButton("y")
28+
r_z = QRadioButton("z")
29+
xyz_group.addWidget(QLabel("Axis"))
30+
xyz_group.addWidget(r_x)
31+
xyz_group.addWidget(r_y)
32+
xyz_group.addWidget(r_z)
33+
r_x.clicked.connect(self.loadXData)
34+
r_y.clicked.connect(self.loadYData)
35+
r_z.clicked.connect(self.loadZData)
36+
37+
layout_v.addLayout(xyz_group)
38+
39+
btn_ok = QPushButton("Load selection")
40+
btn_ok.clicked.connect(self.loadLog)
41+
layout_v.addWidget(btn_ok)
42+
43+
self.setLayout(layout_v)
44+
45+
self.refreshInputOutputData()
46+
47+
def loadLog(self):
48+
if self.t_stop > self.t_start:
49+
(self.t, self.u, self.y) = getInputOutputData(self.file_name, self.axis, self.t_start, self.t_stop)
50+
self.accept()
51+
else:
52+
self.printRangeError()
53+
54+
def printRangeError(self):
55+
msg = QMessageBox()
56+
msg.setIcon(QMessageBox.Critical)
57+
msg.setWindowTitle("Error")
58+
msg.setText("Range is invalid")
59+
msg.exec_()
60+
61+
def loadXData(self):
62+
if self.file_name:
63+
self.refreshInputOutputData(0)
64+
65+
def loadYData(self):
66+
if self.file_name:
67+
self.refreshInputOutputData(1)
68+
69+
def loadZData(self):
70+
if self.file_name:
71+
self.refreshInputOutputData(2)
72+
73+
def refreshInputOutputData(self, axis=0):
74+
if self.file_name:
75+
self.axis = axis
76+
(self.t, self.u, self.y) = getInputOutputData(self.file_name, axis)
77+
self.plotInputOutput(redraw=True)
78+
79+
def plotInputOutput(self, redraw=False):
80+
self.figure.clear()
81+
self.ax = self.figure.add_subplot(1,1,1)
82+
self.ax.plot(self.t, self.u, self.t, self.y)
83+
self.ax.set_title("Click and drag to select data range")
84+
self.ax.set_xlabel("Time (s)")
85+
self.ax.set_ylabel("Amplitude")
86+
self.ax.legend(["Input", "Output"])
87+
88+
self.span = SpanSelector(self.ax, self.onselect, 'horizontal', useblit=False,
89+
props=dict(alpha=0.2, facecolor='green'), interactive=True)
90+
91+
self.t_start = self.t[0]
92+
self.t_stop = self.t[-1]
93+
94+
self.canvas.mpl_connect('scroll_event', self.zoom_fun)
95+
96+
self.canvas.draw()
97+
98+
def onselect(self, xmin, xmax):
99+
indmin, indmax = np.searchsorted(self.t, (xmin, xmax))
100+
indmax = min(len(self.t) - 1, indmax)
101+
indmin = min(indmin, indmax)
102+
103+
self.t_start = self.t[indmin]
104+
self.t_stop = self.t[indmax]
105+
self.ax.set_xlim(self.t_start - 1.0, self.t_stop + 1.0)
106+
self.canvas.draw()
107+
108+
def zoom_fun(self, event):
109+
base_scale = 1.1
110+
# get the current x and y limits
111+
cur_xlim = self.ax.get_xlim()
112+
cur_xrange = cur_xlim[1] - cur_xlim[0]
113+
xdata = event.xdata # get event x location
114+
if xdata is None or xdata < cur_xlim[0] or xdata > cur_xlim[1]:
115+
return
116+
117+
if event.button == 'up':
118+
# deal with zoom in
119+
scale_factor = 1/base_scale
120+
elif event.button == 'down':
121+
# deal with zoom out
122+
scale_factor = base_scale
123+
else:
124+
# deal with something that should never happen
125+
scale_factor = 1
126+
# set new limits
127+
new_x_min = xdata - (xdata - cur_xlim[0])*scale_factor
128+
new_x_max = xdata + (xdata - new_x_min) / (xdata - cur_xlim[0]) * (cur_xlim[1] - xdata)
129+
130+
new_x_min = max(new_x_min, self.t[0] - 1.0)
131+
new_x_max = min(new_x_max, self.t[-1] + 1.0)
132+
self.ax.set_xlim([new_x_min, new_x_max])
133+
self.canvas.draw()

0 commit comments

Comments
 (0)