"""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 3–5 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, )