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:
@@ -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 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,
|
||||
)
|
||||
Reference in New Issue
Block a user