sprint-7: Commissioning wizard + relay ZN auto-tuner + NVS config + knob encoder

Python: - autotuner.py: RelayAutoTuner (Astrom-Hagglund relay method) → TunerResult with Ku/Tu; TunerResult.to_pid_gains() applies ZN formulas - commissioning_wizard.py: 4-phase state machine (RUDDER_LIMITS → SENSOR_CAL → AUTO_TUNE → DONE); transport-injected for testability; abort on invalid cal - test_autotuner.py: 17 tests covering relay convergence, ZN formulas, wizard full-run, abort, ADC swap, identical-ADC guard Firmware: - nvs_config.h/cpp: NVS-backed CalibrationData store (adc limits, rudder angles, outer Kp/Ki/Kd, commissioned flag); float stored as uint32 via memcpy - knob_encoder.h/cpp: quadrature rotary encoder on GPIO 16/17 with ISR Gray-code decode; knob_arm coil arms for 5 s window; updates heading setpoint ±1 deg/detent - modbus_slave.cpp: COIL_CMD_KNOB_ARM now calls knob_encoder_set_armed() - main.cpp: nvs_config_init/load at boot; apply commissioned calibration to rudder sensor and outer loop gains; start knob encoder task Tests: 326 passed | Flash: 28.5% Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-20 02:55:26 -04:00
parent e82dbc449c
commit 5f9b445572
9 changed files with 1068 additions and 2 deletions
@@ -0,0 +1,259 @@
+"""Commissioning wizard state machine -- Sprint 7.
+
+A pure Python state machine (no UI dependencies) that guides an operator
+through the four commissioning steps:
+
+    1. RUDDER_LIMITS   -- drive rudder full port/stbd, record ADC limits.
+    2. SENSOR_CAL      -- compute linear calibration from ADC limits.
+    3. AUTO_TUNE       -- relay-feedback ZN auto-tune at the configured speed.
+    4. DONE            -- results ready to be written to NVS and ProjectConfig.
+
+The wizard produces a :class:`CommissioningResult` containing calibration
+values and recommended PID gains.  The Studio main window can drive it by
+calling :meth:`CommissioningWizard.step` on each 10 Hz control tick.
+
+Design intent
+-------------
+All hardware interaction is behind a ``transport`` callable (inject the real
+Modbus client or a test stub).  This keeps the wizard fully unit-testable
+without hardware.
+
+Usage::
+
+    def send(cmd: str, value: float) -> float:
+        # cmd is one of: 'move_port', 'move_stbd', 'stop',
+        #                 'read_adc', 'read_error', 'set_rudder'
+        ...
+
+    wiz = CommissioningWizard(transport=send)
+    while not wiz.done:
+        wiz.step()
+    result = wiz.result
+"""
+
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass, field
+from enum import auto, Enum
+
+from arautopilot.core.autotuner import RelayAutoTuner, TunerResult
+from arautopilot.core.pid_config import PidGains
+
+
+class WizardPhase(Enum):
+    RUDDER_LIMITS = auto()
+    SENSOR_CAL    = auto()
+    AUTO_TUNE     = auto()
+    DONE          = auto()
+    ABORTED       = auto()
+
+
+@dataclass
+class CommissioningResult:
+    """Output produced by a completed commissioning run."""
+
+    # Rudder sensor calibration
+    adc_port_limit: int = 0       # raw ADC at full-port stop
+    adc_stbd_limit: int = 4095    # raw ADC at full-stbd stop
+    rudder_port_deg: float = -35.0
+    rudder_stbd_deg: float = 35.0
+
+    # PID recommendations (from ZN auto-tune)
+    tuner_result: TunerResult | None = None
+    recommended_outer_gains: PidGains | None = None
+
+    # Metadata
+    completed: bool = False
+    aborted_reason: str = ""
+
+
+# ---------------------------------------------------------------------------
+# Transport protocol
+# ---------------------------------------------------------------------------
+# The transport callable is called with a command string and an optional
+# float argument; it must return a float (the "response" value, 0.0 if N/A).
+# Commands:
+#   "move_port"    (value ignored) → 0.0
+#   "move_stbd"    (value ignored) → 0.0
+#   "stop"         (value ignored) → 0.0
+#   "read_adc"     (value ignored) → current raw ADC (0..4095)
+#   "read_error"   (value ignored) → current heading error (deg)
+#   "set_rudder"   (value = deg)   → 0.0  (apply relay output to inner loop)
+
+TransportFn = "Callable[[str, float], float]"
+
+
+class CommissioningWizard:
+    """Drives the commissioning sequence at 10 Hz (call ``step()`` each tick).
+
+    Parameters
+    ----------
+    transport:
+        Callable conforming to the protocol above.
+    relay_amplitude:
+        Relay output magnitude for auto-tune, degrees of rudder.
+    min_tune_cycles:
+        Minimum oscillation cycles before auto-tune declares convergence.
+    limit_settle_s:
+        Seconds to wait at each limit stop before sampling ADC.
+    dt_s:
+        Control step in seconds (default 0.1 for 10 Hz).
+    """
+
+    def __init__(
+        self,
+        transport: "Callable[[str, float], float]",
+        relay_amplitude: float = 5.0,
+        min_tune_cycles: int = 3,
+        limit_settle_s: float = 2.0,
+        dt_s: float = 0.1,
+    ) -> None:
+        from typing import Callable  # noqa: F401
+
+        self._tx = transport
+        self._relay_amp = relay_amplitude
+        self._min_cycles = min_tune_cycles
+        self._settle_s = limit_settle_s
+        self._dt = dt_s
+
+        self._phase: WizardPhase = WizardPhase.RUDDER_LIMITS
+        self._sub: str = "drive_port"   # sub-state within RUDDER_LIMITS
+        self._timer: float = 0.0        # time accumulator for settle waits
+        self._result = CommissioningResult()
+        self._tuner: RelayAutoTuner | None = None
+
+    # ------------------------------------------------------------------
+
+    @property
+    def phase(self) -> WizardPhase:
+        return self._phase
+
+    @property
+    def done(self) -> bool:
+        return self._phase in (WizardPhase.DONE, WizardPhase.ABORTED)
+
+    @property
+    def result(self) -> CommissioningResult:
+        return self._result
+
+    def abort(self, reason: str = "operator abort") -> None:
+        self._tx("stop", 0.0)
+        self._phase = WizardPhase.ABORTED
+        self._result.aborted_reason = reason
+
+    # ------------------------------------------------------------------
+    #  Main tick
+    # ------------------------------------------------------------------
+
+    def step(self) -> str:
+        """Advance the wizard by one 10 Hz tick.
+
+        Returns a human-readable status string (for display / logging).
+        """
+        if self.done:
+            return f"wizard {self._phase.name}"
+
+        if self._phase is WizardPhase.RUDDER_LIMITS:
+            return self._tick_limits()
+        if self._phase is WizardPhase.SENSOR_CAL:
+            return self._tick_cal()
+        if self._phase is WizardPhase.AUTO_TUNE:
+            return self._tick_tune()
+        return "unknown phase"
+
+    # ------------------------------------------------------------------
+    #  Phase: RUDDER_LIMITS
+    # ------------------------------------------------------------------
+
+    def _tick_limits(self) -> str:
+        if self._sub == "drive_port":
+            self._tx("move_port", 0.0)
+            self._timer += self._dt
+            if self._timer >= self._settle_s:
+                self._result.adc_port_limit = int(self._tx("read_adc", 0.0))
+                self._sub = "settle_port"
+                self._timer = 0.0
+            return f"driving to port limit... ({self._timer:.1f}s)"
+
+        if self._sub == "settle_port":
+            self._tx("stop", 0.0)
+            self._timer += self._dt
+            if self._timer >= 0.5:
+                self._sub = "drive_stbd"
+                self._timer = 0.0
+            return "settled at port limit"
+
+        if self._sub == "drive_stbd":
+            self._tx("move_stbd", 0.0)
+            self._timer += self._dt
+            if self._timer >= self._settle_s:
+                self._result.adc_stbd_limit = int(self._tx("read_adc", 0.0))
+                self._sub = "settle_stbd"
+                self._timer = 0.0
+            return f"driving to stbd limit... ({self._timer:.1f}s)"
+
+        if self._sub == "settle_stbd":
+            self._tx("stop", 0.0)
+            self._timer += self._dt
+            if self._timer >= 0.5:
+                self._phase = WizardPhase.SENSOR_CAL
+                self._timer = 0.0
+            return "settled at stbd limit"
+
+        return "limits: unknown sub-state"
+
+    # ------------------------------------------------------------------
+    #  Phase: SENSOR_CAL
+    # ------------------------------------------------------------------
+
+    def _tick_cal(self) -> str:
+        # The calibration is computed from the ADC limits gathered above.
+        # This is instantaneous (no hardware interaction needed).
+        lo = self._result.adc_port_limit
+        hi = self._result.adc_stbd_limit
+        if lo == hi:
+            self.abort("sensor cal: port and stbd ADC values are identical")
+            return "sensor cal FAILED: identical ADC values"
+
+        # Ensure lo < hi (swap if wiring is inverted).
+        if lo > hi:
+            lo, hi = hi, lo
+            self._result.rudder_port_deg, self._result.rudder_stbd_deg = (
+                self._result.rudder_stbd_deg,
+                self._result.rudder_port_deg,
+            )
+
+        self._result.adc_port_limit = lo
+        self._result.adc_stbd_limit = hi
+
+        self._phase = WizardPhase.AUTO_TUNE
+        self._tuner = RelayAutoTuner(
+            relay_amplitude=self._relay_amp,
+            min_cycles=self._min_cycles,
+            dt_s=self._dt,
+        )
+        return "sensor calibration done"
+
+    # ------------------------------------------------------------------
+    #  Phase: AUTO_TUNE
+    # ------------------------------------------------------------------
+
+    def _tick_tune(self) -> str:
+        assert self._tuner is not None
+        error = self._tx("read_error", 0.0)
+        rudder_cmd = self._tuner.step(error)
+        self._tx("set_rudder", rudder_cmd)
+
+        if self._tuner.is_done:
+            res = self._tuner.result()
+            self._result.tuner_result = res
+            if res.converged:
+                self._result.recommended_outer_gains = res.to_pid_gains()
+            self._tx("stop", 0.0)
+            self._result.completed = res.converged
+            self._phase = WizardPhase.DONE
+            return f"auto-tune done: Ku={res.ku:.3f} Tu={res.tu_s:.2f}s"
+
+        steps = self._tuner._step_count  # noqa: SLF001
+        return f"auto-tuning... step={steps} cycles={len(self._tuner._half_period_steps)//2}"