feat(autotune): Step response options and gain/phare margin colors

autotune/README.md

@@ -27,7 +27,7 @@ pip3 install numpy scipy pyulog control pyqt5
 python3 autotune.py
 ```
 
-![image](https://github.com/user-attachments/assets/fcdf5c25-d92d-4487-9736-e77f6576d180)
+![Autotune GUI main window](docs/gui_main_window.png)
 
 # Tuning Worflow
 

autotune/autotune.py

@@ -61,6 +61,7 @@
     QHeaderView,
     QLabel,
     QLineEdit,
+    QMenu,
     QMessageBox,
     QPushButton,
     QRadioButton,
@@ -72,6 +73,7 @@
     QTabWidget,
     QVBoxLayout,
     QWidget,
+    QWidgetAction,
 )
 from scipy.signal import detrend
 from system_identification import SystemIdentification
@@ -168,6 +170,14 @@ def run(self):
         self.finished.emit(best_params, best_fit)
 
 
+def thresholdColor(value, limit, limit_is_minimum):
+    """Green when `value` respects `limit`, red when it violates it, None if unknown."""
+    if value is None or not np.isfinite(value):
+        return None
+    respected = value >= limit if limit_is_minimum else value <= limit
+    return "green" if respected else "red"
+
+
 def isNumber(value):
     try:
         float(value)
@@ -183,6 +193,7 @@ def __init__(self, parent=None):
         self.model_ref = None
         self.input_ref = None
         self.closed_loop_ref = None
+        self.closed_loop_step_ref = None
         self.closed_loop_ax = None
         self.measured_step_info = None
         self.step_info_patches = []
@@ -194,6 +205,7 @@ def __init__(self, parent=None):
             "settling_time": 0.4,
         }
         self.bode_plot_ref = []
+        self.margin_text_refs = {}
         self.pz_plot_refs = []
         self.file_name = None
         self.is_system_identified = False
@@ -212,6 +224,11 @@ def __init__(self, parent=None):
         self.sys_id_n_poles = 2
 
         self.kDisturbanceTime = 1.0
+        self.kMinGainMarginDb = 6.0
+        self.kMinPhaseMarginDeg = 45.0
+        self.step_duration = 2.0
+        self.disturbance_amplitude = -0.05
+        self.step_sim_spinbox = {}
 
         # this is the Canvas Widget that displays the `figure`
         # it takes the `figure` instance as a parameter to __init__
@@ -281,6 +298,7 @@ def reset(self):
         self.model_ref = None
         self.input_ref = None
         self.closed_loop_ref = None
+        self.closed_loop_step_ref = None
         self.measured_step_info = None
         self.step_info_patches = []
         self.lbl_fit.setText("—")
@@ -289,6 +307,7 @@ def reset(self):
         self.lbl_stability.setStyleSheet("")
         self.btn_stabilize.setVisible(False)
         self.bode_plot_ref = []
+        self.margin_text_refs = {}
         self.pz_plot_refs = []
         self.is_system_identified = False
 
@@ -324,7 +343,7 @@ def createPreprocessingWidget(self, layout):
         self.f_hp_spinbox.setRange(0.0, 50.0)
         self.f_hp_spinbox.setSingleStep(0.1)
         self.f_hp_spinbox.setDecimals(1)
-        self.f_hp_spinbox.setValue(0.5)
+        self.f_hp_spinbox.setValue(0.0)
         self.f_hp_spinbox.valueChanged.connect(
             lambda: self.btn_run_sys_id.setEnabled(True)
         )
@@ -628,9 +647,71 @@ def createStepInfoGroup(self):
             grid.addWidget(sb, row, 1)
             grid.addWidget(measured_lbl, row, 2)
 
+        btn_plot_options = QPushButton("Plot options")
+        btn_plot_options.setMenu(self.createPlotOptionsMenu(btn_plot_options))
+        grid.addWidget(btn_plot_options, len(specs) + 1, 0, 1, 3)
+
         group.setLayout(grid)
         return group
 
+    def createPlotOptionsMenu(self, parent):
+        sim_specs = {
+            "step_duration": (
+                "Plot duration",
+                self.step_duration,
+                0.1,
+                20.0,
+                1.0,
+                1,
+                "s",
+            ),
+            "disturbance_time": (
+                "Disturbance time",
+                self.kDisturbanceTime,
+                0.0,
+                20.0,
+                1.0,
+                1,
+                "s",
+            ),
+            "disturbance_amplitude": (
+                "Disturbance ampl.",
+                self.disturbance_amplitude,
+                -1.0,
+                1.0,
+                0.1,
+                2,
+                "",
+            ),
+        }
+        menu = QMenu(parent)
+        content = QWidget(menu)
+        form = QFormLayout(content)
+        for key, (label, default, lo, hi, step, decimals, unit) in sim_specs.items():
+            sb = QDoubleSpinBox()
+            sb.setRange(lo, hi)
+            sb.setSingleStep(step)
+            sb.setDecimals(decimals)
+            sb.setValue(default)
+            sb.valueChanged.connect(self.onStepSimChanged)
+            self.step_sim_spinbox[key] = sb
+
+            unit_str = " (" + unit + ")" if unit else ""
+            form.addRow(label + unit_str, sb)
+
+        action = QWidgetAction(menu)
+        action.setDefaultWidget(content)
+        menu.addAction(action)
+        return menu
+
+    def onStepSimChanged(self):
+        self.step_duration = self.step_sim_spinbox["step_duration"].value()
+        self.kDisturbanceTime = self.step_sim_spinbox["disturbance_time"].value()
+        self.disturbance_amplitude = self.step_sim_spinbox[
+            "disturbance_amplitude"
+        ].value()
+        self.updateClosedLoop()
+
     def onStepInfoChanged(self):
         for key in self.step_info:
             self.step_info[key] = self.step_info_spinbox[key].value()
@@ -670,12 +751,10 @@ def updateStepInfoEnvelope(self):
             else:
                 measured = self.measured_step_info[info_key]
                 lbl.setText(fmt(measured))
-                if np.isnan(measured):
-                    lbl.setStyleSheet("")
-                elif measured > self.step_info[key]:
-                    lbl.setStyleSheet("color: red")
-                else:
-                    lbl.setStyleSheet("color: green")
+                color = thresholdColor(
+                    measured, self.step_info[key], limit_is_minimum=False
+                )
+                lbl.setStyleSheet(f"color: {color}" if color else "")
 
         self.canvas.draw()
 
@@ -1037,7 +1116,9 @@ def updateClosedLoop(self):
             outputs="y",
         )
 
-        t_out, y_out = ctrl.step_response(closed_loop, T=np.arange(0, 2, dt))
+        t_out, y_out = ctrl.step_response(
+            closed_loop, T=np.arange(0, self.step_duration, dt)
+        )
 
         # Add disturbance
         sum_feedback_no_ref = ctrl.summing_junction(inputs=["-y"], output="e")
@@ -1060,7 +1141,7 @@ def updateClosedLoop(self):
             outputs="y",
         )
         d = np.zeros_like(t_out)
-        d[t_out >= self.kDisturbanceTime] = -0.05  # TODO: parameterize
+        d[t_out >= self.kDisturbanceTime] = self.disturbance_amplitude
         _, y_d = ctrl.forced_response(disturbance_loop, t_out, d)
         y_out += y_d
 
@@ -1101,19 +1182,22 @@ def plotClosedLoop(self, t, y):
         except (IndexError, ValueError):
             self.measured_step_info = None
 
+        step_ref = [1 if i > 0 else 0 for i in t]
         if self.closed_loop_ref is None:
             ax = self.figure.add_subplot(3, 3, 7)
-            ax.step(t, [1 if i > 0 else 0 for i in t], "k--")
+            self.closed_loop_step_ref = ax.step(t, step_ref, "k--")[0]
             plot_ref = ax.plot(t, y)
             self.closed_loop_ref = plot_ref[0]
             self.closed_loop_ax = ax
             ax.set_title("Closed-loop step response")
             ax.set_xlabel("Time (s)")
             ax.set_ylabel("Amplitude (rad/s)")
         else:
+            self.closed_loop_step_ref.set_data(t, step_ref)
             self.closed_loop_ref.set_xdata(t)
             self.closed_loop_ref.set_ydata(y)
             self.closed_loop_ax.set_ylim(np.min(y), np.max([1.5, np.max(y)]))
+        self.closed_loop_ax.set_xlim(t[0], t[-1])
 
         self.updateStepInfoEnvelope()
 
@@ -1127,7 +1211,21 @@ def plotBode(self, open_loop, closed_loop):
             gain_crossover,
             stab_margin_w,
         ) = ctrl.stability_margins(open_loop)
-        stability_margins_text = f"Gain margin: {20 * np.log10(gain_margin):.2f}dB (@{phase_crossover / (2 * np.pi):.1f}Hz)\nPhase margin: {phase_margin:.1f}deg (@{gain_crossover / (2 * np.pi):.1f}Hz)"
+        gain_margin_db = 20 * np.log10(gain_margin)
+        margins = {
+            "gain": (
+                f"Gain margin: {gain_margin_db:.2f}dB (@{phase_crossover / (2 * np.pi):.1f}Hz)",
+                thresholdColor(
+                    gain_margin_db, self.kMinGainMarginDb, limit_is_minimum=True
+                ),
+            ),
+            "phase": (
+                f"Phase margin: {phase_margin:.1f}deg (@{gain_crossover / (2 * np.pi):.1f}Hz)",
+                thresholdColor(
+                    phase_margin, self.kMinPhaseMarginDeg, limit_is_minimum=True
+                ),
+            ),
+        }
 
         w = np.logspace(-1, 3, 40).tolist()
         (mag_ol, phase_ol, omega_ol) = ctrl.frequency_response(
@@ -1162,13 +1260,14 @@ def plotBode(self, open_loop, closed_loop):
 
             ax.set_xlabel("Frequency (Hz)")
             ax.set_ylabel("Phase (deg)")
-            self.gain_margin_text_ref = ax.text(
-                0.01,
-                0.9,
-                stability_margins_text,
-                verticalalignment="top",
-                transform=ax.transAxes,
-            )
+            for row, key in enumerate(margins):
+                self.margin_text_refs[key] = ax.text(
+                    0.01,
+                    0.9 - 0.12 * row,
+                    "",
+                    verticalalignment="top",
+                    transform=ax.transAxes,
+                )
 
         else:
             self.bode_plot_ref[0].set_xdata(f)
@@ -1177,13 +1276,16 @@ def plotBode(self, open_loop, closed_loop):
             self.bode_plot_ref[1].set_xdata(f)
             mag_cl_db = 20 * np.log10(mag_cl)
             self.bode_plot_ref[1].set_ydata(mag_cl_db)
-            self.gain_margin_text_ref.set_text(stability_margins_text)
 
             self.bode_plot_ref[2].set_xdata(f)
             self.bode_plot_ref[2].set_ydata(phase_ol * 180 / np.pi)
             self.bode_plot_ref[3].set_xdata(f)
             self.bode_plot_ref[3].set_ydata(phase_cl * 180 / np.pi)
 
+        for key, (text, color) in margins.items():
+            self.margin_text_refs[key].set_text(text)
+            self.margin_text_refs[key].set_color(color or "black")
+
         self.canvas.draw()
 
     def plotInputOutput(self, redraw=False):