alro65
42ee63b776
End-to-end implementation per docs/sprint-3-plan.md.
Closes the cascade: outer loop (heading control, 10 Hz on Core 1) drives
the inner loop (rudder position control, 50 Hz from Sprint 2). First real
mode other than STANDBY is now activable: HEADING_HOLD.
Builds: pio run -e esp32-dev SUCCESS, RAM 6.8%, Flash 27.1% (355 KB).
Tests: pytest 258/258 green (231 Sprint 2.5 + 27 Sprint 3 new).
Python (arautopilot/studio/simulator/):
- vessel_heading.py: first-order yaw model. ROT responds to
rudder*speed; damping returns ROT to zero when rudder is centred.
Defaults tuned so 5 deg rudder @ 10 kn -> ~3 dps steady-state ROT.
Includes heading_error_deg() shortest-arc helper.
- pid_outer.py: pure-Python outer heading PID. Anti-windup via back-
calculation, gain scheduling by SOG, deadband, derivative LPF,
output saturation, ROT feed-forward (brief sec. 6 -- the term that
distinguishes a premium autopilot from a basic one), rate limit on
produced rudder setpoint, shortest-arc heading wrap-around.
Firmware (firmware/ar_autopilot_v1/src/pid/):
- pid_outer.h: header-only C++17 port. Same algorithm, same variables,
same numerics. Fixed-capacity gain schedule (up to 8 points).
- pid_outer_task.{h,cpp}: 10 Hz FreeRTOS task on Core 1. Subscribes to
TWDT. Reads heading + ROT from the NMEA 2000 snapshot. Uses
operator-configurable SOG (default 15 kn until PGN 129026 wiring in
Sprint 5). Pushes rudder setpoint into the inner loop only when
current_mode == HEADING_HOLD.
Modes (firmware/ar_autopilot_v1/src/modes/standby.cpp):
- HEADING_HOLD activable via request_mode(). Pre-conditions:
* NMEA 2000 heading sensor valid (fresh PGN 127250)
* Rudder sensor valid (median filter filled)
On success, captures current heading as initial setpoint so the
operator doesn't get a sudden swing toward an old setpoint.
Modbus (regenerated from YAML):
- 7 new INPUTs (50-56): outer heading setpoint, produced rudder
setpoint, error, current SOG, live kp/ki/kd.
- 5 new HOLDINGs (24-28): writable heading setpoint, SOG override,
outer base gains. Writing any of kp/ki/kd disables the built-in
3-point gain schedule (operator override).
Tests:
- test_vessel_heading_simulator.py: 6 dynamics tests + 9 parameterised
heading_error_deg edge cases (wrap-around).
- test_pid_outer_python.py: 12 tests covering gain interpolation,
per-tick PID behaviour (deadband, sign, ROT feed-forward,
saturation, rate limit, allowed=false), and three end-to-end cascade
tests (positive step, negative step, wrap-around 360->10).
Cascade verification: outer + inner + rudder dynamics + vessel-heading
simulator settles a 30 deg step within +-2 deg in 60 s.
NOT in Sprint 3 (intentional):
- True Course / Track Keeping / Dodge -- Sprint 5
- Off-course alarms + auto-disengage on sensor loss -- Sprint 6
- COG / SOG / Position via N2K PGN 129025/9/6 -- Sprint 5
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
86 lines
2.6 KiB
Python
86 lines
2.6 KiB
Python
"""Tests for the vessel-heading simulator."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import pytest
|
|
|
|
from arautopilot.studio.simulator.vessel_heading import (
|
|
VesselHeadingConfig,
|
|
VesselHeadingSimulator,
|
|
heading_error_deg,
|
|
)
|
|
|
|
|
|
def test_zero_rudder_holds_heading() -> None:
|
|
sim = VesselHeadingSimulator()
|
|
sim.reset(heading_deg=42.0)
|
|
for _ in range(2000):
|
|
sim.step(dt=0.01, rudder_deg=0.0)
|
|
assert sim.state.heading_deg == pytest.approx(42.0, abs=1e-3)
|
|
|
|
|
|
def test_positive_rudder_turns_starboard() -> None:
|
|
sim = VesselHeadingSimulator()
|
|
sim.reset(heading_deg=0.0)
|
|
for _ in range(2000):
|
|
sim.step(dt=0.01, rudder_deg=5.0)
|
|
# After 20 s with +5 deg of rudder, heading should advance (mod 360).
|
|
assert sim.state.rate_of_turn_dps > 0.0
|
|
assert sim.state.heading_deg != 0.0
|
|
|
|
|
|
def test_negative_rudder_turns_port() -> None:
|
|
sim = VesselHeadingSimulator()
|
|
sim.reset(heading_deg=0.0)
|
|
for _ in range(2000):
|
|
sim.step(dt=0.01, rudder_deg=-5.0)
|
|
assert sim.state.rate_of_turn_dps < 0.0
|
|
|
|
|
|
def test_speed_increases_yaw_response() -> None:
|
|
sim_slow = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=5.0))
|
|
sim_fast = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=20.0))
|
|
sim_slow.reset()
|
|
sim_fast.reset()
|
|
for _ in range(2000):
|
|
sim_slow.step(dt=0.01, rudder_deg=5.0)
|
|
sim_fast.step(dt=0.01, rudder_deg=5.0)
|
|
# Fast vessel turns farther in the same time.
|
|
assert abs(sim_fast.state.rate_of_turn_dps) > abs(sim_slow.state.rate_of_turn_dps)
|
|
|
|
|
|
def test_heading_wraps_at_360() -> None:
|
|
sim = VesselHeadingSimulator()
|
|
sim.reset(heading_deg=359.0, rate_of_turn_dps=10.0)
|
|
for _ in range(20):
|
|
sim.step(dt=0.1, rudder_deg=0.0)
|
|
# heading must remain in [0, 360)
|
|
assert 0.0 <= sim.state.heading_deg < 360.0
|
|
|
|
|
|
def test_invalid_dt() -> None:
|
|
sim = VesselHeadingSimulator()
|
|
sim.reset()
|
|
with pytest.raises(ValueError):
|
|
sim.step(dt=0.0, rudder_deg=5.0)
|
|
|
|
|
|
# ----------------------------------------------------------------------------
|
|
# heading_error_deg
|
|
# ----------------------------------------------------------------------------
|
|
|
|
|
|
@pytest.mark.parametrize("sp, meas, expected", [
|
|
(90.0, 80.0, 10.0),
|
|
(80.0, 90.0, -10.0),
|
|
(0.0, 0.0, 0.0),
|
|
(0.0, 359.0, 1.0), # crossing 0 going stbd
|
|
(359.0, 0.0, -1.0),
|
|
(180.0, 0.0, 180.0),
|
|
(0.0, 180.0, 180.0), # ambiguity at 180 -- convention is +180
|
|
(10.0, 350.0, 20.0),
|
|
(350.0, 10.0, -20.0),
|
|
])
|
|
def test_heading_error_shortest_arc(sp: float, meas: float, expected: float) -> None:
|
|
assert heading_error_deg(sp, meas) == pytest.approx(expected, abs=1e-9)
|