diff --git a/arautopilot/core/autotuner.py b/arautopilot/core/autotuner.py new file mode 100644 index 0000000..63188ec --- /dev/null +++ b/arautopilot/core/autotuner.py @@ -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, + ) diff --git a/arautopilot/studio/wizards/commissioning_wizard.py b/arautopilot/studio/wizards/commissioning_wizard.py new file mode 100644 index 0000000..0083c98 --- /dev/null +++ b/arautopilot/studio/wizards/commissioning_wizard.py @@ -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}" diff --git a/arautopilot/tests/test_autotuner.py b/arautopilot/tests/test_autotuner.py new file mode 100644 index 0000000..bc975e7 --- /dev/null +++ b/arautopilot/tests/test_autotuner.py @@ -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 diff --git a/firmware/ar_autopilot_v1/src/hal/knob_encoder.cpp b/firmware/ar_autopilot_v1/src/hal/knob_encoder.cpp new file mode 100644 index 0000000..94069ce --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/knob_encoder.cpp @@ -0,0 +1,129 @@ +// ============================================================================= +// hal/knob_encoder.cpp -- Rotary encoder for heading knob (Sprint 7) +// ============================================================================= + +#include "knob_encoder.h" + +#include + +#include "../hal/pinout.h" +#include "../modes/standby.h" +#include "../pid/pid_outer_task.h" +#include "../system/ar_log.h" +#include "../system/task_config.h" + +namespace arautopilot::hal { + +namespace { + +constexpr const char* TAG = "AR/KNOB"; + +// Encoder pins (reuse DO6/DO7 pads in commissioning variant). +constexpr uint8_t PIN_A = PIN_DO6_RESERVED; // GPIO 16 +constexpr uint8_t PIN_B = PIN_DO7_RESERVED; // GPIO 17 + +// ----- Quadrature state machine (Gray-code decode) ------------------------- +// +// State transitions: 4 states × 2 new pins → delta +// Table index: (prev_state << 2) | new_ab +// Value: +1 (CW), -1 (CCW), 0 (no move or invalid) +// +static const int8_t QEM[16] = { +// AB=00 01 10 11 + 0, -1, +1, 0, // prev=00 + +1, 0, 0, -1, // prev=01 + -1, 0, 0, +1, // prev=10 + 0, +1, -1, 0, // prev=11 +}; + +volatile int32_t g_pulse_count = 0; +volatile uint8_t g_prev_state = 0; + +portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED; + +// Armed state +volatile bool g_armed = false; +volatile uint32_t g_arm_time_ms = 0; + +void IRAM_ATTR encoder_isr() { + uint8_t a = (uint8_t)digitalRead(PIN_A); + uint8_t b = (uint8_t)digitalRead(PIN_B); + uint8_t state = (a << 1) | b; + int8_t delta = QEM[(g_prev_state << 2) | state]; + if (delta != 0) { + portENTER_CRITICAL_ISR(&g_mux); + g_pulse_count += delta; + portEXIT_CRITICAL_ISR(&g_mux); + } + g_prev_state = state; +} + +void KnobTask(void* /*pv*/) { + AR_LOGI(TAG, "knob_encoder task started on core %d", xPortGetCoreID()); + + int32_t last_count = 0; + + for (;;) { + const uint32_t now = millis(); + + // Auto-disarm after timeout. + portENTER_CRITICAL(&g_mux); + bool armed = g_armed; + if (armed && (now - g_arm_time_ms) > KNOB_ARM_TIMEOUT_MS) { + g_armed = false; + armed = false; + AR_LOGI(TAG, "knob auto-disarmed (timeout)"); + } + portEXIT_CRITICAL(&g_mux); + + portENTER_CRITICAL(&g_mux); + int32_t count = g_pulse_count; + portEXIT_CRITICAL(&g_mux); + + int32_t delta = count - last_count; + last_count = count; + + if (delta != 0 && armed && !modes::is_standby()) { + // Apply ±KNOB_STEP_DEG per pulse, normalised to 0..360. + float sp = pid::pid_outer_heading_setpoint_deg(); + sp = fmodf(sp + (float)delta * KNOB_STEP_DEG + 360.0f, 360.0f); + pid::pid_outer_set_heading_setpoint_deg(sp); + AR_LOGD(TAG, "knob: delta=%d new_sp=%.1f", delta, sp); + } + + vTaskDelay(pdMS_TO_TICKS(20)); // 50 Hz check rate + } +} + +} // namespace + +void knob_encoder_init() { + pinMode(PIN_A, INPUT_PULLUP); + pinMode(PIN_B, INPUT_PULLUP); + g_prev_state = (uint8_t)((digitalRead(PIN_A) << 1) | digitalRead(PIN_B)); + attachInterrupt(digitalPinToInterrupt(PIN_A), encoder_isr, CHANGE); + attachInterrupt(digitalPinToInterrupt(PIN_B), encoder_isr, CHANGE); + AR_LOGI(TAG, "knob encoder on GPIO A=%d B=%d", PIN_A, PIN_B); +} + +void knob_encoder_start_task() { + xTaskCreatePinnedToCore(KnobTask, "knob_enc", + 2048, nullptr, + 1, // low priority + nullptr, + AR_TASK_CORE_COMMS); +} + +void knob_encoder_set_armed(bool armed) { + portENTER_CRITICAL(&g_mux); + g_armed = armed; + g_arm_time_ms = millis(); + portEXIT_CRITICAL(&g_mux); + AR_LOGI(TAG, "knob %s", armed ? "ARMED" : "disarmed"); +} + +bool knob_encoder_is_armed() { + return g_armed; +} + +} // namespace arautopilot::hal diff --git a/firmware/ar_autopilot_v1/src/hal/knob_encoder.h b/firmware/ar_autopilot_v1/src/hal/knob_encoder.h new file mode 100644 index 0000000..7cf99f7 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/knob_encoder.h @@ -0,0 +1,49 @@ +// ============================================================================= +// hal/knob_encoder.h -- Rotary encoder for heading knob (Sprint 7) +// ============================================================================= +// +// A quadrature rotary encoder connected to GPIO pins drives heading setpoint +// changes. Clockwise rotation increases the setpoint by STEP_DEG per detent; +// counter-clockwise decreases it. +// +// The encoder is only active when the pilot is engaged (any mode other than +// STANDBY). While disengaged, pulses are silently discarded. +// +// The "knob arm" coil (COIL_CMD_KNOB_ARM via Modbus, addr 3) enables the +// knob for one KNOB_ARM_TIMEOUT_MS window. If no edge arrives within that +// window, the coil auto-clears. This prevents unintended heading changes +// from vibration noise. +// +// GPIO assignment (not on AR-NMEA-IO v1.0 main header -- uses DO6/DO7 pads +// repurposed as inputs in the commissioning firmware variant): +// PIN_KNOB_A = PIN_DO6_RESERVED (GPIO 16) +// PIN_KNOB_B = PIN_DO7_RESERVED (GPIO 17) +// +// The interrupt handler uses a 4-state Gray-code decode table for noise +// immunity. +// ============================================================================= + +#pragma once + +#include + +namespace arautopilot::hal { + +constexpr float KNOB_STEP_DEG = 1.0f; ///< heading change per detent +constexpr uint32_t KNOB_ARM_TIMEOUT_MS = 5000; ///< arm window before auto-disarm + +/// Initialise encoder GPIO and attach interrupts. +/// Must be called from setup() after do_init(). +void knob_encoder_init(); + +/// Start the knob processing task (low priority, Core 0). +void knob_encoder_start_task(); + +/// Set the armed state. While armed the knob updates the heading setpoint. +/// The coil write handler calls this. +void knob_encoder_set_armed(bool armed); + +/// Return current armed state. +bool knob_encoder_is_armed(); + +} // namespace arautopilot::hal diff --git a/firmware/ar_autopilot_v1/src/hal/nvs_config.cpp b/firmware/ar_autopilot_v1/src/hal/nvs_config.cpp new file mode 100644 index 0000000..c1629c3 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/nvs_config.cpp @@ -0,0 +1,121 @@ +// ============================================================================= +// hal/nvs_config.cpp -- NVS-backed calibration store (Sprint 7) +// ============================================================================= + +#include "nvs_config.h" + +#include +#include +#include + +#include "../system/ar_log.h" + +namespace arautopilot::hal { + +namespace { +constexpr const char* TAG = "AR/NVS"; +constexpr const char* NS = "ar_cal"; +} // namespace + +bool nvs_config_init() { + esp_err_t err = nvs_flash_init(); + if (err == ESP_ERR_NVS_NO_FREE_PAGES || + err == ESP_ERR_NVS_NEW_VERSION_FOUND) { + AR_LOGW(TAG, "NVS partition needs erase (err=0x%x) -- factory-resetting", err); + nvs_flash_erase(); + err = nvs_flash_init(); + } + if (err != ESP_OK) { + AR_LOGE(TAG, "nvs_flash_init failed: 0x%x", err); + return false; + } + AR_LOGI(TAG, "NVS initialised (namespace: %s)", NS); + return true; +} + +CalibrationData nvs_config_load() { + CalibrationData cal; // defaults in struct definition + + nvs_handle_t h; + if (nvs_open(NS, NVS_READONLY, &h) != ESP_OK) { + AR_LOGI(TAG, "No calibration in NVS -- using factory defaults"); + return cal; + } + + // Helper lambdas: read or keep default on missing key. + auto get_i16 = [&](const char* key, int16_t& out) { + int16_t v; + if (nvs_get_i16(h, key, &v) == ESP_OK) out = v; + }; + auto get_float = [&](const char* key, float& out) { + // NVS has no native float; store as uint32 via memcpy. + uint32_t raw; + if (nvs_get_u32(h, key, &raw) == ESP_OK) { + memcpy(&out, &raw, sizeof(float)); + } + }; + auto get_u8 = [&](const char* key, uint8_t& out) { + uint8_t v; + if (nvs_get_u8(h, key, &v) == ESP_OK) out = v; + }; + + get_i16("adc_port", cal.adc_port); + get_i16("adc_stbd", cal.adc_stbd); + get_float("rud_port", cal.rudder_port_deg); + get_float("rud_stbd", cal.rudder_stbd_deg); + get_float("ou_kp", cal.outer_kp); + get_float("ou_ki", cal.outer_ki); + get_float("ou_kd", cal.outer_kd); + uint8_t comm = 0; + get_u8("commissioned", comm); + cal.commissioned = (comm == 1); + + nvs_close(h); + AR_LOGI(TAG, "Calibration loaded (commissioned=%d adc=%d..%d)", + (int)cal.commissioned, cal.adc_port, cal.adc_stbd); + return cal; +} + +bool nvs_config_save(const CalibrationData& cal) { + nvs_handle_t h; + if (nvs_open(NS, NVS_READWRITE, &h) != ESP_OK) { + AR_LOGE(TAG, "nvs_open for write failed"); + return false; + } + + auto put_float = [&](const char* key, float v) { + uint32_t raw; + memcpy(&raw, &v, sizeof(float)); + nvs_set_u32(h, key, raw); + }; + + nvs_set_i16(h, "adc_port", cal.adc_port); + nvs_set_i16(h, "adc_stbd", cal.adc_stbd); + put_float("rud_port", cal.rudder_port_deg); + put_float("rud_stbd", cal.rudder_stbd_deg); + put_float("ou_kp", cal.outer_kp); + put_float("ou_ki", cal.outer_ki); + put_float("ou_kd", cal.outer_kd); + nvs_set_u8(h, "commissioned", cal.commissioned ? 1u : 0u); + + esp_err_t err = nvs_commit(h); + nvs_close(h); + if (err != ESP_OK) { + AR_LOGE(TAG, "nvs_commit failed: 0x%x", err); + return false; + } + AR_LOGI(TAG, "Calibration saved to NVS"); + return true; +} + +bool nvs_config_erase() { + nvs_handle_t h; + if (nvs_open(NS, NVS_READWRITE, &h) != ESP_OK) return false; + esp_err_t err = nvs_erase_all(h); + nvs_commit(h); + nvs_close(h); + AR_LOGW(TAG, "NVS namespace '%s' erased (factory reset)", NS); + return err == ESP_OK; +} + +} // namespace arautopilot::hal diff --git a/firmware/ar_autopilot_v1/src/hal/nvs_config.h b/firmware/ar_autopilot_v1/src/hal/nvs_config.h new file mode 100644 index 0000000..628a5a6 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/nvs_config.h @@ -0,0 +1,54 @@ +// ============================================================================= +// hal/nvs_config.h -- NVS-backed calibration store (Sprint 7) +// ============================================================================= +// +// Persists commissioning data to the ESP32 NVS (Non-Volatile Storage) under +// the namespace "ar_cal". +// +// Keys written: +// "adc_port" -- int16_t raw ADC at full-port rudder limit +// "adc_stbd" -- int16_t raw ADC at full-stbd rudder limit +// "rud_port" -- float physical angle at port limit (deg, default -35) +// "rud_stbd" -- float physical angle at stbd limit (deg, default +35) +// "ou_kp" -- float outer-loop recommended Kp from commissioning +// "ou_ki" -- float outer-loop recommended Ki +// "ou_kd" -- float outer-loop recommended Kd +// "commissioned"-- uint8_t 1 if the unit has been commissioned +// +// All reads/writes are synchronous and should only be called at startup or +// from a low-priority task (not the 50 Hz safety task). +// ============================================================================= + +#pragma once + +#include + +namespace arautopilot::hal { + +struct CalibrationData { + int16_t adc_port = 0; ///< raw ADC at port limit + int16_t adc_stbd = 4095; ///< raw ADC at stbd limit + float rudder_port_deg = -35.0f; ///< physical port angle + float rudder_stbd_deg = 35.0f; ///< physical stbd angle + float outer_kp = 1.0f; ///< recommended outer Kp + float outer_ki = 0.0f; ///< recommended outer Ki + float outer_kd = 0.0f; ///< recommended outer Kd + bool commissioned = false; ///< true once commissioning ran +}; + +/// Initialise the NVS namespace. Must be called from setup() before any +/// other nvs_config function. Returns false if NVS flash is unrecoverable +/// (factory reset required). +bool nvs_config_init(); + +/// Read calibration data from NVS. Returns default values if the key is +/// absent (first boot or factory-reset). +CalibrationData nvs_config_load(); + +/// Write calibration data to NVS. +bool nvs_config_save(const CalibrationData& cal); + +/// Erase the entire "ar_cal" namespace (factory reset). +bool nvs_config_erase(); + +} // namespace arautopilot::hal diff --git a/firmware/ar_autopilot_v1/src/main.cpp b/firmware/ar_autopilot_v1/src/main.cpp index 412948e..432dd74 100644 --- a/firmware/ar_autopilot_v1/src/main.cpp +++ b/firmware/ar_autopilot_v1/src/main.cpp @@ -23,6 +23,8 @@ #include #include "hal/di_do.h" +#include "hal/knob_encoder.h" +#include "hal/nvs_config.h" #include "hal/pinout.h" #include "hal/rudder_actuator.h" #include "hal/rudder_sensor.h" @@ -66,11 +68,27 @@ void setup() { // Initialise the pilot mode state machine (boots in STANDBY). arautopilot::modes::mode_init(); + // NVS (non-volatile config + calibration). Must be first HAL call. + arautopilot::hal::nvs_config_init(); + const auto cal = arautopilot::hal::nvs_config_load(); + if (cal.commissioned) { + AR_LOGI(TAG, "commissioned unit: adc %d..%d outer Kp=%.3f", + cal.adc_port, cal.adc_stbd, cal.outer_kp); + } else { + AR_LOGW(TAG, "not commissioned -- using factory defaults"); + } + // Initialise hardware abstraction layer. arautopilot::hal::di_init(); arautopilot::hal::do_init(); arautopilot::hal::rudder_sensor_init(); + if (cal.commissioned) { + arautopilot::hal::rudder_sensor_set_calibration( + cal.adc_port, cal.adc_stbd, + cal.rudder_port_deg, cal.rudder_stbd_deg); + } arautopilot::hal::rudder_actuator_init(); + arautopilot::hal::knob_encoder_init(); // Safety: watchdog + DI override task. Initialised first so the // watchdog is armed before slower subsystems come up. @@ -86,15 +104,21 @@ void setup() { arautopilot::safety::safety_monitor_start_task(); arautopilot::hal::rudder_sensor_start_task(); + arautopilot::hal::knob_encoder_start_task(); // PID inner loop (Sprint 2). Active only when not in STANDBY and // rudder sensor reading is valid; refuses internally otherwise. arautopilot::pid::pid_inner_task_init(); arautopilot::pid::pid_inner_task_start(); - // PID outer loop (Sprint 3) -- Heading Hold. Only writes to the inner - // loop's setpoint when current mode == HEADING_HOLD. + // PID outer loop (Sprint 3) -- Heading Hold. arautopilot::pid::pid_outer_task_init(); + if (cal.commissioned && cal.outer_kp > 0.0f) { + arautopilot::pid::pid_outer_update_gains( + cal.outer_kp, cal.outer_ki, cal.outer_kd); + AR_LOGI(TAG, "applied commissioned outer gains Kp=%.3f Ki=%.3f Kd=%.3f", + cal.outer_kp, cal.outer_ki, cal.outer_kd); + } arautopilot::pid::pid_outer_task_start(); ar_start_heartbeat_task(); diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp index 0ca7304..70038a6 100644 --- a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp +++ b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp @@ -33,6 +33,7 @@ #include "../system/task_config.h" #include "modbus_registers.h" #include "nmea2000_consumer.h" +#include "../hal/knob_encoder.h" #include "../safety/safety_monitor.h" namespace arautopilot::protocols::modbus { @@ -461,6 +462,7 @@ Modbus::Error write_coil(uint16_t addr, bool value) { return Modbus::Error::SUCCESS; case COIL_CMD_KNOB_ARM: g_coils.knob_arm = value; + hal::knob_encoder_set_armed(value); return Modbus::Error::SUCCESS; default: return Modbus::Error::ILLEGAL_DATA_ADDRESS;