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
|
||||
@@ -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
|
||||
@@ -23,6 +23,8 @@
|
||||
#include <esp_system.h>
|
||||
|
||||
#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();
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user