Skip to content

Commit

Permalink
autotune: add data selector for manual range selection
Browse files Browse the repository at this point in the history
  • Loading branch information
bresch committed Aug 17, 2022
1 parent 3f9f2f6 commit d643084
Show file tree
Hide file tree
Showing 3 changed files with 155 additions and 46 deletions.
50 changes: 11 additions & 39 deletions autotune/autotune.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@
import control as ctrl
from scipy.signal import resample, detrend

from data_selection_window import DataSelectionWindow

class Window(QDialog):
def __init__(self, parent=None):
super(Window, self).__init__(parent)
Expand Down Expand Up @@ -95,21 +97,6 @@ def __init__(self, parent=None):
left_menu = QVBoxLayout()
left_menu.addWidget(self.btn_open_log)

xyz_group = QHBoxLayout()
r_x = QRadioButton("x")
r_x.setChecked(True)
r_y = QRadioButton("y")
r_z = QRadioButton("z")
xyz_group.addWidget(QLabel("Axis"))
xyz_group.addWidget(r_x)
xyz_group.addWidget(r_y)
xyz_group.addWidget(r_z)
r_x.clicked.connect(self.loadXData)
r_y.clicked.connect(self.loadYData)
r_z.clicked.connect(self.loadZData)

left_menu.addLayout(xyz_group)

pz_group = QFormLayout()
self.line_edit_zeros = QSpinBox()
self.line_edit_zeros.setValue(self.sys_id_n_zeros)
Expand Down Expand Up @@ -601,37 +588,22 @@ def loadLog(self):
options = QFileDialog.Options()
options |= QFileDialog.DontUseNativeDialog
self.file_name, _ = QFileDialog.getOpenFileName(self,"QFileDialog.getOpenFileName()", "","ULog (*.ulg)", options=options)
if self.file_name:
self.reset()
self.refreshInputOutputData()
self.runIdentification()
self.computeController()

def loadXData(self):
self.axis = 0
if self.file_name:
self.refreshInputOutputData()
self.runIdentification()
self.computeController()
select = DataSelectionWindow(self.file_name)

def loadYData(self):
self.axis = 1
if self.file_name:
self.refreshInputOutputData()
self.runIdentification()
self.computeController()

def loadZData(self):
self.axis = 2
if self.file_name:
self.refreshInputOutputData()
self.runIdentification()
self.computeController()
if select.exec_():
self.reset()
self.t = select.t - select.t[0]
self.u = select.u
self.y = select.y
self.refreshInputOutputData()
self.runIdentification()
self.computeController()

def refreshInputOutputData(self):
self.reset()
if self.file_name:
(self.t, self.u, self.y) = getInputOutputData(self.file_name, self.axis)
dt = max(get_delta_mean(self.t), 0.008)
self.resampleData(dt)
self.plotInputOutput(redraw=True)
Expand Down
18 changes: 11 additions & 7 deletions autotune/data_extractor.py
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
from pyulog import ULog
from scipy.signal import resample

def getInputOutputData(logfile, axis, instance=0):
def getInputOutputData(logfile, axis, t_start=0.0, t_stop=0.0, instance=0):
log = ULog(logfile)

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

(t_aligned, u_aligned, y_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis)

t_aligned -= t_aligned[0]
(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)

return (t_aligned, u_aligned, y_aligned)

Expand All @@ -79,7 +77,7 @@ def get_delta_mean(data_list):
dx = dx/(length-1)
return dx

def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis):
def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_start, t_stop):
status_data = get_data(log, 'autotune_attitude_control_status', 'state')
t_status = us2s(get_data(log, 'autotune_attitude_control_status', 'timestamp'))

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

if t_start == 0.0:
t_start = t_u_data[0]

if t_stop == 0.0:
t_stop = t_u_data[-1]

for i_u in range(len(t_u_data)):
t_u = t_u_data[i_u]
while t_y_data[i_y] <= t_u and i_y < len_y-1:
Expand All @@ -103,12 +107,12 @@ def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis):

status_aligned = status_data[i_s-1]

if status_aligned == axis_to_state[axis]:
if status_aligned == axis_to_state[axis] and t_u >= t_start and t_u <= t_stop:
u_aligned.append(u_data[i_u])
y_aligned.append(y_data[i_y-1])
t_aligned.append(t_u)

else:
elif t_u >= t_start and t_u <= t_stop:
u_aligned.append(u_data[i_u])
y_aligned.append(y_data[i_y-1])
t_aligned.append(t_u)
Expand Down
133 changes: 133 additions & 0 deletions autotune/data_selection_window.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,133 @@
from PyQt5.QtWidgets import QDialog, QVBoxLayout, QHBoxLayout, QPushButton, QLabel, QFormLayout, QRadioButton, QMessageBox

import matplotlib.pyplot as plt
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.widgets import SpanSelector

import numpy as np

from data_extractor import getInputOutputData

class DataSelectionWindow(QDialog):
def __init__(self, filename):
QDialog.__init__(self)

self.file_name = filename

self.figure = plt.figure(1)
self.canvas = FigureCanvas(self.figure)

layout_v = QVBoxLayout()

layout_v.addWidget(self.canvas)

xyz_group = QHBoxLayout()
r_x = QRadioButton("x")
r_x.setChecked(True)
r_y = QRadioButton("y")
r_z = QRadioButton("z")
xyz_group.addWidget(QLabel("Axis"))
xyz_group.addWidget(r_x)
xyz_group.addWidget(r_y)
xyz_group.addWidget(r_z)
r_x.clicked.connect(self.loadXData)
r_y.clicked.connect(self.loadYData)
r_z.clicked.connect(self.loadZData)

layout_v.addLayout(xyz_group)

btn_ok = QPushButton("Load selection")
btn_ok.clicked.connect(self.loadLog)
layout_v.addWidget(btn_ok)

self.setLayout(layout_v)

self.refreshInputOutputData()

def loadLog(self):
if self.t_stop > self.t_start:
(self.t, self.u, self.y) = getInputOutputData(self.file_name, self.axis, self.t_start, self.t_stop)
self.accept()
else:
self.printRangeError()

def printRangeError(self):
msg = QMessageBox()
msg.setIcon(QMessageBox.Critical)
msg.setWindowTitle("Error")
msg.setText("Range is invalid")
msg.exec_()

def loadXData(self):
if self.file_name:
self.refreshInputOutputData(0)

def loadYData(self):
if self.file_name:
self.refreshInputOutputData(1)

def loadZData(self):
if self.file_name:
self.refreshInputOutputData(2)

def refreshInputOutputData(self, axis=0):
if self.file_name:
self.axis = axis
(self.t, self.u, self.y) = getInputOutputData(self.file_name, axis)
self.plotInputOutput(redraw=True)

def plotInputOutput(self, redraw=False):
self.figure.clear()
self.ax = self.figure.add_subplot(1,1,1)
self.ax.plot(self.t, self.u, self.t, self.y)
self.ax.set_title("Click and drag to select data range")
self.ax.set_xlabel("Time (s)")
self.ax.set_ylabel("Amplitude")
self.ax.legend(["Input", "Output"])

self.span = SpanSelector(self.ax, self.onselect, 'horizontal', useblit=False,
props=dict(alpha=0.2, facecolor='green'), interactive=True)

self.t_start = self.t[0]
self.t_stop = self.t[-1]

self.canvas.mpl_connect('scroll_event', self.zoom_fun)

self.canvas.draw()

def onselect(self, xmin, xmax):
indmin, indmax = np.searchsorted(self.t, (xmin, xmax))
indmax = min(len(self.t) - 1, indmax)
indmin = min(indmin, indmax)

self.t_start = self.t[indmin]
self.t_stop = self.t[indmax]
self.ax.set_xlim(self.t_start - 1.0, self.t_stop + 1.0)
self.canvas.draw()

def zoom_fun(self, event):
base_scale = 1.1
# get the current x and y limits
cur_xlim = self.ax.get_xlim()
cur_xrange = cur_xlim[1] - cur_xlim[0]
xdata = event.xdata # get event x location
if xdata is None or xdata < cur_xlim[0] or xdata > cur_xlim[1]:
return

if event.button == 'up':
# deal with zoom in
scale_factor = 1/base_scale
elif event.button == 'down':
# deal with zoom out
scale_factor = base_scale
else:
# deal with something that should never happen
scale_factor = 1
# set new limits
new_x_min = xdata - (xdata - cur_xlim[0])*scale_factor
new_x_max = xdata + (xdata - new_x_min) / (xdata - cur_xlim[0]) * (cur_xlim[1] - xdata)

new_x_min = max(new_x_min, self.t[0] - 1.0)
new_x_max = min(new_x_max, self.t[-1] + 1.0)
self.ax.set_xlim([new_x_min, new_x_max])
self.canvas.draw()

0 comments on commit d643084

Please sign in to comment.