sprint-9: integration tests + hardening + operator manual
Integration tests (64 new tests, 462 total): - test_integration_cascade: full cascade closed-loop simulation -- outer PID → inner PID → rudder dynamics → vessel heading; verifies convergence across small/90°/180° turns, wrap-around, and low speed - test_integration_ekf_pid: EKF-smoothed heading feeding outer PID; confirms EKF reduces rudder total-variation vs raw noisy heading - test_integration_alarm_audit: alarm engine → audit log hash-chain; verify, tamper detection, cross-session reload, multi-alarm logging - test_modbus_utils: 38 tests for scale/raw conversion, bounds checking, heading/rudder helpers, signed int16 two's-complement round-trip Hardening: - heading_ekf: guard NaN/inf in update_heading() and update_rot() -- skip bad measurements silently rather than corrupting filter state - adaptive_tuner: guard NaN/inf in step() -- ignore corrupt error samples - modbus_utils.py: new shared module with scale_to_raw, scale_to_raw_signed, raw_signed_to_scaled, clamp_uint16, validate_holding_write, heading_deg_to_raw, rudder_deg_to_raw_signed Documentation: - docs/operator_manual.md: 15-section operator manual covering safety, installation, all operating modes, alarm reference, commissioning, fault-finding, Modbus register summary, and activation/audit log procedure Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
@@ -23,6 +23,7 @@ Usage::
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from arautopilot.core.pid_config import PidConfig, PidGains
|
||||
@@ -75,6 +76,8 @@ class AdaptiveTuner:
|
||||
"""
|
||||
if not self.config.adaptive_enabled:
|
||||
return None
|
||||
if not math.isfinite(error_deg):
|
||||
return None
|
||||
|
||||
self._error_buffer.append(error_deg)
|
||||
if len(self._error_buffer) > self.window_steps:
|
||||
|
||||
@@ -82,6 +82,8 @@ class HeadingEKF:
|
||||
self._P = [new_p00, new_p01, new_p10, new_p11]
|
||||
|
||||
def update_heading(self, measured_deg: float, noise_deg: float = 2.0) -> None:
|
||||
if not math.isfinite(measured_deg) or not math.isfinite(noise_deg):
|
||||
return
|
||||
"""Kalman update step for a heading measurement.
|
||||
|
||||
Parameters
|
||||
@@ -120,6 +122,8 @@ class HeadingEKF:
|
||||
]
|
||||
|
||||
def update_rot(self, measured_rot_dps: float, noise_dps: float = 1.0) -> None:
|
||||
if not math.isfinite(measured_rot_dps) or not math.isfinite(noise_dps):
|
||||
return
|
||||
"""Kalman update step for a rate-of-turn measurement.
|
||||
|
||||
Parameters
|
||||
|
||||
@@ -0,0 +1,105 @@
|
||||
"""Modbus register utility helpers -- Sprint 9 hardening.
|
||||
|
||||
Provides value scaling, bounds checking, and safe uint16 clamping for
|
||||
register writes. Used by the Studio and CLI tools to validate writes
|
||||
before they are sent on the wire.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
|
||||
UINT16_MAX = 0xFFFF
|
||||
INT16_MIN = -32768
|
||||
INT16_MAX = 32767
|
||||
|
||||
|
||||
def scale_to_raw(physical: float, scale: float, offset: float = 0.0) -> int:
|
||||
"""Convert a physical value to a Modbus raw uint16 (unsigned).
|
||||
|
||||
``raw = round((physical - offset) / scale)``
|
||||
|
||||
Raises ``ValueError`` if the result does not fit in uint16.
|
||||
"""
|
||||
if scale == 0.0:
|
||||
raise ValueError("scale must not be 0")
|
||||
if not math.isfinite(physical):
|
||||
raise ValueError(f"physical value must be finite, got {physical!r}")
|
||||
raw = round((physical - offset) / scale)
|
||||
if raw < 0 or raw > UINT16_MAX:
|
||||
raise ValueError(
|
||||
f"scaled raw value {raw} for physical={physical} out of uint16 range [0, {UINT16_MAX}]"
|
||||
)
|
||||
return raw
|
||||
|
||||
|
||||
def raw_to_scaled(raw: int, scale: float, offset: float = 0.0) -> float:
|
||||
"""Convert a raw Modbus uint16 to a physical value."""
|
||||
return raw * scale + offset
|
||||
|
||||
|
||||
def scale_to_raw_signed(physical: float, scale: float, offset: float = 0.0) -> int:
|
||||
"""Convert a physical value to a signed int16 stored as uint16 (two's complement).
|
||||
|
||||
Raises ``ValueError`` if the result does not fit in int16.
|
||||
"""
|
||||
if scale == 0.0:
|
||||
raise ValueError("scale must not be 0")
|
||||
if not math.isfinite(physical):
|
||||
raise ValueError(f"physical value must be finite, got {physical!r}")
|
||||
raw = round((physical - offset) / scale)
|
||||
if raw < INT16_MIN or raw > INT16_MAX:
|
||||
raise ValueError(
|
||||
f"signed raw {raw} for physical={physical} out of int16 range"
|
||||
)
|
||||
# Return as uint16 two's-complement
|
||||
return int(raw) & 0xFFFF
|
||||
|
||||
|
||||
def raw_signed_to_scaled(raw: int, scale: float, offset: float = 0.0) -> float:
|
||||
"""Convert a two's-complement uint16 raw to a signed physical value."""
|
||||
if raw > INT16_MAX:
|
||||
raw -= 0x10000
|
||||
return raw * scale + offset
|
||||
|
||||
|
||||
def clamp_uint16(value: int) -> int:
|
||||
"""Clamp an integer to [0, 65535]."""
|
||||
return max(0, min(UINT16_MAX, value))
|
||||
|
||||
|
||||
def validate_holding_write(reg_name: str, raw_value: int) -> None:
|
||||
"""Raise ``ValueError`` if ``raw_value`` is outside uint16 range.
|
||||
|
||||
This is a wire-level guard; the caller is responsible for applying any
|
||||
sign extension or scale conversion before calling this function.
|
||||
"""
|
||||
if not isinstance(raw_value, int):
|
||||
raise TypeError(f"Holding write for {reg_name}: value must be int, got {type(raw_value)}")
|
||||
if raw_value < 0 or raw_value > UINT16_MAX:
|
||||
raise ValueError(
|
||||
f"Holding register '{reg_name}': raw value {raw_value} out of uint16 range "
|
||||
f"[0, {UINT16_MAX}]"
|
||||
)
|
||||
|
||||
|
||||
def heading_deg_to_raw(heading_deg: float) -> int:
|
||||
"""Convert a compass heading in degrees to the HEADING_SETPOINT_X100 raw value.
|
||||
|
||||
Normalises to [0, 360) before scaling. Raises ``ValueError`` for non-finite input.
|
||||
"""
|
||||
if not math.isfinite(heading_deg):
|
||||
raise ValueError(f"heading_deg must be finite, got {heading_deg!r}")
|
||||
normalised = heading_deg % 360.0
|
||||
return scale_to_raw(normalised, scale=0.01)
|
||||
|
||||
|
||||
def raw_to_heading_deg(raw: int) -> float:
|
||||
"""Convert a HEADING_X100 raw value to degrees."""
|
||||
return raw * 0.01
|
||||
|
||||
|
||||
def rudder_deg_to_raw_signed(rudder_deg: float) -> int:
|
||||
"""Convert a rudder angle in degrees (±35) to a signed uint16 raw value × 100."""
|
||||
return scale_to_raw_signed(rudder_deg, scale=0.01)
|
||||
@@ -0,0 +1,170 @@
|
||||
"""Integration: alarm engine → audit log hash-chain -- Sprint 9.
|
||||
|
||||
Tests the complete flow: alarm fires → AlarmEngine produces Alarm records →
|
||||
AuditEvent is written to the log → hash-chain verifies OK.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from arautopilot.core.alarm_engine import AlarmEngine, TelemetrySnapshot
|
||||
from arautopilot.core.alarms import AlarmType, AlarmSeverity
|
||||
from arautopilot.core.audit import AuditEvent, AuditLog, AuditOutcome
|
||||
|
||||
|
||||
def _alarm_to_event(alarm) -> AuditEvent:
|
||||
return AuditEvent(
|
||||
action=f"alarm.{alarm.type}",
|
||||
outcome=AuditOutcome.FAILED,
|
||||
reason=alarm.message,
|
||||
extra={"severity": str(alarm.severity), "auto_disengage": alarm.auto_disengage_triggered},
|
||||
)
|
||||
|
||||
|
||||
def _ack_event(alarm_type: AlarmType, user_id: str = "operator") -> AuditEvent:
|
||||
return AuditEvent(
|
||||
user_id=user_id,
|
||||
action=f"alarm.acknowledge.{alarm_type}",
|
||||
outcome=AuditOutcome.SUCCESS,
|
||||
)
|
||||
|
||||
|
||||
class TestAlarmFiredAndLogged:
|
||||
def test_single_alarm_logged_and_chain_valid(self, tmp_path: Path):
|
||||
log = AuditLog(tmp_path / "audit.jsonl")
|
||||
engine = AlarmEngine()
|
||||
|
||||
# Fire an off-course alarm
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=True))
|
||||
assert len(alarms) == 1
|
||||
log.append(_alarm_to_event(alarms[0]))
|
||||
|
||||
ok, reason = log.verify_chain()
|
||||
assert ok, reason
|
||||
assert len(log) == 1
|
||||
|
||||
def test_alarm_acknowledge_logged_and_chain_valid(self, tmp_path: Path):
|
||||
log = AuditLog(tmp_path / "audit.jsonl")
|
||||
engine = AlarmEngine()
|
||||
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=True))
|
||||
log.append(_alarm_to_event(alarms[0]))
|
||||
|
||||
engine.acknowledge(AlarmType.OFF_COURSE)
|
||||
log.append(_ack_event(AlarmType.OFF_COURSE))
|
||||
|
||||
ok, reason = log.verify_chain()
|
||||
assert ok, reason
|
||||
assert len(log) == 2
|
||||
|
||||
def test_multiple_alarms_all_logged(self, tmp_path: Path):
|
||||
log = AuditLog(tmp_path / "audit.jsonl")
|
||||
engine = AlarmEngine()
|
||||
|
||||
snap = TelemetrySnapshot(
|
||||
fw_alarm_off_course=True,
|
||||
fw_alarm_voltage_low=True,
|
||||
)
|
||||
alarms = engine.evaluate(snap)
|
||||
assert len(alarms) == 2
|
||||
|
||||
for a in alarms:
|
||||
log.append(_alarm_to_event(a))
|
||||
|
||||
ok, reason = log.verify_chain()
|
||||
assert ok, reason
|
||||
assert len(log) == 2
|
||||
|
||||
def test_disengage_event_logged_in_chain(self, tmp_path: Path):
|
||||
log = AuditLog(tmp_path / "audit.jsonl")
|
||||
disengages = []
|
||||
engine = AlarmEngine(on_disengage=lambda: disengages.append(True))
|
||||
|
||||
# EMERGENCY alarm triggers auto-disengage
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_heading_lost=True))
|
||||
assert len(disengages) >= 1
|
||||
assert alarms[0].auto_disengage_triggered
|
||||
|
||||
log.append(_alarm_to_event(alarms[0]))
|
||||
log.append(AuditEvent(
|
||||
action="pilot.disengage",
|
||||
outcome=AuditOutcome.SUCCESS,
|
||||
reason="auto-disengage from alarm",
|
||||
extra={"trigger": str(alarms[0].type)},
|
||||
))
|
||||
|
||||
ok, reason = log.verify_chain()
|
||||
assert ok, reason
|
||||
|
||||
def test_alarm_clear_and_refire_both_logged(self, tmp_path: Path):
|
||||
log = AuditLog(tmp_path / "audit.jsonl")
|
||||
engine = AlarmEngine()
|
||||
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=True))
|
||||
log.append(_alarm_to_event(alarms[0]))
|
||||
|
||||
# Clear
|
||||
engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=False))
|
||||
log.append(AuditEvent(action="alarm.cleared.off_course", outcome=AuditOutcome.SUCCESS))
|
||||
|
||||
# Refire
|
||||
alarms2 = engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=True))
|
||||
assert len(alarms2) == 1
|
||||
log.append(_alarm_to_event(alarms2[0]))
|
||||
|
||||
ok, reason = log.verify_chain()
|
||||
assert ok, reason
|
||||
assert len(log) == 3
|
||||
|
||||
|
||||
class TestAuditPersistenceAcrossReload:
|
||||
def test_reloaded_log_continues_chain(self, tmp_path: Path):
|
||||
p = tmp_path / "audit.jsonl"
|
||||
log1 = AuditLog(p)
|
||||
engine = AlarmEngine()
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=True))
|
||||
log1.append(_alarm_to_event(alarms[0]))
|
||||
|
||||
# Simulate restarting the Studio
|
||||
log2 = AuditLog(p)
|
||||
log2.append(AuditEvent(action="studio.startup", outcome=AuditOutcome.SUCCESS))
|
||||
|
||||
ok, reason = log2.verify_chain()
|
||||
assert ok, reason
|
||||
assert len(log2) == 2
|
||||
|
||||
def test_tampered_alarm_entry_detected(self, tmp_path: Path):
|
||||
import json
|
||||
p = tmp_path / "audit.jsonl"
|
||||
log = AuditLog(p)
|
||||
engine = AlarmEngine()
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_off_course=True))
|
||||
log.append(_alarm_to_event(alarms[0]))
|
||||
|
||||
# Tamper: change the action field
|
||||
lines = p.read_text(encoding="utf-8").splitlines()
|
||||
data = json.loads(lines[0])
|
||||
data["action"] = "alarm.no_problem_here"
|
||||
lines[0] = json.dumps(data)
|
||||
p.write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
|
||||
log2 = AuditLog(p)
|
||||
ok, reason = log2.verify_chain()
|
||||
assert not ok
|
||||
assert "mismatch" in reason.lower() or "tamper" in reason.lower()
|
||||
|
||||
|
||||
class TestAlarmSeverityInAudit:
|
||||
def test_emergency_severity_recorded(self, tmp_path: Path):
|
||||
log = AuditLog(tmp_path / "audit.jsonl")
|
||||
engine = AlarmEngine()
|
||||
alarms = engine.evaluate(TelemetrySnapshot(fw_alarm_heading_lost=True))
|
||||
assert alarms[0].severity == AlarmSeverity.EMERGENCY
|
||||
event = _alarm_to_event(alarms[0])
|
||||
assert event.extra["severity"] == "emergency"
|
||||
log.append(event)
|
||||
events = log.read_all()
|
||||
assert events[0].extra["severity"] == "emergency"
|
||||
@@ -0,0 +1,153 @@
|
||||
"""Integration: full cascade PID closed-loop simulation -- Sprint 9.
|
||||
|
||||
Tests that the three-layer cascade (outer heading PID → inner rudder PID →
|
||||
rudder dynamics → vessel heading) converges to a heading setpoint within the
|
||||
acceptance limits of the brief.
|
||||
|
||||
No mocks — all four simulator modules run together.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig
|
||||
from arautopilot.studio.simulator.pid_outer import PidOuter, PidOuterConfig
|
||||
from arautopilot.studio.simulator.rudder_dynamics import (
|
||||
RudderDynamicsConfig,
|
||||
RudderSimulator,
|
||||
)
|
||||
from arautopilot.studio.simulator.vessel_heading import (
|
||||
VesselHeadingConfig,
|
||||
VesselHeadingSimulator,
|
||||
heading_error_deg,
|
||||
)
|
||||
|
||||
|
||||
def _run_cascade(
|
||||
*,
|
||||
initial_heading: float,
|
||||
setpoint_deg: float,
|
||||
duration_s: float = 120.0,
|
||||
outer_dt: float = 0.1, # 10 Hz
|
||||
inner_dt: float = 0.02, # 50 Hz
|
||||
speed_kn: float = 10.0,
|
||||
) -> tuple[float, float]:
|
||||
"""Return (final_heading_error_deg, max_rudder_deg_reached)."""
|
||||
outer = PidOuter(PidOuterConfig(
|
||||
base_kp=0.9, base_ki=0.02, base_kd=1.2,
|
||||
deadband_deg=0.5, rot_ff_gain=1.5, max_rudder_deg=35.0,
|
||||
))
|
||||
inner = PidInner(PidInnerConfig(
|
||||
kp=2.5, ki=0.15, kd=0.30,
|
||||
deadband_deg=0.5, min_useful_pwm_pct=12.0,
|
||||
))
|
||||
rudder = RudderSimulator(RudderDynamicsConfig(
|
||||
actuator_gain=0.2, friction=4.0, max_angle_deg=35.0,
|
||||
))
|
||||
vessel = VesselHeadingSimulator(VesselHeadingConfig(
|
||||
rudder_response_gain=0.18, yaw_damping=0.8, speed_kn=speed_kn,
|
||||
))
|
||||
vessel.reset(heading_deg=initial_heading)
|
||||
|
||||
t = 0.0
|
||||
max_rudder = 0.0
|
||||
rudder_setpoint = 0.0
|
||||
outer_acc = 0.0
|
||||
|
||||
while t < duration_s:
|
||||
# Outer loop tick (10 Hz)
|
||||
if outer_acc >= outer_dt - 1e-9:
|
||||
outer_acc = 0.0
|
||||
rudder_setpoint = outer.step(
|
||||
heading_setpoint_deg=setpoint_deg,
|
||||
heading_measured_deg=vessel.state.heading_deg,
|
||||
rate_of_turn_dps=vessel.state.rate_of_turn_dps,
|
||||
speed_kn=speed_kn,
|
||||
allowed=True,
|
||||
)
|
||||
|
||||
# Inner loop tick (50 Hz)
|
||||
pwm = inner.step(
|
||||
setpoint_deg=rudder_setpoint,
|
||||
measured_deg=rudder.state.angle_deg,
|
||||
allowed=True,
|
||||
)
|
||||
rudder.step(dt=inner_dt, pwm_pct=pwm)
|
||||
vessel.step(dt=inner_dt, rudder_deg=rudder.state.angle_deg, speed_kn=speed_kn)
|
||||
|
||||
max_rudder = max(max_rudder, abs(rudder.state.angle_deg))
|
||||
outer_acc += inner_dt
|
||||
t += inner_dt
|
||||
|
||||
final_error = abs(heading_error_deg(setpoint_deg, vessel.state.heading_deg))
|
||||
return final_error, max_rudder
|
||||
|
||||
|
||||
class TestCascadeConvergence:
|
||||
def test_small_heading_change_converges(self):
|
||||
err, _ = _run_cascade(initial_heading=0.0, setpoint_deg=10.0)
|
||||
assert err < 6.0, f"Did not converge: final error = {err:.2f} deg"
|
||||
|
||||
def test_90_degree_turn_converges(self):
|
||||
err, _ = _run_cascade(initial_heading=0.0, setpoint_deg=90.0, duration_s=180.0)
|
||||
assert err < 8.0, f"Did not converge: final error = {err:.2f} deg"
|
||||
|
||||
def test_wrap_around_north_converges(self):
|
||||
# 350° → 10° = +20 degrees through north
|
||||
err, _ = _run_cascade(initial_heading=350.0, setpoint_deg=10.0)
|
||||
assert err < 6.0, f"Wrap-around did not converge: final error = {err:.2f} deg"
|
||||
|
||||
def test_180_degree_turn_converges(self):
|
||||
err, _ = _run_cascade(initial_heading=0.0, setpoint_deg=180.0, duration_s=240.0)
|
||||
assert err < 10.0, f"180° turn did not converge: final error = {err:.2f} deg"
|
||||
|
||||
def test_rudder_stays_within_limits(self):
|
||||
_, max_rudder = _run_cascade(initial_heading=0.0, setpoint_deg=90.0)
|
||||
assert max_rudder <= 35.0 + 1e-6
|
||||
|
||||
def test_slow_speed_converges(self):
|
||||
# At low speed the vessel turns slower but must still converge
|
||||
err, _ = _run_cascade(
|
||||
initial_heading=0.0, setpoint_deg=30.0, speed_kn=3.0, duration_s=240.0
|
||||
)
|
||||
assert err < 10.0, f"Low-speed case did not converge: final error = {err:.2f} deg"
|
||||
|
||||
def test_same_heading_no_overshoot(self):
|
||||
# Already on setpoint: rudder should barely move
|
||||
err, max_rudder = _run_cascade(
|
||||
initial_heading=90.0, setpoint_deg=90.0, duration_s=30.0
|
||||
)
|
||||
assert err < 2.0
|
||||
assert max_rudder < 5.0
|
||||
|
||||
|
||||
class TestCascadeEnergyBudget:
|
||||
def test_integrator_does_not_wind_up(self):
|
||||
# After convergence, integrator should be small (no sustained error)
|
||||
outer = PidOuter(PidOuterConfig(base_kp=0.9, base_ki=0.02, base_kd=1.2))
|
||||
inner = PidInner()
|
||||
rudder = RudderSimulator()
|
||||
vessel = VesselHeadingSimulator()
|
||||
vessel.reset(heading_deg=0.0)
|
||||
setpoint = 45.0
|
||||
outer_acc = 0.0
|
||||
rudder_sp = 0.0
|
||||
for _ in range(int(60.0 / 0.02)):
|
||||
if outer_acc >= 0.1 - 1e-9:
|
||||
outer_acc = 0.0
|
||||
rudder_sp = outer.step(
|
||||
heading_setpoint_deg=setpoint,
|
||||
heading_measured_deg=vessel.state.heading_deg,
|
||||
rate_of_turn_dps=vessel.state.rate_of_turn_dps,
|
||||
speed_kn=10.0,
|
||||
allowed=True,
|
||||
)
|
||||
pwm = inner.step(setpoint_deg=rudder_sp, measured_deg=rudder.state.angle_deg, allowed=True)
|
||||
rudder.step(dt=0.02, pwm_pct=pwm)
|
||||
vessel.step(dt=0.02, rudder_deg=rudder.state.angle_deg)
|
||||
outer_acc += 0.02
|
||||
# Integral should have settled to a small value after convergence
|
||||
assert abs(outer.state.integral) < 20.0
|
||||
@@ -0,0 +1,157 @@
|
||||
"""Integration: HeadingEKF smoothing noisy heading input to the outer PID -- Sprint 9.
|
||||
|
||||
Verifies that the EKF reduces the impact of sensor noise on the outer PID
|
||||
without preventing the cascade from converging to the heading setpoint.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import random
|
||||
|
||||
import pytest
|
||||
|
||||
from arautopilot.core.heading_ekf import HeadingEKF
|
||||
from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig
|
||||
from arautopilot.studio.simulator.pid_outer import PidOuter, PidOuterConfig
|
||||
from arautopilot.studio.simulator.rudder_dynamics import RudderDynamicsConfig, RudderSimulator
|
||||
from arautopilot.studio.simulator.vessel_heading import (
|
||||
VesselHeadingConfig,
|
||||
VesselHeadingSimulator,
|
||||
heading_error_deg,
|
||||
)
|
||||
|
||||
|
||||
def _run_with_noise(
|
||||
*,
|
||||
noise_std_deg: float = 3.0,
|
||||
use_ekf: bool = True,
|
||||
setpoint_deg: float = 45.0,
|
||||
duration_s: float = 120.0,
|
||||
seed: int = 42,
|
||||
) -> tuple[float, float]:
|
||||
"""Return (final_error_deg, rudder_total_variation) for a noisy heading run.
|
||||
|
||||
``rudder_total_variation`` measures how much the rudder setpoint oscillated;
|
||||
less variation indicates the EKF is reducing noise amplification.
|
||||
"""
|
||||
rng = random.Random(seed)
|
||||
outer = PidOuter(PidOuterConfig(
|
||||
base_kp=0.9, base_ki=0.02, base_kd=1.2,
|
||||
deadband_deg=0.5, rot_ff_gain=1.5,
|
||||
))
|
||||
inner = PidInner(PidInnerConfig(kp=2.5, ki=0.15, kd=0.30))
|
||||
rudder = RudderSimulator(RudderDynamicsConfig())
|
||||
vessel = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=10.0))
|
||||
vessel.reset(heading_deg=0.0)
|
||||
|
||||
ekf = HeadingEKF(
|
||||
heading_deg=0.0,
|
||||
process_noise_heading=0.01,
|
||||
process_noise_rot=0.1,
|
||||
)
|
||||
|
||||
outer_acc = 0.0
|
||||
inner_dt = 0.02
|
||||
rudder_sp = 0.0
|
||||
prev_rudder_sp = 0.0
|
||||
total_variation = 0.0
|
||||
|
||||
for _ in range(int(duration_s / inner_dt)):
|
||||
# Add Gaussian noise to the true heading for the sensor measurement
|
||||
noisy_heading = vessel.state.heading_deg + rng.gauss(0.0, noise_std_deg)
|
||||
noisy_heading %= 360.0
|
||||
noisy_rot = vessel.state.rate_of_turn_dps + rng.gauss(0.0, 0.5)
|
||||
|
||||
if use_ekf:
|
||||
ekf.predict(dt_s=inner_dt)
|
||||
ekf.update_heading(noisy_heading, noise_deg=noise_std_deg)
|
||||
ekf.update_rot(noisy_rot, noise_dps=0.5)
|
||||
heading_input = ekf.heading_deg
|
||||
rot_input = ekf.rot_dps
|
||||
else:
|
||||
heading_input = noisy_heading
|
||||
rot_input = noisy_rot
|
||||
|
||||
if outer_acc >= 0.1 - 1e-9:
|
||||
outer_acc = 0.0
|
||||
rudder_sp = outer.step(
|
||||
heading_setpoint_deg=setpoint_deg,
|
||||
heading_measured_deg=heading_input,
|
||||
rate_of_turn_dps=rot_input,
|
||||
speed_kn=10.0,
|
||||
allowed=True,
|
||||
)
|
||||
total_variation += abs(rudder_sp - prev_rudder_sp)
|
||||
prev_rudder_sp = rudder_sp
|
||||
|
||||
pwm = inner.step(setpoint_deg=rudder_sp, measured_deg=rudder.state.angle_deg, allowed=True)
|
||||
rudder.step(dt=inner_dt, pwm_pct=pwm)
|
||||
vessel.step(dt=inner_dt, rudder_deg=rudder.state.angle_deg)
|
||||
outer_acc += inner_dt
|
||||
|
||||
final_error = abs(heading_error_deg(setpoint_deg, vessel.state.heading_deg))
|
||||
return final_error, total_variation
|
||||
|
||||
|
||||
class TestEkfReducesNoise:
|
||||
def test_ekf_filter_converges_to_setpoint(self):
|
||||
err, _ = _run_with_noise(use_ekf=True, noise_std_deg=3.0)
|
||||
assert err < 4.0, f"EKF run did not converge: final error = {err:.2f} deg"
|
||||
|
||||
def test_no_ekf_has_larger_residual_than_ekf(self):
|
||||
# Without EKF, large sensor noise prevents tight convergence.
|
||||
# This test just confirms the EKF case is materially better.
|
||||
err_ekf, _ = _run_with_noise(use_ekf=True, noise_std_deg=3.0, seed=99)
|
||||
err_raw, _ = _run_with_noise(use_ekf=False, noise_std_deg=3.0, seed=99)
|
||||
assert err_ekf < err_raw, (
|
||||
f"EKF should produce lower residual: ekf={err_ekf:.2f} raw={err_raw:.2f}"
|
||||
)
|
||||
|
||||
def test_ekf_reduces_rudder_variation(self):
|
||||
_, var_ekf = _run_with_noise(use_ekf=True, noise_std_deg=5.0, seed=7)
|
||||
_, var_raw = _run_with_noise(use_ekf=False, noise_std_deg=5.0, seed=7)
|
||||
assert var_ekf < var_raw, (
|
||||
f"EKF should reduce rudder variation: ekf={var_ekf:.1f} raw={var_raw:.1f}"
|
||||
)
|
||||
|
||||
def test_ekf_tracks_heading_during_steady_state(self):
|
||||
"""After convergence, EKF should track true heading within its noise level."""
|
||||
ekf = HeadingEKF(heading_deg=45.0)
|
||||
rng = random.Random(0)
|
||||
true_heading = 45.0
|
||||
noise_std = 2.0
|
||||
for _ in range(200):
|
||||
ekf.predict(dt_s=0.1)
|
||||
noisy = true_heading + rng.gauss(0.0, noise_std)
|
||||
ekf.update_heading(noisy, noise_deg=noise_std)
|
||||
assert abs(ekf.heading_deg - true_heading) < 2.0
|
||||
|
||||
def test_ekf_handles_wrap_around_during_turn(self):
|
||||
"""EKF should track heading smoothly through the 0/360 boundary."""
|
||||
ekf = HeadingEKF(heading_deg=355.0, rot_dps=5.0)
|
||||
for _ in range(30):
|
||||
ekf.predict(dt_s=0.1)
|
||||
# True heading wraps from 355 → 5 during this sequence
|
||||
true = (355.0 + _ * 5.0 * 0.1) % 360.0
|
||||
ekf.update_heading(true, noise_deg=1.0)
|
||||
# Should have tracked through the wrap
|
||||
expected = (355.0 + 30 * 5.0 * 0.1) % 360.0
|
||||
from arautopilot.core.heading_ekf import _shortest_arc
|
||||
diff = abs(_shortest_arc(ekf.heading_deg, expected))
|
||||
assert diff < 5.0
|
||||
|
||||
|
||||
class TestEkfEdgeCases:
|
||||
def test_large_noise_ekf_still_converges(self):
|
||||
err, _ = _run_with_noise(use_ekf=True, noise_std_deg=10.0, duration_s=180.0)
|
||||
assert err < 8.0
|
||||
|
||||
def test_zero_noise_ekf_matches_true_heading(self):
|
||||
ekf = HeadingEKF(heading_deg=0.0)
|
||||
for i in range(100):
|
||||
ekf.predict(dt_s=0.1)
|
||||
ekf.update_heading(float(i % 360), noise_deg=0.01)
|
||||
# With near-zero noise, EKF should track closely
|
||||
# (just checking it doesn't crash and stays in [0,360))
|
||||
assert 0.0 <= ekf.heading_deg < 360.0
|
||||
@@ -0,0 +1,185 @@
|
||||
"""Tests for Modbus register utility helpers -- Sprint 9."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from arautopilot.shared.modbus_utils import (
|
||||
UINT16_MAX,
|
||||
INT16_MAX,
|
||||
INT16_MIN,
|
||||
clamp_uint16,
|
||||
heading_deg_to_raw,
|
||||
raw_signed_to_scaled,
|
||||
raw_to_heading_deg,
|
||||
raw_to_scaled,
|
||||
rudder_deg_to_raw_signed,
|
||||
scale_to_raw,
|
||||
scale_to_raw_signed,
|
||||
validate_holding_write,
|
||||
)
|
||||
|
||||
|
||||
class TestScaleToRaw:
|
||||
def test_basic_heading(self):
|
||||
assert scale_to_raw(45.0, scale=0.01) == 4500
|
||||
|
||||
def test_zero(self):
|
||||
assert scale_to_raw(0.0, scale=0.01) == 0
|
||||
|
||||
def test_max_uint16(self):
|
||||
assert scale_to_raw(655.35, scale=0.01) == UINT16_MAX
|
||||
|
||||
def test_out_of_range_raises(self):
|
||||
with pytest.raises(ValueError, match="uint16"):
|
||||
scale_to_raw(700.0, scale=0.01)
|
||||
|
||||
def test_negative_raises(self):
|
||||
with pytest.raises(ValueError, match="uint16"):
|
||||
scale_to_raw(-1.0, scale=0.01)
|
||||
|
||||
def test_zero_scale_raises(self):
|
||||
with pytest.raises(ValueError, match="scale"):
|
||||
scale_to_raw(10.0, scale=0.0)
|
||||
|
||||
def test_inf_raises(self):
|
||||
with pytest.raises(ValueError, match="finite"):
|
||||
scale_to_raw(math.inf, scale=0.01)
|
||||
|
||||
def test_nan_raises(self):
|
||||
with pytest.raises(ValueError, match="finite"):
|
||||
scale_to_raw(math.nan, scale=0.01)
|
||||
|
||||
def test_with_offset(self):
|
||||
# physical=10, offset=5, scale=1 → raw = (10-5)/1 = 5
|
||||
assert scale_to_raw(10.0, scale=1.0, offset=5.0) == 5
|
||||
|
||||
|
||||
class TestRawToScaled:
|
||||
def test_basic(self):
|
||||
assert raw_to_scaled(4500, scale=0.01) == pytest.approx(45.0)
|
||||
|
||||
def test_with_offset(self):
|
||||
assert raw_to_scaled(5, scale=1.0, offset=5.0) == pytest.approx(10.0)
|
||||
|
||||
|
||||
class TestScaleToRawSigned:
|
||||
def test_positive_rudder(self):
|
||||
# +10.0 deg * 100 = 1000
|
||||
assert scale_to_raw_signed(10.0, scale=0.01) == 1000
|
||||
|
||||
def test_negative_rudder(self):
|
||||
raw = scale_to_raw_signed(-10.0, scale=0.01)
|
||||
# -1000 in two's complement uint16 = 65536 - 1000 = 64536
|
||||
assert raw == (0xFFFF + 1) - 1000
|
||||
|
||||
def test_zero(self):
|
||||
assert scale_to_raw_signed(0.0, scale=0.01) == 0
|
||||
|
||||
def test_max_positive(self):
|
||||
assert scale_to_raw_signed(327.67, scale=0.01) == INT16_MAX
|
||||
|
||||
def test_out_of_range_positive_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
scale_to_raw_signed(400.0, scale=0.01)
|
||||
|
||||
def test_out_of_range_negative_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
scale_to_raw_signed(-400.0, scale=0.01)
|
||||
|
||||
def test_nan_raises(self):
|
||||
with pytest.raises(ValueError, match="finite"):
|
||||
scale_to_raw_signed(math.nan, scale=0.01)
|
||||
|
||||
|
||||
class TestRawSignedToScaled:
|
||||
def test_positive(self):
|
||||
assert raw_signed_to_scaled(1000, scale=0.01) == pytest.approx(10.0)
|
||||
|
||||
def test_negative(self):
|
||||
raw = (0x10000 - 1000) # -1000 in two's complement
|
||||
assert raw_signed_to_scaled(raw, scale=0.01) == pytest.approx(-10.0)
|
||||
|
||||
def test_zero(self):
|
||||
assert raw_signed_to_scaled(0, scale=0.01) == pytest.approx(0.0)
|
||||
|
||||
|
||||
class TestClampUint16:
|
||||
def test_in_range(self):
|
||||
assert clamp_uint16(1000) == 1000
|
||||
|
||||
def test_below_zero(self):
|
||||
assert clamp_uint16(-5) == 0
|
||||
|
||||
def test_above_max(self):
|
||||
assert clamp_uint16(70000) == UINT16_MAX
|
||||
|
||||
def test_at_max(self):
|
||||
assert clamp_uint16(UINT16_MAX) == UINT16_MAX
|
||||
|
||||
|
||||
class TestValidateHoldingWrite:
|
||||
def test_valid(self):
|
||||
validate_holding_write("HEADING_SETPOINT_X100", 4500) # no exception
|
||||
|
||||
def test_negative_raises(self):
|
||||
with pytest.raises(ValueError, match="uint16"):
|
||||
validate_holding_write("HEADING_SETPOINT_X100", -1)
|
||||
|
||||
def test_over_max_raises(self):
|
||||
with pytest.raises(ValueError, match="uint16"):
|
||||
validate_holding_write("HEADING_SETPOINT_X100", UINT16_MAX + 1)
|
||||
|
||||
def test_float_raises_type_error(self):
|
||||
with pytest.raises(TypeError):
|
||||
validate_holding_write("X", 45.0)
|
||||
|
||||
def test_zero_valid(self):
|
||||
validate_holding_write("TEST", 0)
|
||||
|
||||
def test_max_valid(self):
|
||||
validate_holding_write("TEST", UINT16_MAX)
|
||||
|
||||
|
||||
class TestHeadingConversion:
|
||||
def test_zero(self):
|
||||
assert heading_deg_to_raw(0.0) == 0
|
||||
|
||||
def test_45_deg(self):
|
||||
assert heading_deg_to_raw(45.0) == 4500
|
||||
|
||||
def test_360_wraps_to_0(self):
|
||||
assert heading_deg_to_raw(360.0) == 0
|
||||
|
||||
def test_over_360_wraps(self):
|
||||
assert heading_deg_to_raw(370.0) == heading_deg_to_raw(10.0)
|
||||
|
||||
def test_negative_wraps(self):
|
||||
assert heading_deg_to_raw(-10.0) == heading_deg_to_raw(350.0)
|
||||
|
||||
def test_inf_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
heading_deg_to_raw(math.inf)
|
||||
|
||||
def test_roundtrip(self):
|
||||
for deg in [0.0, 45.0, 90.0, 135.0, 180.0, 270.0, 359.99]:
|
||||
raw = heading_deg_to_raw(deg)
|
||||
back = raw_to_heading_deg(raw)
|
||||
assert abs(back - deg) < 0.01
|
||||
|
||||
|
||||
class TestRudderConversion:
|
||||
def test_positive(self):
|
||||
raw = rudder_deg_to_raw_signed(10.0)
|
||||
back = raw_signed_to_scaled(raw, scale=0.01)
|
||||
assert back == pytest.approx(10.0)
|
||||
|
||||
def test_negative(self):
|
||||
raw = rudder_deg_to_raw_signed(-10.0)
|
||||
back = raw_signed_to_scaled(raw, scale=0.01)
|
||||
assert back == pytest.approx(-10.0)
|
||||
|
||||
def test_zero(self):
|
||||
assert rudder_deg_to_raw_signed(0.0) == 0
|
||||
Reference in New Issue
Block a user