autotune: add possibility to select any signal for input/output

Author: bresch

autotune/data_extractor.py

@@ -41,27 +41,51 @@
 from pyulog import ULog
 from scipy.interpolate import make_interp_spline
 
-def getInputOutputData(logfile, axis, t_start=0.0, t_stop=0.0, instance=0):
-    log = ULog(logfile)
+class FieldDefinition():
+    def __init__(self, topic, var, inst):
+        self.topic_name = topic
+        self.variable_name = var
+        self.instance = inst
 
-    y_data = get_data(log, 'vehicle_angular_velocity', 'xyz[{}]'.format(axis))
-    t_y_data = us2s(get_data(log, 'vehicle_angular_velocity', 'timestamp'))
+class DataExtractor():
+    def __init__(self, logfile_name):
+        self.log = ULog(logfile_name)
 
-    u_data = get_data(log, 'vehicle_torque_setpoint', 'xyz[{}]'.format(axis))
-    t_u_data = us2s(get_data(log, 'vehicle_torque_setpoint', 'timestamp'))
+    def get_topics_list(self):
+        fields = []
+        for elem in self.log.data_list:
+            for var_name in elem.data.keys():
+                fields.append(FieldDefinition(elem.name, var_name, elem.multi_id))
 
-    v_data = get_data(log, 'airspeed_validated', 'true_airspeed_m_s')
-    t_v_data = us2s(get_data(log, 'airspeed_validated', 'timestamp'))
+        return fields
 
-    if not np.any(u_data):
-        # Check for legacy topics
-        actuator_controls_n = 'actuator_controls_{}'.format(instance)
-        u_data = get_data(log, actuator_controls_n, 'control[{}]'.format(axis))
-        t_u_data = us2s(get_data(log, actuator_controls_n, 'timestamp'))
+    def getPreview(self, field_def):
+        (t_data, data) = self.getData(field_def)
 
-    (t_aligned, u_aligned, y_aligned, v_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_v_data, v_data, t_start, t_stop)
+        if(len(t_data) > 10e3):
+            # Downsample to speed up plotting preview
+            downsampling_factor = int(len(t_data)/10e3)+1
+            t_data = t_data[:-downsampling_factor+1:downsampling_factor]
+            data = data[:-downsampling_factor+1:downsampling_factor]
 
-    return (t_aligned, u_aligned, y_aligned, v_aligned)
+        return (t_data, data)
+
+    def getData(self, field_def):
+        data = get_data(self.log, field_def.topic_name, field_def.variable_name, field_def.instance)
+        t_data = us2s(get_data(self.log, field_def.topic_name, 'timestamp', field_def.instance))
+
+        return (t_data, data)
+
+    def getInputOutputData(self, field_def_u, field_def_y, t_start=0.0, t_stop=0.0):
+        (t_u_data, u_data) = self.getData(field_def_u)
+        (t_y_data, y_data) = self.getData(field_def_y)
+
+        v_data = get_data(self.log, 'airspeed_validated', 'true_airspeed_m_s')
+        t_v_data = us2s(get_data(self.log, 'airspeed_validated', 'timestamp'))
+
+        (t_aligned, u_aligned, y_aligned, v_aligned) = resampleIdentificationData(t_u_data, u_data, t_y_data, y_data, t_v_data, v_data, t_start, t_stop)
+
+        return (t_aligned, u_aligned, y_aligned, v_aligned)
 
 def get_data(log, topic_name, variable_name, instance=0):
     variable_data = np.array([])
@@ -89,31 +113,34 @@ def resample_interp(t, u, t_new):
     interp = make_interp_spline(t, u, k=1)
     return interp(t_new)
 
-def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_v_data, v_data, t_start, t_stop):
-    if t_start == 0.0 and t_stop == 0.0:
-        # Find autotune sequence
-        status_data = get_data(log, 'autotune_attitude_control_status', 'state')
-        t_status = us2s(get_data(log, 'autotune_attitude_control_status', 'timestamp'))
-        axis_to_state = [2, 4, 6] # roll, pitch, yaw states
+def find_autotune_sequence(log, axis):
+    t_start = None
+    t_stop = None
+    status_data = get_data(log, 'autotune_attitude_control_status', 'state')
+    t_status = us2s(get_data(log, 'autotune_attitude_control_status', 'timestamp'))
+    axis_to_state = [2, 4, 6] # roll, pitch, yaw states
 
-        status_prev = 0
+    status_prev = 0
 
-        for i_s in range(len(t_status)):
-            if status_data[i_s] == axis_to_state[axis]:
-                if status_prev != axis_to_state[axis]:
-                    t_start = t_status[i_s]
+    for i_s in range(len(t_status)):
+        if status_data[i_s] == axis_to_state[axis]:
+            if status_prev != axis_to_state[axis]:
+                t_start = t_status[i_s]
+
+        else:
+            if status_prev == axis_to_state[axis]:
+                t_stop = t_status[i_s]
+                break
 
-            else:
-                if status_prev == axis_to_state[axis]:
-                    t_stop = t_status[i_s]
-                    break
+        status_prev = status_data[i_s]
 
-            status_prev = status_data[i_s]
+    return (t_start, t_stop)
 
-    if t_start == 0.0:
+def resampleIdentificationData(t_u_data, u_data, t_y_data, y_data, t_v_data, v_data, t_start, t_stop):
+    if not t_start:
         t_start = t_u_data[0]
 
-    if t_stop == 0.0:
+    if not t_stop:
         t_stop = t_u_data[-1]
 
     dt = get_delta_mean(t_y_data)

autotune/data_selection_window.py

@@ -1,19 +1,28 @@
-from PyQt5.QtWidgets import QDialog, QVBoxLayout, QHBoxLayout, QPushButton, QLabel, QFormLayout, QRadioButton, QMessageBox, QFileDialog
+from PyQt5.QtWidgets import QDialog, QVBoxLayout, QHBoxLayout, QFormLayout, QPushButton, QLabel, QFormLayout, QRadioButton, QMessageBox, QFileDialog, QComboBox
 
 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
+from data_extractor import DataExtractor
 
 class DataSelectionWindow(QDialog):
     def __init__(self, filename):
         QDialog.__init__(self)
 
+        self.t = []
+        self.u = []
+        self.y = []
+        self.t_start = None
+        self.t_stop = None
+
+        self.input_ref = None
+        self.output_ref = None
         self.figure = plt.figure(1)
         self.canvas = FigureCanvas(self.figure)
+        self.initPlot()
 
         layout_v = QVBoxLayout()
 
@@ -22,51 +31,59 @@ def __init__(self, filename):
         btn_browse.clicked.connect(self.browseFiles)
         top_group.addWidget(btn_browse)
 
+        in_out_group = QFormLayout()
+        self.combo_u = QComboBox()
+        self.combo_u.setEditable(True)
+        self.combo_u.setInsertPolicy(QComboBox.NoInsert)
+        self.combo_u.currentIndexChanged.connect(self.selectUData)
+        in_out_group.addRow(QLabel("Input:"), self.combo_u)
+
+        self.combo_y = QComboBox()
+        self.combo_y.setEditable(True)
+        self.combo_y.setInsertPolicy(QComboBox.NoInsert)
+        self.combo_y.currentIndexChanged.connect(self.selectYData)
+        in_out_group.addRow(QLabel("Output:"), self.combo_y)
+        top_group.addLayout(in_out_group)
+
         layout_v.addLayout(top_group)
         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)
+        btn_ok.clicked.connect(self.loadSelection)
         layout_v.addWidget(btn_ok)
 
         self.setLayout(layout_v)
 
         if filename:
             self.file_name = filename
-            self.refreshInputOutputData()
+            self.openFile()
 
         else:
             self.browseFiles()
 
-    def loadLog(self):
-        if self.t_stop > self.t_start:
-            (self.t, self.u, self.y, self.v) = getInputOutputData(self.file_name, self.axis, self.t_start, self.t_stop)
+    def loadSelection(self):
+        if (self.t_start is None and self.t_start is None) or (self.t_stop > self.t_start):
+            (self.t, self.u, self.y, self.v) = self.data_extractor.getInputOutputData(self.topics[self.index_u], self.topics[self.index_y], self.t_start, self.t_stop)
             self.accept()
         else:
             self.printRangeError()
 
     def browseFiles(self):
         options = QFileDialog.Options()
         options |= QFileDialog.DontUseNativeDialog
-        self.file_name, _ = QFileDialog.getOpenFileName(self,"QFileDialog.getOpenFileName()", "","ULog (*.ulg)", options=options)
+        file_name, _ = QFileDialog.getOpenFileName(self,"Select ULog file", "","ULog (*.ulg)", options=options)
+        self.file_name = file_name
+        self.openFile()
 
+    def openFile(self):
         if self.file_name:
-            self.refreshInputOutputData()
+            self.data_extractor = DataExtractor(self.file_name)
+            self.topics = self.data_extractor.get_topics_list()
+            list_names = [f"{topic.topic_name}/{topic.variable_name}.{topic.instance}" for topic in self.topics]
+            self.combo_u.clear()
+            self.combo_u.addItems(list_names)
+            self.combo_y.clear()
+            self.combo_y.addItems(list_names)
 
     def printRangeError(self):
         msg = QMessageBox()
@@ -75,55 +92,59 @@ def printRangeError(self):
         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
-            (t, u, y, _) = getInputOutputData(self.file_name, axis)
-
-            if(len(t) > 10e3):
-                # Downsample to speed up plotting preview
-                downsampling_factor = int(len(t)/10e3)+1
-                self.t = t[:-downsampling_factor+1:downsampling_factor]
-                self.u = u[:-downsampling_factor+1:downsampling_factor]
-                self.y = y[:-downsampling_factor+1:downsampling_factor]
-
-            else:
-                self.t = t
-                self.u = u
-                self.y = y
-
-            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)
+    def selectUData(self, index):
+        self.index_u = index
+        (self.t, self.u) = self.data_extractor.getPreview(self.topics[index])
+        self.plotU()
+
+    def selectYData(self, index):
+        self.index_y = index
+        (self.t, self.y) = self.data_extractor.getPreview(self.topics[index])
+        self.plotY()
+
+    def initPlot(self):
+        if self.input_ref is None:
+            self.figure.clear()
+            self.ax = self.figure.add_subplot(1,1,1)
+            plot_refs = self.ax.plot([], [])
+            self.input_ref = plot_refs[0]
+
+            plot_refs = self.ax.plot([], [])
+            self.output_ref = plot_refs[0]
+            self.ax.autoscale(False)
+
+            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.canvas.mpl_connect('scroll_event', self.zoom_fun)
+            self.canvas.draw()
+
+    def plotU(self):
+        self.input_ref.set_xdata(self.t)
+        self.input_ref.set_ydata(self.u)
+        self.resetXYLim()
+        self.canvas.draw()
 
-        self.t_start = self.t[0]
-        self.t_stop = self.t[-1]
+    def plotY(self):
+        self.output_ref.set_xdata(self.t)
+        self.output_ref.set_ydata(self.y)
+        self.resetXYLim()
+        self.canvas.draw()
 
-        self.canvas.mpl_connect('scroll_event', self.zoom_fun)
+    def resetXYLim(self):
+        self.ax.set_xlim([self.t[0], self.t[-1]])
 
-        self.canvas.draw()
+        if len(self.u) > 0 and len(self.y) > 0:
+            self.ax.set_ylim([min([min(self.u), min(self.y)]), max([max(self.u), max(self.y)])])
+        elif len(self.u) > 0:
+            self.ax.set_ylim([min(self.u), max(self.u)])
+        elif len(self.y) > 0:
+            self.ax.set_ylim([min(self.y), max(self.y)])
 
     def onselect(self, xmin, xmax):
         indmin, indmax = np.searchsorted(self.t, (xmin, xmax))