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>
This commit is contained in:
2026-05-20 02:55:26 -04:00
parent e82dbc449c
commit 5f9b445572
9 changed files with 1068 additions and 2 deletions
+199
View File
@@ -0,0 +1,199 @@
"""Relay-feedback Ziegler-Nichols auto-tuner -- Sprint 7.
Implements the closed-loop relay auto-tuning method (Åström & Hägglund 1984)
to identify the ultimate gain ``Ku`` and ultimate period ``Tu`` from a single
relay experiment, then converts them to PID gains using the ZN formulas.
The relay oscillation amplitude ``d`` is chosen so the actuator stays within
a safe excitation band during commissioning (typically ±5 % of full-scale
rudder output).
Usage::
tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=3)
# Feed the error signal at the control rate (10 Hz outer loop).
for step in simulation:
output = tuner.step(error_deg)
result = tuner.result()
if result.converged:
gains = result.to_pid_gains()
Theory
------
The relay switches its output between ``+d`` and ``-d`` based on the sign of
the process error. The resulting limit cycle has amplitude ``a`` (peak
deviation of the output) and period ``Tu``. The ultimate gain is:
Ku = 4d / (π a)
ZN formulas for PID (tuned for setpoint tracking):
Kp = 0.6 · Ku
Ki = 1.2 · Ku / Tu (= 2 · Kp / Tu)
Kd = 0.075 · Ku · Tu (= Kp · Tu / 8)
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
from arautopilot.core.pid_config import PidGains
@dataclass
class TunerResult:
"""Outcome of one relay experiment."""
converged: bool = False
ku: float = 0.0 # ultimate gain
tu_s: float = 0.0 # ultimate period, seconds
cycles_detected: int = 0 # full half-cycles ÷ 2
# Raw measurements
amplitude: float = 0.0 # peak-to-peak process amplitude / 2 (deg)
relay_amplitude: float = 0.0 # relay output magnitude used
def to_pid_gains(self) -> PidGains:
"""Convert Ku / Tu to ZN-formula PID gains."""
if not self.converged:
raise RuntimeError("Tuner has not converged yet")
kp = 0.6 * self.ku
ki = 1.2 * self.ku / self.tu_s if self.tu_s > 0 else 0.0
kd = 0.075 * self.ku * self.tu_s
return PidGains(kp=round(kp, 4), ki=round(ki, 4), kd=round(kd, 4))
class RelayAutoTuner:
"""Closed-loop relay auto-tuner (10 Hz outer-loop step clock).
Parameters
----------
relay_amplitude:
Magnitude of the relay output in degrees of rudder.
min_cycles:
Minimum number of complete oscillation cycles before declaring
convergence. Typically 35 for a reliable estimate.
dt_s:
Time step in seconds (default 0.1 for 10 Hz outer loop).
max_steps:
Safety cut-off: abort if the experiment runs longer than this many
control steps.
"""
def __init__(
self,
relay_amplitude: float = 5.0,
min_cycles: int = 3,
dt_s: float = 0.1,
max_steps: int = 6000, # 10 min at 10 Hz
) -> None:
if relay_amplitude <= 0:
raise ValueError("relay_amplitude must be positive")
if min_cycles < 1:
raise ValueError("min_cycles must be >= 1")
self._d: float = relay_amplitude
self._min_cycles: int = min_cycles
self._dt: float = dt_s
self._max_steps: int = max_steps
# State
self._relay_output: float = self._d # starts positive
self._step_count: int = 0
self._done: bool = False
# Peak tracking for amplitude estimation
self._peak_buffer: list[float] = [] # absolute peak values
self._last_sign: int = 0 # sign of the last error sample
# Half-cycle timing
self._half_cycle_start: int = 0
self._half_period_steps: list[float] = [] # in steps
# Running amplitude samples (one per half-cycle)
self._amp_peaks: list[float] = [] # max abs(error) per half-cycle
self._current_max: float = 0.0
# ------------------------------------------------------------------
def step(self, error: float) -> float:
"""Feed one error sample; return the relay output (degrees of rudder).
Parameters
----------
error:
Heading error in degrees (setpoint measured).
Returns
-------
float
Rudder command in degrees to apply for this step.
"""
if self._done:
return 0.0
self._step_count += 1
self._current_max = max(self._current_max, abs(error))
sign = 1 if error >= 0 else -1
# Detect zero-crossing (sign change) → half-cycle boundary.
if self._last_sign != 0 and sign != self._last_sign:
half_len = self._step_count - self._half_cycle_start
self._half_period_steps.append(half_len)
self._amp_peaks.append(self._current_max)
self._current_max = 0.0
self._half_cycle_start = self._step_count
# Toggle relay on sign change.
self._relay_output = self._d if sign > 0 else -self._d
# Check convergence: need 2*min_cycles half-cycles for min_cycles
# full cycles.
if len(self._half_period_steps) >= 2 * self._min_cycles:
self._done = True
self._last_sign = sign
# Safety cut-off
if self._step_count >= self._max_steps:
self._done = True
return self._relay_output
@property
def is_done(self) -> bool:
return self._done
def result(self) -> TunerResult:
"""Return the tuning result. Call only after ``is_done`` is True."""
n_half = len(self._half_period_steps)
if n_half < 2:
return TunerResult(converged=False, relay_amplitude=self._d)
# Average the last ``2*min_cycles`` half-period measurements
# (discard the first half-cycle which is often distorted).
use = self._half_period_steps[-2 * self._min_cycles:]
tu_s = (sum(use) / len(use)) * 2 * self._dt # full period
# Amplitude: average peak-to-peak / 2 of last min_cycles cycles
amp_use = self._amp_peaks[-2 * self._min_cycles:]
amplitude = sum(amp_use) / len(amp_use)
if amplitude <= 0:
return TunerResult(converged=False, relay_amplitude=self._d)
# Åström-Hägglund: Ku = 4d / (π a)
ku = 4.0 * self._d / (math.pi * amplitude)
return TunerResult(
converged=True,
ku=round(ku, 4),
tu_s=round(tu_s, 3),
cycles_detected=n_half // 2,
amplitude=round(amplitude, 3),
relay_amplitude=self._d,
)