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,
|
||||
)
|
||||
@@ -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}"
|
||||
@@ -0,0 +1,229 @@
|
||||
"""Tests for the relay auto-tuner and commissioning wizard -- Sprint 7."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import pytest
|
||||
|
||||
from arautopilot.core.autotuner import RelayAutoTuner, TunerResult
|
||||
from arautopilot.core.pid_config import PidGains
|
||||
from arautopilot.studio.wizards.commissioning_wizard import (
|
||||
CommissioningWizard,
|
||||
WizardPhase,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Simple first-order vessel model: heading error driven by relay rudder output.
|
||||
#
|
||||
# dh/dt = Kv * rudder (Kv = vessel heading rate gain, deg/(s·deg_rud))
|
||||
#
|
||||
# At 10 Hz: err_{k+1} = err_k - Kv * relay_k * dt
|
||||
# This produces sinusoidal heading oscillation (limit cycle) under relay control.
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class FirstOrderVessel:
|
||||
"""Minimal heading process for relay-tuner tests."""
|
||||
|
||||
def __init__(self, kv: float = 0.3, dt: float = 0.1) -> None:
|
||||
self.heading = 0.0
|
||||
self.setpoint = 0.0
|
||||
self.kv = kv
|
||||
self.dt = dt
|
||||
|
||||
def step(self, rudder_cmd: float) -> float:
|
||||
self.heading += self.kv * rudder_cmd * self.dt
|
||||
return self.setpoint - self.heading # error
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# RelayAutoTuner tests
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestRelayAutoTuner:
|
||||
def test_invalid_amplitude_raises(self):
|
||||
with pytest.raises(ValueError, match="relay_amplitude"):
|
||||
RelayAutoTuner(relay_amplitude=0.0)
|
||||
|
||||
def test_invalid_min_cycles_raises(self):
|
||||
with pytest.raises(ValueError, match="min_cycles"):
|
||||
RelayAutoTuner(min_cycles=0)
|
||||
|
||||
def test_not_done_before_experiment(self):
|
||||
tuner = RelayAutoTuner()
|
||||
assert not tuner.is_done
|
||||
|
||||
def test_result_not_converged_before_start(self):
|
||||
tuner = RelayAutoTuner()
|
||||
r = tuner.result()
|
||||
assert not r.converged
|
||||
|
||||
def test_converges_on_first_order_plant(self):
|
||||
vessel = FirstOrderVessel(kv=0.4, dt=0.1)
|
||||
tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=3, dt_s=0.1)
|
||||
for _ in range(2000):
|
||||
error = vessel.step(tuner.step(vessel.setpoint - vessel.heading))
|
||||
if tuner.is_done:
|
||||
break
|
||||
assert tuner.is_done
|
||||
r = tuner.result()
|
||||
assert r.converged
|
||||
assert r.cycles_detected >= 3
|
||||
assert r.ku > 0
|
||||
assert r.tu_s > 0
|
||||
|
||||
def test_ku_reasonable_range(self):
|
||||
"""Ku must be positive and finite; integrating plant gives high Ku (normal)."""
|
||||
vessel = FirstOrderVessel(kv=0.4, dt=0.1)
|
||||
tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=3, dt_s=0.1)
|
||||
for _ in range(2000):
|
||||
err = vessel.step(tuner.step(vessel.setpoint - vessel.heading))
|
||||
if tuner.is_done:
|
||||
break
|
||||
r = tuner.result()
|
||||
assert r.ku > 0
|
||||
import math as _math
|
||||
assert _math.isfinite(r.ku)
|
||||
|
||||
def test_to_pid_gains_zn_formulas(self):
|
||||
"""ZN formulas: Kp=0.6Ku, Ki=1.2Ku/Tu, Kd=0.075Ku*Tu."""
|
||||
result = TunerResult(converged=True, ku=2.0, tu_s=4.0)
|
||||
gains = result.to_pid_gains()
|
||||
assert abs(gains.kp - 0.6 * 2.0) < 1e-3
|
||||
assert abs(gains.ki - 1.2 * 2.0 / 4.0) < 1e-3
|
||||
assert abs(gains.kd - 0.075 * 2.0 * 4.0) < 1e-3
|
||||
|
||||
def test_to_pid_gains_raises_if_not_converged(self):
|
||||
r = TunerResult(converged=False)
|
||||
with pytest.raises(RuntimeError):
|
||||
r.to_pid_gains()
|
||||
|
||||
def test_output_toggles_at_zero_crossing(self):
|
||||
"""After one half-cycle the relay output should flip sign."""
|
||||
tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=1, dt_s=0.1)
|
||||
# Drive error positive for several steps.
|
||||
outputs = []
|
||||
for i in range(5):
|
||||
outputs.append(tuner.step(1.0))
|
||||
# Force sign flip.
|
||||
for i in range(3):
|
||||
outputs.append(tuner.step(-1.0))
|
||||
# After the sign change the relay should have flipped.
|
||||
assert outputs[-1] < 0
|
||||
|
||||
def test_max_steps_safety_cutoff(self):
|
||||
tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=100, max_steps=50)
|
||||
for _ in range(60):
|
||||
tuner.step(1.0)
|
||||
assert tuner.is_done
|
||||
|
||||
def test_returns_zero_after_done(self):
|
||||
tuner = RelayAutoTuner(relay_amplitude=5.0, min_cycles=100, max_steps=5)
|
||||
for _ in range(10):
|
||||
out = tuner.step(1.0)
|
||||
assert out == 0.0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# CommissioningWizard tests
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class StubTransport:
|
||||
"""Simple stub that records commands and returns configurable ADC/error values."""
|
||||
|
||||
def __init__(self, adc_port: int = 100, adc_stbd: int = 3900) -> None:
|
||||
self.calls: list[tuple[str, float]] = []
|
||||
self._adc_port = adc_port
|
||||
self._adc_stbd = adc_stbd
|
||||
self._at_port = True # track which limit we're simulating
|
||||
self.heading = 0.0
|
||||
self.setpoint = 0.0
|
||||
|
||||
def __call__(self, cmd: str, value: float) -> float:
|
||||
self.calls.append((cmd, value))
|
||||
if cmd == "move_port":
|
||||
self._at_port = True
|
||||
return 0.0
|
||||
if cmd == "move_stbd":
|
||||
self._at_port = False
|
||||
return 0.0
|
||||
if cmd == "stop":
|
||||
return 0.0
|
||||
if cmd == "read_adc":
|
||||
return float(self._adc_port if self._at_port else self._adc_stbd)
|
||||
if cmd == "read_error":
|
||||
return self.setpoint - self.heading
|
||||
if cmd == "set_rudder":
|
||||
# Simple first-order response: heading moves 0.3 deg per deg-rudder per 0.1s
|
||||
self.heading += 0.3 * value * 0.1
|
||||
return 0.0
|
||||
return 0.0
|
||||
|
||||
|
||||
class TestCommissioningWizard:
|
||||
def test_starts_in_rudder_limits_phase(self):
|
||||
tx = StubTransport()
|
||||
wiz = CommissioningWizard(transport=tx)
|
||||
assert wiz.phase is WizardPhase.RUDDER_LIMITS
|
||||
assert not wiz.done
|
||||
|
||||
def test_abort_stops_wizard(self):
|
||||
tx = StubTransport()
|
||||
wiz = CommissioningWizard(transport=tx)
|
||||
wiz.abort("test abort")
|
||||
assert wiz.phase is WizardPhase.ABORTED
|
||||
assert wiz.done
|
||||
assert wiz.result.aborted_reason == "test abort"
|
||||
|
||||
def test_full_run_completes(self):
|
||||
tx = StubTransport()
|
||||
wiz = CommissioningWizard(
|
||||
transport=tx,
|
||||
relay_amplitude=5.0,
|
||||
min_tune_cycles=3,
|
||||
limit_settle_s=0.5, # short settle for test speed
|
||||
dt_s=0.1,
|
||||
)
|
||||
# Run until done (up to 15000 steps = 25 min at 10 Hz)
|
||||
for _ in range(15000):
|
||||
wiz.step()
|
||||
if wiz.done:
|
||||
break
|
||||
assert wiz.phase is WizardPhase.DONE
|
||||
assert wiz.result.completed
|
||||
assert wiz.result.recommended_outer_gains is not None
|
||||
gains = wiz.result.recommended_outer_gains
|
||||
assert isinstance(gains, PidGains)
|
||||
assert gains.kp > 0
|
||||
|
||||
def test_adc_limits_recorded(self):
|
||||
tx = StubTransport(adc_port=200, adc_stbd=3800)
|
||||
wiz = CommissioningWizard(transport=tx, limit_settle_s=0.3, dt_s=0.1)
|
||||
for _ in range(15000):
|
||||
wiz.step()
|
||||
if wiz.done:
|
||||
break
|
||||
r = wiz.result
|
||||
# After possible swap, lo < hi
|
||||
assert r.adc_port_limit < r.adc_stbd_limit
|
||||
|
||||
def test_identical_adc_limits_aborts(self):
|
||||
tx = StubTransport(adc_port=2048, adc_stbd=2048)
|
||||
wiz = CommissioningWizard(transport=tx, limit_settle_s=0.3, dt_s=0.1)
|
||||
for _ in range(200):
|
||||
wiz.step()
|
||||
if wiz.done:
|
||||
break
|
||||
assert wiz.phase is WizardPhase.ABORTED
|
||||
assert "identical" in wiz.result.aborted_reason.lower()
|
||||
|
||||
def test_stop_called_on_completion(self):
|
||||
tx = StubTransport()
|
||||
wiz = CommissioningWizard(transport=tx, limit_settle_s=0.3, dt_s=0.1)
|
||||
for _ in range(15000):
|
||||
wiz.step()
|
||||
if wiz.done:
|
||||
break
|
||||
cmds = [c[0] for c in tx.calls]
|
||||
# 'stop' must have been called at least once (after limits and at tune end)
|
||||
assert "stop" in cmds
|
||||
Reference in New Issue
Block a user