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,
)
@@ -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}"
+229
View File
@@ -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
@@ -0,0 +1,129 @@
// =============================================================================
// hal/knob_encoder.cpp -- Rotary encoder for heading knob (Sprint 7)
// =============================================================================
#include "knob_encoder.h"
#include <Arduino.h>
#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
@@ -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 <cstdint>
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
@@ -0,0 +1,121 @@
// =============================================================================
// hal/nvs_config.cpp -- NVS-backed calibration store (Sprint 7)
// =============================================================================
#include "nvs_config.h"
#include <nvs.h>
#include <nvs_flash.h>
#include <Arduino.h>
#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
@@ -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 <cstdint>
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
+26 -2
View File
@@ -23,6 +23,8 @@
#include <esp_system.h> #include <esp_system.h>
#include "hal/di_do.h" #include "hal/di_do.h"
#include "hal/knob_encoder.h"
#include "hal/nvs_config.h"
#include "hal/pinout.h" #include "hal/pinout.h"
#include "hal/rudder_actuator.h" #include "hal/rudder_actuator.h"
#include "hal/rudder_sensor.h" #include "hal/rudder_sensor.h"
@@ -66,11 +68,27 @@ void setup() {
// Initialise the pilot mode state machine (boots in STANDBY). // Initialise the pilot mode state machine (boots in STANDBY).
arautopilot::modes::mode_init(); 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. // Initialise hardware abstraction layer.
arautopilot::hal::di_init(); arautopilot::hal::di_init();
arautopilot::hal::do_init(); arautopilot::hal::do_init();
arautopilot::hal::rudder_sensor_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::rudder_actuator_init();
arautopilot::hal::knob_encoder_init();
// Safety: watchdog + DI override task. Initialised first so the // Safety: watchdog + DI override task. Initialised first so the
// watchdog is armed before slower subsystems come up. // watchdog is armed before slower subsystems come up.
@@ -86,15 +104,21 @@ void setup() {
arautopilot::safety::safety_monitor_start_task(); arautopilot::safety::safety_monitor_start_task();
arautopilot::hal::rudder_sensor_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 // PID inner loop (Sprint 2). Active only when not in STANDBY and
// rudder sensor reading is valid; refuses internally otherwise. // rudder sensor reading is valid; refuses internally otherwise.
arautopilot::pid::pid_inner_task_init(); arautopilot::pid::pid_inner_task_init();
arautopilot::pid::pid_inner_task_start(); arautopilot::pid::pid_inner_task_start();
// PID outer loop (Sprint 3) -- Heading Hold. Only writes to the inner // PID outer loop (Sprint 3) -- Heading Hold.
// loop's setpoint when current mode == HEADING_HOLD.
arautopilot::pid::pid_outer_task_init(); 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(); arautopilot::pid::pid_outer_task_start();
ar_start_heartbeat_task(); ar_start_heartbeat_task();
@@ -33,6 +33,7 @@
#include "../system/task_config.h" #include "../system/task_config.h"
#include "modbus_registers.h" #include "modbus_registers.h"
#include "nmea2000_consumer.h" #include "nmea2000_consumer.h"
#include "../hal/knob_encoder.h"
#include "../safety/safety_monitor.h" #include "../safety/safety_monitor.h"
namespace arautopilot::protocols::modbus { namespace arautopilot::protocols::modbus {
@@ -461,6 +462,7 @@ Modbus::Error write_coil(uint16_t addr, bool value) {
return Modbus::Error::SUCCESS; return Modbus::Error::SUCCESS;
case COIL_CMD_KNOB_ARM: case COIL_CMD_KNOB_ARM:
g_coils.knob_arm = value; g_coils.knob_arm = value;
hal::knob_encoder_set_armed(value);
return Modbus::Error::SUCCESS; return Modbus::Error::SUCCESS;
default: default:
return Modbus::Error::ILLEGAL_DATA_ADDRESS; return Modbus::Error::ILLEGAL_DATA_ADDRESS;