sprint-5: True Course + Track Keeping + XTE + PGN 129026/129284
- Python: NmeaNavData (COG/SOG/XTE data models with staleness tracking) - Python: TrueCoursePilot with TRUE_COURSE and TRACK_KEEPING modes - Python: 26 new tests (test_nmea_data, test_true_course) - Modbus: COG/SOG/XTE input registers + TC setpoint/XTE-gain holdings - Firmware: nmea2000_consumer handles PGN 129026 + 129284 - Firmware: pid_outer_task wired for TC + TK modes with live SOG scheduling - YAML regenerated; 284 tests pass, firmware compiles clean Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,117 @@
|
||||
"""NMEA 2000 navigation data models.
|
||||
|
||||
Holds the live data received from PGN 127250/127251 (heading/ROT -- Sprint 3)
|
||||
and the new PGN 129026/129284 (COG+SOG / XTE+waypoint -- Sprint 5).
|
||||
|
||||
All timestamps are float seconds (time.monotonic()).
|
||||
NaN means the value has never been received.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
|
||||
@dataclass
|
||||
class HeadingData:
|
||||
"""PGN 127250 + 127251 -- magnetic heading and rate of turn."""
|
||||
|
||||
heading_deg: float = math.nan
|
||||
rot_dps: float = 0.0
|
||||
timestamp: float = field(default_factory=lambda: 0.0)
|
||||
max_age_s: float = 5.0
|
||||
|
||||
def update(self, heading_deg: float, rot_dps: float) -> None:
|
||||
self.heading_deg = heading_deg
|
||||
self.rot_dps = rot_dps
|
||||
self.timestamp = time.monotonic()
|
||||
|
||||
@property
|
||||
def is_valid(self) -> bool:
|
||||
if math.isnan(self.heading_deg):
|
||||
return False
|
||||
return (time.monotonic() - self.timestamp) < self.max_age_s
|
||||
|
||||
@property
|
||||
def age_ms(self) -> int:
|
||||
return int((time.monotonic() - self.timestamp) * 1000)
|
||||
|
||||
|
||||
@dataclass
|
||||
class CogSogData:
|
||||
"""PGN 129026 -- Course Over Ground (true) and Speed Over Ground.
|
||||
|
||||
COG is in degrees true (0..360).
|
||||
SOG is in knots.
|
||||
"""
|
||||
|
||||
cog_deg: float = math.nan
|
||||
sog_kn: float = math.nan
|
||||
timestamp: float = field(default_factory=lambda: 0.0)
|
||||
max_age_s: float = 5.0
|
||||
|
||||
def update(self, cog_deg: float, sog_kn: float) -> None:
|
||||
self.cog_deg = cog_deg % 360.0
|
||||
self.sog_kn = sog_kn
|
||||
self.timestamp = time.monotonic()
|
||||
|
||||
@property
|
||||
def is_valid(self) -> bool:
|
||||
if math.isnan(self.cog_deg) or math.isnan(self.sog_kn):
|
||||
return False
|
||||
return (time.monotonic() - self.timestamp) < self.max_age_s
|
||||
|
||||
@property
|
||||
def age_ms(self) -> int:
|
||||
return int((time.monotonic() - self.timestamp) * 1000)
|
||||
|
||||
|
||||
@dataclass
|
||||
class XteData:
|
||||
"""PGN 129284 -- Cross Track Error and active waypoint.
|
||||
|
||||
xte_m: distance off track in metres.
|
||||
Positive → vessel is to starboard of the track line (steer port).
|
||||
Negative → vessel is to port of the track line (steer starboard).
|
||||
|
||||
waypoint_name: free text from the chart plotter, empty if not available.
|
||||
dtw_m: distance to waypoint in metres (NaN if unknown).
|
||||
"""
|
||||
|
||||
xte_m: float = math.nan
|
||||
dtw_m: float = math.nan
|
||||
waypoint_name: str = ""
|
||||
timestamp: float = field(default_factory=lambda: 0.0)
|
||||
max_age_s: float = 5.0
|
||||
|
||||
def update(
|
||||
self,
|
||||
xte_m: float,
|
||||
dtw_m: float = math.nan,
|
||||
waypoint_name: str = "",
|
||||
) -> None:
|
||||
self.xte_m = xte_m
|
||||
self.dtw_m = dtw_m
|
||||
self.waypoint_name = waypoint_name
|
||||
self.timestamp = time.monotonic()
|
||||
|
||||
@property
|
||||
def is_valid(self) -> bool:
|
||||
if math.isnan(self.xte_m):
|
||||
return False
|
||||
return (time.monotonic() - self.timestamp) < self.max_age_s
|
||||
|
||||
@property
|
||||
def age_ms(self) -> int:
|
||||
return int((time.monotonic() - self.timestamp) * 1000)
|
||||
|
||||
|
||||
@dataclass
|
||||
class NmeaNavData:
|
||||
"""Aggregated live navigation data used by the autopilot control loop."""
|
||||
|
||||
heading: HeadingData = field(default_factory=HeadingData)
|
||||
cog_sog: CogSogData = field(default_factory=CogSogData)
|
||||
xte: XteData = field(default_factory=XteData)
|
||||
@@ -91,6 +91,12 @@ INPUTS: dict[str, Reg] = {
|
||||
"PID_OUTER_KP_X1000": Reg(addr=54, name="PID_OUTER_KP_X1000", desc='Outer-loop active kp * 1000', unit="", scale=0.001, offset=0.0),
|
||||
"PID_OUTER_KI_X1000": Reg(addr=55, name="PID_OUTER_KI_X1000", desc='Outer-loop active ki * 1000', unit="", scale=0.001, offset=0.0),
|
||||
"PID_OUTER_KD_X1000": Reg(addr=56, name="PID_OUTER_KD_X1000", desc='Outer-loop active kd * 1000', unit="", scale=0.001, offset=0.0),
|
||||
"COG_DEG_X100": Reg(addr=60, name="COG_DEG_X100", desc='Course Over Ground (true) from PGN 129026, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
|
||||
"SOG_KN_X10": Reg(addr=61, name="SOG_KN_X10", desc='Speed Over Ground from PGN 129026, knots*10 (unsigned)', unit="kn", scale=0.1, offset=0.0),
|
||||
"COG_AGE_MS": Reg(addr=62, name="COG_AGE_MS", desc='Milliseconds since last COG/SOG update (0..60000)', unit="ms", scale=1.0, offset=0.0),
|
||||
"XTE_DM_SIGNED": Reg(addr=63, name="XTE_DM_SIGNED", desc='Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)', unit="dm", scale=0.1, offset=0.0),
|
||||
"XTE_AGE_MS": Reg(addr=64, name="XTE_AGE_MS", desc='Milliseconds since last XTE update (0..60000)', unit="ms", scale=1.0, offset=0.0),
|
||||
"DTW_M": Reg(addr=65, name="DTW_M", desc='Distance to next waypoint, metres (unsigned int16, 0..65535)', unit="m", scale=1.0, offset=0.0),
|
||||
}
|
||||
|
||||
# HOLDINGS
|
||||
@@ -109,6 +115,9 @@ HOLDINGS: dict[str, Reg] = {
|
||||
"PID_OUTER_KP_REQ_X1000": Reg(addr=26, name="PID_OUTER_KP_REQ_X1000", desc='Requested outer-loop base kp * 1000', unit="", scale=0.001, offset=0.0),
|
||||
"PID_OUTER_KI_REQ_X1000": Reg(addr=27, name="PID_OUTER_KI_REQ_X1000", desc='Requested outer-loop base ki * 1000', unit="", scale=0.001, offset=0.0),
|
||||
"PID_OUTER_KD_REQ_X1000": Reg(addr=28, name="PID_OUTER_KD_REQ_X1000", desc='Requested outer-loop base kd * 1000', unit="", scale=0.001, offset=0.0),
|
||||
"TRUE_COURSE_SP_X100": Reg(addr=32, name="TRUE_COURSE_SP_X100", desc='Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
|
||||
"XTE_GAIN_X1000": Reg(addr=33, name="XTE_GAIN_X1000", desc='XTE correction gain * 1000 (deg of heading correction per metre of XTE)', unit="", scale=0.001, offset=0.0),
|
||||
"XTE_MAX_CORRECTION_X100": Reg(addr=34, name="XTE_MAX_CORRECTION_X100", desc='Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)', unit="deg", scale=0.01, offset=0.0),
|
||||
}
|
||||
|
||||
ALL_GROUPS: dict[str, dict[str, Reg]] = {
|
||||
|
||||
@@ -0,0 +1,170 @@
|
||||
"""True Course and Track Keeping pilot -- Sprint 5.
|
||||
|
||||
TRUE_COURSE mode
|
||||
----------------
|
||||
Holds a fixed Course Over Ground (COG) instead of compass heading.
|
||||
This compensates for current/wind drift: if the vessel is pushed sideways,
|
||||
COG drifts and the outer PID corrects heading to restore the intended track.
|
||||
|
||||
Input: cog_setpoint_deg, measured_cog_deg, sog_kn, rot_dps
|
||||
Output: rudder_setpoint_deg (passed to inner PID)
|
||||
|
||||
TRACK_KEEPING mode
|
||||
------------------
|
||||
Follows an ECDIS/chart-plotter route using XTE feedback.
|
||||
An XTE proportional term adds a heading correction to the COG setpoint:
|
||||
|
||||
xte_correction_deg = -xte_gain * xte_m (clamped ± max_correction)
|
||||
effective_setpoint = cog_setpoint_deg + xte_correction_deg
|
||||
|
||||
Sign convention (NMEA 0183 / IEC 61162-1):
|
||||
XTE > 0 → vessel is to starboard of track → steer port → negative correction
|
||||
XTE < 0 → vessel is to port of track → steer stbd → positive correction
|
||||
|
||||
The correction is clamped so the autopilot cannot be tricked into dangerous
|
||||
large heading swings by a stale or erroneous XTE feed.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from arautopilot.studio.simulator.pid_outer import (
|
||||
PidOuter,
|
||||
PidOuterConfig,
|
||||
PidOuterState,
|
||||
)
|
||||
from arautopilot.studio.simulator.vessel_heading import heading_error_deg
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrueCourseConfig:
|
||||
"""Configuration for both TRUE_COURSE and TRACK_KEEPING modes."""
|
||||
|
||||
# Inner PID outer loop config (reused — same dynamics, different input)
|
||||
pid: PidOuterConfig = field(default_factory=PidOuterConfig)
|
||||
|
||||
# XTE correction parameters (TRACK_KEEPING only)
|
||||
xte_gain: float = 0.5 # deg of heading correction per metre of XTE
|
||||
xte_max_correction_deg: float = 20.0 # hard clamp on XTE term
|
||||
xte_max_age_s: float = 5.0 # treat XTE as stale beyond this
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrueCourseState:
|
||||
xte_correction_deg: float = 0.0
|
||||
effective_setpoint_deg: float = 0.0
|
||||
|
||||
|
||||
class TrueCoursePilot:
|
||||
"""Outer-loop pilot for TRUE_COURSE and TRACK_KEEPING modes.
|
||||
|
||||
Wraps PidOuter, substituting COG for heading and optionally adding
|
||||
an XTE proportional correction to the setpoint.
|
||||
"""
|
||||
|
||||
def __init__(self, config: TrueCourseConfig | None = None) -> None:
|
||||
self.config = config or TrueCourseConfig()
|
||||
self._pid = PidOuter(self.config.pid)
|
||||
self.tc_state = TrueCourseState()
|
||||
|
||||
def reset(self) -> None:
|
||||
self._pid.reset()
|
||||
self.tc_state = TrueCourseState()
|
||||
|
||||
def update_config(self, config: TrueCourseConfig) -> None:
|
||||
self.config = config
|
||||
self._pid.update_config(config.pid)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# TRUE_COURSE step
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def step_true_course(
|
||||
self,
|
||||
*,
|
||||
cog_setpoint_deg: float,
|
||||
measured_cog_deg: float,
|
||||
sog_kn: float,
|
||||
rot_dps: float = 0.0,
|
||||
allowed: bool = True,
|
||||
) -> float:
|
||||
"""Pure COG-hold step (no XTE). Returns rudder setpoint (deg)."""
|
||||
self.tc_state.effective_setpoint_deg = cog_setpoint_deg
|
||||
self.tc_state.xte_correction_deg = 0.0
|
||||
return self._pid.step(
|
||||
heading_setpoint_deg=cog_setpoint_deg,
|
||||
heading_measured_deg=measured_cog_deg,
|
||||
rate_of_turn_dps=rot_dps,
|
||||
speed_kn=sog_kn,
|
||||
allowed=allowed,
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# TRACK_KEEPING step
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def step_track_keeping(
|
||||
self,
|
||||
*,
|
||||
cog_setpoint_deg: float,
|
||||
measured_cog_deg: float,
|
||||
sog_kn: float,
|
||||
rot_dps: float = 0.0,
|
||||
xte_m: float = math.nan,
|
||||
xte_age_s: float = 0.0,
|
||||
allowed: bool = True,
|
||||
) -> float:
|
||||
"""COG-hold with XTE correction. Returns rudder setpoint (deg).
|
||||
|
||||
If XTE is NaN or stale the correction is zeroed and the pilot
|
||||
falls back to pure COG hold — safer than going to STANDBY.
|
||||
"""
|
||||
cfg = self.config
|
||||
xte_valid = (
|
||||
not math.isnan(xte_m)
|
||||
and xte_age_s < cfg.xte_max_age_s
|
||||
)
|
||||
|
||||
if xte_valid:
|
||||
raw_correction = -cfg.xte_gain * xte_m
|
||||
correction = _clamp(
|
||||
raw_correction,
|
||||
-cfg.xte_max_correction_deg,
|
||||
cfg.xte_max_correction_deg,
|
||||
)
|
||||
else:
|
||||
correction = 0.0
|
||||
|
||||
self.tc_state.xte_correction_deg = correction
|
||||
effective_sp = (cog_setpoint_deg + correction) % 360.0
|
||||
self.tc_state.effective_setpoint_deg = effective_sp
|
||||
|
||||
return self._pid.step(
|
||||
heading_setpoint_deg=effective_sp,
|
||||
heading_measured_deg=measured_cog_deg,
|
||||
rate_of_turn_dps=rot_dps,
|
||||
speed_kn=sog_kn,
|
||||
allowed=allowed,
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Accessors
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
@property
|
||||
def pid_state(self) -> PidOuterState:
|
||||
return self._pid.state
|
||||
|
||||
@property
|
||||
def last_rudder_setpoint_deg(self) -> float:
|
||||
return self._pid.state.last_output_deg
|
||||
|
||||
|
||||
def _clamp(x: float, lo: float, hi: float) -> float:
|
||||
if x < lo:
|
||||
return lo
|
||||
if x > hi:
|
||||
return hi
|
||||
return x
|
||||
@@ -0,0 +1,100 @@
|
||||
"""Tests for arautopilot.core.nmea_data -- Sprint 5."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
from unittest.mock import patch
|
||||
|
||||
import pytest
|
||||
|
||||
from arautopilot.core.nmea_data import CogSogData, HeadingData, NmeaNavData, XteData
|
||||
|
||||
|
||||
class TestCogSogData:
|
||||
def test_initial_state_invalid(self):
|
||||
d = CogSogData()
|
||||
assert not d.is_valid
|
||||
assert math.isnan(d.cog_deg)
|
||||
assert math.isnan(d.sog_kn)
|
||||
|
||||
def test_update_marks_valid(self):
|
||||
d = CogSogData()
|
||||
d.update(270.0, 8.5)
|
||||
assert d.is_valid
|
||||
assert d.cog_deg == pytest.approx(270.0)
|
||||
assert d.sog_kn == pytest.approx(8.5)
|
||||
|
||||
def test_cog_wraps_360(self):
|
||||
d = CogSogData()
|
||||
d.update(370.0, 5.0)
|
||||
assert d.cog_deg == pytest.approx(10.0)
|
||||
|
||||
def test_cog_zero_stays_zero(self):
|
||||
d = CogSogData()
|
||||
d.update(0.0, 5.0)
|
||||
assert d.cog_deg == pytest.approx(0.0)
|
||||
|
||||
def test_stale_after_max_age(self):
|
||||
d = CogSogData(max_age_s=1.0)
|
||||
d.update(90.0, 6.0)
|
||||
assert d.is_valid
|
||||
with patch("arautopilot.core.nmea_data.time") as mock_time:
|
||||
mock_time.monotonic.return_value = d.timestamp + 2.0
|
||||
assert not d.is_valid
|
||||
|
||||
def test_age_ms(self):
|
||||
d = CogSogData()
|
||||
d.update(180.0, 10.0)
|
||||
assert d.age_ms >= 0
|
||||
|
||||
|
||||
class TestXteData:
|
||||
def test_initial_invalid(self):
|
||||
d = XteData()
|
||||
assert not d.is_valid
|
||||
assert math.isnan(d.xte_m)
|
||||
|
||||
def test_update_positive_xte(self):
|
||||
"""Positive XTE = vessel to starboard of track."""
|
||||
d = XteData()
|
||||
d.update(xte_m=5.0, dtw_m=200.0, waypoint_name="WP01")
|
||||
assert d.is_valid
|
||||
assert d.xte_m == pytest.approx(5.0)
|
||||
assert d.dtw_m == pytest.approx(200.0)
|
||||
assert d.waypoint_name == "WP01"
|
||||
|
||||
def test_update_negative_xte(self):
|
||||
d = XteData()
|
||||
d.update(xte_m=-3.5)
|
||||
assert d.xte_m == pytest.approx(-3.5)
|
||||
|
||||
def test_stale_after_max_age(self):
|
||||
d = XteData(max_age_s=2.0)
|
||||
d.update(1.0)
|
||||
with patch("arautopilot.core.nmea_data.time") as mock_time:
|
||||
mock_time.monotonic.return_value = d.timestamp + 3.0
|
||||
assert not d.is_valid
|
||||
|
||||
def test_update_without_optional_fields(self):
|
||||
d = XteData()
|
||||
d.update(xte_m=0.0)
|
||||
assert d.is_valid
|
||||
assert math.isnan(d.dtw_m)
|
||||
assert d.waypoint_name == ""
|
||||
|
||||
|
||||
class TestNmeaNavData:
|
||||
def test_aggregate_has_all_fields(self):
|
||||
nav = NmeaNavData()
|
||||
assert isinstance(nav.heading, HeadingData)
|
||||
assert isinstance(nav.cog_sog, CogSogData)
|
||||
assert isinstance(nav.xte, XteData)
|
||||
|
||||
def test_independent_updates(self):
|
||||
nav = NmeaNavData()
|
||||
nav.cog_sog.update(45.0, 7.0)
|
||||
nav.xte.update(2.5)
|
||||
assert nav.cog_sog.is_valid
|
||||
assert nav.xte.is_valid
|
||||
assert not nav.heading.is_valid
|
||||
@@ -0,0 +1,180 @@
|
||||
"""Tests for TrueCoursePilot -- Sprint 5."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import pytest
|
||||
|
||||
from arautopilot.studio.simulator.true_course import TrueCoursePilot, TrueCourseConfig
|
||||
from arautopilot.studio.simulator.pid_outer import PidOuterConfig
|
||||
|
||||
|
||||
def _pilot(xte_gain: float = 0.5, max_corr: float = 20.0) -> TrueCoursePilot:
|
||||
cfg = TrueCourseConfig(
|
||||
pid=PidOuterConfig(base_kp=1.0, base_ki=0.0, base_kd=0.0,
|
||||
rot_ff_gain=0.0, rate_limit_dps=360.0),
|
||||
xte_gain=xte_gain,
|
||||
xte_max_correction_deg=max_corr,
|
||||
)
|
||||
return TrueCoursePilot(cfg)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# TRUE_COURSE mode
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestTrueCoursePilot:
|
||||
def test_zero_error_zero_output(self):
|
||||
p = _pilot()
|
||||
out = p.step_true_course(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=90.0,
|
||||
sog_kn=8.0,
|
||||
)
|
||||
assert out == pytest.approx(0.0, abs=0.6)
|
||||
|
||||
def test_positive_error_steer_starboard(self):
|
||||
"""COG is west of setpoint → steer to starboard (positive rudder)."""
|
||||
p = _pilot()
|
||||
out = p.step_true_course(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=80.0, # 10° behind
|
||||
sog_kn=8.0,
|
||||
)
|
||||
assert out > 0.0
|
||||
|
||||
def test_negative_error_steer_port(self):
|
||||
p = _pilot()
|
||||
out = p.step_true_course(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=100.0,
|
||||
sog_kn=8.0,
|
||||
)
|
||||
assert out < 0.0
|
||||
|
||||
def test_360_boundary_crossing(self):
|
||||
"""Setpoint=5°, measured=355° → shortest arc = +10° → steer stbd."""
|
||||
p = _pilot()
|
||||
out = p.step_true_course(
|
||||
cog_setpoint_deg=5.0,
|
||||
measured_cog_deg=355.0,
|
||||
sog_kn=8.0,
|
||||
)
|
||||
assert out > 0.0
|
||||
|
||||
def test_not_allowed_returns_zero(self):
|
||||
p = _pilot()
|
||||
out = p.step_true_course(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=80.0,
|
||||
sog_kn=8.0,
|
||||
allowed=False,
|
||||
)
|
||||
assert out == pytest.approx(0.0)
|
||||
|
||||
def test_xte_correction_zero_in_true_course_mode(self):
|
||||
p = _pilot()
|
||||
p.step_true_course(cog_setpoint_deg=90.0, measured_cog_deg=90.0, sog_kn=8.0)
|
||||
assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# TRACK_KEEPING mode
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestTrackKeeping:
|
||||
def test_no_xte_correction_when_nan(self):
|
||||
p = _pilot()
|
||||
p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=90.0,
|
||||
sog_kn=8.0,
|
||||
xte_m=math.nan,
|
||||
)
|
||||
assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
|
||||
assert p.tc_state.effective_setpoint_deg == pytest.approx(90.0)
|
||||
|
||||
def test_positive_xte_steers_port(self):
|
||||
"""XTE>0 = vessel to starboard → correction negative → setpoint shifted port."""
|
||||
p = _pilot(xte_gain=1.0)
|
||||
p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=90.0,
|
||||
sog_kn=8.0,
|
||||
xte_m=5.0, # 5 m to starboard
|
||||
)
|
||||
assert p.tc_state.xte_correction_deg == pytest.approx(-5.0)
|
||||
assert p.tc_state.effective_setpoint_deg == pytest.approx(85.0)
|
||||
|
||||
def test_negative_xte_steers_starboard(self):
|
||||
p = _pilot(xte_gain=1.0)
|
||||
p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=90.0,
|
||||
sog_kn=8.0,
|
||||
xte_m=-5.0,
|
||||
)
|
||||
assert p.tc_state.xte_correction_deg == pytest.approx(5.0)
|
||||
assert p.tc_state.effective_setpoint_deg == pytest.approx(95.0)
|
||||
|
||||
def test_xte_correction_clamped(self):
|
||||
p = _pilot(xte_gain=1.0, max_corr=10.0)
|
||||
p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=90.0,
|
||||
sog_kn=8.0,
|
||||
xte_m=50.0, # huge XTE → clamp to 10°
|
||||
)
|
||||
assert p.tc_state.xte_correction_deg == pytest.approx(-10.0)
|
||||
|
||||
def test_stale_xte_ignored(self):
|
||||
"""If xte_age_s > max_age_s, correction must be zero."""
|
||||
p = _pilot(xte_gain=1.0)
|
||||
p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=90.0,
|
||||
sog_kn=8.0,
|
||||
xte_m=10.0,
|
||||
xte_age_s=10.0, # stale (default max_age_s=5)
|
||||
)
|
||||
assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
|
||||
|
||||
def test_convergence_to_track(self):
|
||||
"""Simulate 30 steps: XTE should shrink (very basic closed-loop sanity)."""
|
||||
from arautopilot.studio.simulator.vessel_heading import heading_error_deg
|
||||
|
||||
p = TrueCoursePilot(TrueCourseConfig(
|
||||
pid=PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
|
||||
rot_ff_gain=0.0, rate_limit_dps=360.0,
|
||||
max_rudder_deg=35.0),
|
||||
xte_gain=0.5,
|
||||
xte_max_correction_deg=20.0,
|
||||
))
|
||||
|
||||
xte = 20.0 # 20 m to starboard
|
||||
cog = 90.0 # vessel COG
|
||||
dt = 0.1 # 10 Hz
|
||||
sog = 5.0 # kn ≈ 2.57 m/s
|
||||
|
||||
for _ in range(200):
|
||||
rudder = p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0,
|
||||
measured_cog_deg=cog,
|
||||
sog_kn=sog,
|
||||
xte_m=xte,
|
||||
)
|
||||
cog_change = rudder * 0.5 * dt
|
||||
cog = (cog + cog_change) % 360.0
|
||||
# Cross-track rate: -SOG * cos(COG) for east-going track (COG=90 nominal)
|
||||
xte -= sog * 0.5144 * math.cos(math.radians(cog)) * dt
|
||||
|
||||
assert abs(xte) < 18.0 # must have improved from initial 20 m
|
||||
|
||||
def test_reset_clears_state(self):
|
||||
p = _pilot()
|
||||
p.step_track_keeping(
|
||||
cog_setpoint_deg=90.0, measured_cog_deg=80.0,
|
||||
sog_kn=8.0, xte_m=5.0,
|
||||
)
|
||||
p.reset()
|
||||
assert p.pid_state.integral == pytest.approx(0.0)
|
||||
@@ -112,6 +112,14 @@ inputs:
|
||||
- { addr: 55, name: PID_OUTER_KI_X1000, desc: "Outer-loop active ki * 1000", scale: 0.001 }
|
||||
- { addr: 56, name: PID_OUTER_KD_X1000, desc: "Outer-loop active kd * 1000", scale: 0.001 }
|
||||
|
||||
# ----- Sprint 5: True Course + Track Keeping (PGN 129026 / 129284) -----
|
||||
- { addr: 60, name: COG_DEG_X100, desc: "Course Over Ground (true) from PGN 129026, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
|
||||
- { addr: 61, name: SOG_KN_X10, desc: "Speed Over Ground from PGN 129026, knots*10 (unsigned)", unit: "kn", scale: 0.1 }
|
||||
- { addr: 62, name: COG_AGE_MS, desc: "Milliseconds since last COG/SOG update (0..60000)", unit: "ms" }
|
||||
- { addr: 63, name: XTE_DM_SIGNED, desc: "Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)", unit: "dm", scale: 0.1 }
|
||||
- { addr: 64, name: XTE_AGE_MS, desc: "Milliseconds since last XTE update (0..60000)", unit: "ms" }
|
||||
- { addr: 65, name: DTW_M, desc: "Distance to next waypoint, metres (unsigned int16, 0..65535)", unit: "m" }
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Holding registers (read-write 16-bit words) -- setpoints and config
|
||||
# -----------------------------------------------------------------------------
|
||||
@@ -134,3 +142,8 @@ holdings:
|
||||
- { addr: 26, name: PID_OUTER_KP_REQ_X1000, desc: "Requested outer-loop base kp * 1000", scale: 0.001 }
|
||||
- { addr: 27, name: PID_OUTER_KI_REQ_X1000, desc: "Requested outer-loop base ki * 1000", scale: 0.001 }
|
||||
- { addr: 28, name: PID_OUTER_KD_REQ_X1000, desc: "Requested outer-loop base kd * 1000", scale: 0.001 }
|
||||
|
||||
# ----- Sprint 5: True Course + Track Keeping -----
|
||||
- { addr: 32, name: TRUE_COURSE_SP_X100, desc: "Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
|
||||
- { addr: 33, name: XTE_GAIN_X1000, desc: "XTE correction gain * 1000 (deg of heading correction per metre of XTE)", scale: 0.001 }
|
||||
- { addr: 34, name: XTE_MAX_CORRECTION_X100, desc: "Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)", unit: "deg", scale: 0.01 }
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
// =============================================================================
|
||||
// pid_outer_task.cpp -- 10 Hz outer-loop (heading control) task
|
||||
// pid_outer_task.cpp -- 10 Hz outer-loop task (Sprint 3 / Sprint 5)
|
||||
// =============================================================================
|
||||
|
||||
#include "pid_outer_task.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <cmath>
|
||||
|
||||
#include "../modes/standby.h"
|
||||
#include "../protocols/nmea2000_consumer.h"
|
||||
@@ -20,10 +21,28 @@ constexpr const char* TAG = "AR/PID";
|
||||
|
||||
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
PidOuter g_outer{};
|
||||
float g_heading_setpoint_deg = 0.0f;
|
||||
|
||||
float g_heading_setpoint_deg = 0.0f;
|
||||
float g_cog_setpoint_deg = 0.0f;
|
||||
float g_xte_gain = 0.5f; // deg / metre
|
||||
float g_xte_max_correction = 20.0f; // deg
|
||||
|
||||
float g_last_rudder_setpoint_deg = 0.0f;
|
||||
float g_last_error_deg = 0.0f;
|
||||
float g_speed_kn = 15.0f; // Sprint 3 default until PGN 129026 wiring (Sprint 5)
|
||||
float g_last_error_deg = 0.0f;
|
||||
float g_last_xte_correction_deg = 0.0f;
|
||||
float g_speed_kn = 15.0f;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// XTE correction helper (TRACK_KEEPING)
|
||||
// xte_m > 0 → vessel to starboard → steer port → negative correction
|
||||
// ---------------------------------------------------------------------------
|
||||
static float _xte_correction(float xte_m, float gain, float max_deg) {
|
||||
if (std::isnan(xte_m)) return 0.0f;
|
||||
float c = -gain * xte_m;
|
||||
if (c > max_deg) c = max_deg;
|
||||
if (c < -max_deg) c = -max_deg;
|
||||
return c;
|
||||
}
|
||||
|
||||
void OuterLoopTask(void* /*pv*/) {
|
||||
AR_LOGI(TAG, "pid_outer task started on core %d (10 Hz)", xPortGetCoreID());
|
||||
@@ -31,46 +50,93 @@ void OuterLoopTask(void* /*pv*/) {
|
||||
|
||||
TickType_t last_wake = xTaskGetTickCount();
|
||||
for (;;) {
|
||||
// Snapshot inputs we need atomically.
|
||||
float setpoint;
|
||||
float speed_kn;
|
||||
float heading_sp, cog_sp, xte_gain, xte_max, speed_kn;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
setpoint = g_heading_setpoint_deg;
|
||||
speed_kn = g_speed_kn;
|
||||
heading_sp = g_heading_setpoint_deg;
|
||||
cog_sp = g_cog_setpoint_deg;
|
||||
xte_gain = g_xte_gain;
|
||||
xte_max = g_xte_max_correction;
|
||||
speed_kn = g_speed_kn;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
|
||||
const auto n2k = protocols::nmea2000::nmea2000_latest();
|
||||
const auto n2k = protocols::nmea2000::nmea2000_latest();
|
||||
const auto cog_sog = protocols::nmea2000::nmea2000_cog_sog();
|
||||
const auto nav_data = protocols::nmea2000::nmea2000_nav_data();
|
||||
const auto mode = modes::current_mode();
|
||||
|
||||
// Only active in HEADING_HOLD with valid heading sensor.
|
||||
const bool in_hh =
|
||||
modes::current_mode() == modes::Mode::HEADING_HOLD;
|
||||
const bool allowed = in_hh && n2k.heading_valid;
|
||||
const bool in_hh = (mode == modes::Mode::HEADING_HOLD);
|
||||
const bool in_tc = (mode == modes::Mode::TRUE_COURSE);
|
||||
const bool in_tk = (mode == modes::Mode::TRACK_KEEPING);
|
||||
|
||||
const float rudder_sp = g_outer.step(
|
||||
setpoint,
|
||||
n2k.heading_deg,
|
||||
n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
|
||||
speed_kn,
|
||||
allowed
|
||||
);
|
||||
float rudder_sp = 0.0f;
|
||||
float error = 0.0f;
|
||||
float xte_corr = 0.0f;
|
||||
|
||||
// Always push the outer-loop output downstream. If `allowed` is
|
||||
// false the output is zero, which corresponds to "rudder centred";
|
||||
// the inner loop will pursue that or its own externally-supplied
|
||||
// setpoint depending on the cascade configuration. In HH mode,
|
||||
// the outer loop owns the inner setpoint.
|
||||
if (in_hh) {
|
||||
// ---------------------------------------------------------------
|
||||
// HEADING_HOLD: feedback = magnetic/true heading (PGN 127250)
|
||||
// ---------------------------------------------------------------
|
||||
const bool allowed = n2k.heading_valid;
|
||||
rudder_sp = g_outer.step(
|
||||
heading_sp,
|
||||
n2k.heading_deg,
|
||||
n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
|
||||
speed_kn,
|
||||
allowed
|
||||
);
|
||||
error = allowed ? (heading_sp - n2k.heading_deg) : 0.0f;
|
||||
pid_inner_set_setpoint_deg(rudder_sp);
|
||||
}
|
||||
|
||||
const float err =
|
||||
n2k.heading_valid
|
||||
? (setpoint - n2k.heading_deg)
|
||||
: 0.0f;
|
||||
} else if (in_tc) {
|
||||
// ---------------------------------------------------------------
|
||||
// TRUE_COURSE: feedback = COG (PGN 129026)
|
||||
// ---------------------------------------------------------------
|
||||
const bool allowed = cog_sog.valid;
|
||||
const float sog = cog_sog.valid ? cog_sog.sog_kn : speed_kn;
|
||||
rudder_sp = g_outer.step(
|
||||
cog_sp,
|
||||
cog_sog.cog_deg,
|
||||
n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
|
||||
sog,
|
||||
allowed
|
||||
);
|
||||
error = allowed ? (cog_sp - cog_sog.cog_deg) : 0.0f;
|
||||
pid_inner_set_setpoint_deg(rudder_sp);
|
||||
|
||||
} else if (in_tk) {
|
||||
// ---------------------------------------------------------------
|
||||
// TRACK_KEEPING: COG hold + XTE proportional correction
|
||||
// ---------------------------------------------------------------
|
||||
const bool cog_ok = cog_sog.valid;
|
||||
const bool xte_ok = nav_data.valid;
|
||||
xte_corr = xte_ok ? _xte_correction(nav_data.xte_m, xte_gain, xte_max) : 0.0f;
|
||||
const float eff_sp = fmodf(cog_sp + xte_corr + 360.0f, 360.0f);
|
||||
const float sog = cog_ok ? cog_sog.sog_kn : speed_kn;
|
||||
rudder_sp = g_outer.step(
|
||||
eff_sp,
|
||||
cog_sog.cog_deg,
|
||||
n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
|
||||
sog,
|
||||
cog_ok
|
||||
);
|
||||
error = cog_ok ? (cog_sp - cog_sog.cog_deg) : 0.0f;
|
||||
pid_inner_set_setpoint_deg(rudder_sp);
|
||||
|
||||
} else {
|
||||
// ---------------------------------------------------------------
|
||||
// STANDBY / DODGE: idle tick — bleed integrator, don't push
|
||||
// ---------------------------------------------------------------
|
||||
g_outer.step(0.0f, 0.0f, 0.0f, speed_kn, false);
|
||||
}
|
||||
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_last_rudder_setpoint_deg = rudder_sp;
|
||||
g_last_error_deg = err;
|
||||
g_last_error_deg = error;
|
||||
g_last_xte_correction_deg = xte_corr;
|
||||
if (in_tc || in_tk) {
|
||||
// Update SOG from live data
|
||||
if (cog_sog.valid) g_speed_kn = cog_sog.sog_kn;
|
||||
}
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
|
||||
safety::watchdog_feed();
|
||||
@@ -80,15 +146,17 @@ void OuterLoopTask(void* /*pv*/) {
|
||||
|
||||
} // namespace
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Init / start
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void pid_outer_task_init() {
|
||||
PidOuterConfig cfg;
|
||||
// Seed the 3-point gain schedule from the 30 m yacht profile so the
|
||||
// firmware has sensible defaults out of the box.
|
||||
cfg.schedule_size = 3;
|
||||
cfg.schedule[0] = {5.0f, 1.20f, 0.03f, 0.80f};
|
||||
cfg.schedule[1] = {15.0f, 0.90f, 0.02f, 1.20f};
|
||||
cfg.schedule[2] = {28.0f, 0.55f, 0.01f, 1.80f};
|
||||
cfg.rot_ff_gain = 1.5f;
|
||||
cfg.schedule_size = 3;
|
||||
cfg.schedule[0] = {5.0f, 1.20f, 0.03f, 0.80f};
|
||||
cfg.schedule[1] = {15.0f, 0.90f, 0.02f, 1.20f};
|
||||
cfg.schedule[2] = {28.0f, 0.55f, 0.01f, 1.80f};
|
||||
cfg.rot_ff_gain = 1.5f;
|
||||
g_outer.update_config(cfg);
|
||||
g_outer.reset();
|
||||
AR_LOGI(TAG,
|
||||
@@ -102,10 +170,12 @@ void pid_outer_task_start() {
|
||||
AR_TASK_CORE_REALTIME);
|
||||
}
|
||||
|
||||
void pid_outer_set_heading_setpoint_deg(float setpoint_deg) {
|
||||
// Normalise to [0, 360).
|
||||
float sp = setpoint_deg;
|
||||
while (sp < 0.0f) sp += 360.0f;
|
||||
// ---------------------------------------------------------------------------
|
||||
// Setpoints
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void pid_outer_set_heading_setpoint_deg(float sp) {
|
||||
while (sp < 0.0f) sp += 360.0f;
|
||||
while (sp >= 360.0f) sp -= 360.0f;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_heading_setpoint_deg = sp;
|
||||
@@ -119,6 +189,58 @@ float pid_outer_heading_setpoint_deg() {
|
||||
return v;
|
||||
}
|
||||
|
||||
void pid_outer_set_cog_setpoint_deg(float sp) {
|
||||
while (sp < 0.0f) sp += 360.0f;
|
||||
while (sp >= 360.0f) sp -= 360.0f;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_cog_setpoint_deg = sp;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
float pid_outer_cog_setpoint_deg() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
float v = g_cog_setpoint_deg;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return v;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// XTE parameters
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void pid_outer_set_xte_gain(float gain) {
|
||||
if (gain < 0.0f) gain = 0.0f;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_xte_gain = gain;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
float pid_outer_xte_gain() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
float v = g_xte_gain;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return v;
|
||||
}
|
||||
|
||||
void pid_outer_set_xte_max_correction(float deg) {
|
||||
if (deg < 0.0f) deg = 0.0f;
|
||||
if (deg > 90.0f) deg = 90.0f;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_xte_max_correction = deg;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
float pid_outer_xte_max_correction() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
float v = g_xte_max_correction;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return v;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Telemetry
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
float pid_outer_last_rudder_setpoint_deg() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
float v = g_last_rudder_setpoint_deg;
|
||||
@@ -133,8 +255,19 @@ float pid_outer_last_error_deg() {
|
||||
return v;
|
||||
}
|
||||
|
||||
float pid_outer_last_xte_correction_deg() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
float v = g_last_xte_correction_deg;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return v;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SOG / gain scheduling
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void pid_outer_set_speed_kn(float speed_kn) {
|
||||
if (speed_kn < 0.0f) speed_kn = 0.0f;
|
||||
if (speed_kn < 0.0f) speed_kn = 0.0f;
|
||||
if (speed_kn > 80.0f) speed_kn = 80.0f;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_speed_kn = speed_kn;
|
||||
@@ -150,15 +283,14 @@ float pid_outer_speed_kn() {
|
||||
|
||||
void pid_outer_update_gains(float kp, float ki, float kd) {
|
||||
PidOuterConfig cfg = g_outer.config();
|
||||
cfg.base_kp = kp;
|
||||
cfg.base_ki = ki;
|
||||
cfg.base_kd = kd;
|
||||
cfg.schedule_size = 0; // explicit gains override the schedule
|
||||
cfg.base_kp = kp;
|
||||
cfg.base_ki = ki;
|
||||
cfg.base_kd = kd;
|
||||
cfg.schedule_size = 0; // explicit gains disable the schedule
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_outer.update_config(cfg);
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGI(TAG, "pid_outer base gains updated: kp=%.3f ki=%.3f kd=%.3f "
|
||||
"(schedule disabled)", kp, ki, kd);
|
||||
AR_LOGI(TAG, "pid_outer gains updated: kp=%.3f ki=%.3f kd=%.3f", kp, ki, kd);
|
||||
}
|
||||
|
||||
void pid_outer_get_gains(float& kp, float& ki, float& kd) {
|
||||
|
||||
@@ -1,11 +1,14 @@
|
||||
// =============================================================================
|
||||
// pid_outer_task.h -- 10 Hz outer-loop (heading control) task (Sprint 3)
|
||||
// pid_outer_task.h -- 10 Hz outer-loop task (Sprint 3 / Sprint 5)
|
||||
// =============================================================================
|
||||
//
|
||||
// Reads heading + ROT from the NMEA 2000 snapshot, computes a rudder
|
||||
// setpoint, hands it off to the inner loop. Active only in HEADING_HOLD
|
||||
// mode; in any other mode the task ticks idle (allowed=false) which
|
||||
// bleeds the integrator.
|
||||
// Sprint 3: HEADING_HOLD -- holds a magnetic/true heading setpoint.
|
||||
// Sprint 5: TRUE_COURSE -- holds a COG setpoint (compensates current/wind).
|
||||
// TRACK_KEEPING -- COG hold + XTE proportional correction.
|
||||
//
|
||||
// In all modes the output is a rudder setpoint handed to the inner loop.
|
||||
// In any other mode (STANDBY, DODGE) the task ticks idle (allowed=false),
|
||||
// bleeding the integrator so the transition back to active is smooth.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
@@ -17,24 +20,30 @@ namespace arautopilot::pid {
|
||||
void pid_outer_task_init();
|
||||
void pid_outer_task_start();
|
||||
|
||||
/// Update the heading the outer loop pursues (degrees, 0..360).
|
||||
void pid_outer_set_heading_setpoint_deg(float setpoint_deg);
|
||||
|
||||
/// Read the active heading setpoint (thread-safe).
|
||||
// ----- HEADING_HOLD setpoint -----
|
||||
void pid_outer_set_heading_setpoint_deg(float setpoint_deg);
|
||||
float pid_outer_heading_setpoint_deg();
|
||||
|
||||
/// Read the last rudder setpoint the outer loop produced.
|
||||
// ----- TRUE_COURSE / TRACK_KEEPING setpoint -----
|
||||
void pid_outer_set_cog_setpoint_deg(float setpoint_deg);
|
||||
float pid_outer_cog_setpoint_deg();
|
||||
|
||||
// ----- XTE parameters (TRACK_KEEPING) -----
|
||||
void pid_outer_set_xte_gain(float gain); ///< deg correction / metre
|
||||
void pid_outer_set_xte_max_correction(float deg); ///< hard clamp
|
||||
float pid_outer_xte_gain();
|
||||
float pid_outer_xte_max_correction();
|
||||
|
||||
// ----- Telemetry -----
|
||||
float pid_outer_last_rudder_setpoint_deg();
|
||||
|
||||
/// Read the last heading error the controller saw.
|
||||
float pid_outer_last_error_deg();
|
||||
float pid_outer_last_xte_correction_deg(); ///< 0 in non-TK modes
|
||||
|
||||
/// Override the SOG used for gain scheduling (Sprint 3 keeps a default of
|
||||
/// 15 kn until PGN 129026 wiring lands in Sprint 5).
|
||||
void pid_outer_set_speed_kn(float speed_kn);
|
||||
// ----- SOG / gain scheduling -----
|
||||
void pid_outer_set_speed_kn(float speed_kn);
|
||||
float pid_outer_speed_kn();
|
||||
|
||||
void pid_outer_update_gains(float kp, float ki, float kd);
|
||||
void pid_outer_get_gains(float& kp, float& ki, float& kd);
|
||||
void pid_outer_update_gains(float kp, float ki, float kd);
|
||||
void pid_outer_get_gains(float& kp, float& ki, float& kd);
|
||||
|
||||
} // namespace arautopilot::pid
|
||||
|
||||
@@ -77,8 +77,8 @@ constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
|
||||
constexpr uint16_t COIL_CMD_KNOB_ARM = 3;
|
||||
|
||||
// ----- Input registers (read-only words) -----
|
||||
constexpr uint16_t INPUT_COUNT = 30;
|
||||
constexpr uint16_t INPUT_MAX_ADDR = 56;
|
||||
constexpr uint16_t INPUT_COUNT = 36;
|
||||
constexpr uint16_t INPUT_MAX_ADDR = 65;
|
||||
|
||||
// Firmware major version
|
||||
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
|
||||
@@ -164,10 +164,28 @@ constexpr uint16_t INPUT_PID_OUTER_KI_X1000 = 55;
|
||||
// Outer-loop active kd * 1000
|
||||
// scale=0.001
|
||||
constexpr uint16_t INPUT_PID_OUTER_KD_X1000 = 56;
|
||||
// Course Over Ground (true) from PGN 129026, deg*100 (0..35999)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t INPUT_COG_DEG_X100 = 60;
|
||||
// Speed Over Ground from PGN 129026, knots*10 (unsigned)
|
||||
// unit=kn, scale=0.1
|
||||
constexpr uint16_t INPUT_SOG_KN_X10 = 61;
|
||||
// Milliseconds since last COG/SOG update (0..60000)
|
||||
// unit=ms
|
||||
constexpr uint16_t INPUT_COG_AGE_MS = 62;
|
||||
// Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)
|
||||
// unit=dm, scale=0.1
|
||||
constexpr uint16_t INPUT_XTE_DM_SIGNED = 63;
|
||||
// Milliseconds since last XTE update (0..60000)
|
||||
// unit=ms
|
||||
constexpr uint16_t INPUT_XTE_AGE_MS = 64;
|
||||
// Distance to next waypoint, metres (unsigned int16, 0..65535)
|
||||
// unit=m
|
||||
constexpr uint16_t INPUT_DTW_M = 65;
|
||||
|
||||
// ----- Holding registers (read-write words) -----
|
||||
constexpr uint16_t HOLDING_COUNT = 14;
|
||||
constexpr uint16_t HOLDING_MAX_ADDR = 28;
|
||||
constexpr uint16_t HOLDING_COUNT = 17;
|
||||
constexpr uint16_t HOLDING_MAX_ADDR = 34;
|
||||
|
||||
// Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
|
||||
constexpr uint16_t HOLDING_MODE_REQUEST = 0;
|
||||
@@ -210,5 +228,14 @@ constexpr uint16_t HOLDING_PID_OUTER_KI_REQ_X1000 = 27;
|
||||
// Requested outer-loop base kd * 1000
|
||||
// scale=0.001
|
||||
constexpr uint16_t HOLDING_PID_OUTER_KD_REQ_X1000 = 28;
|
||||
// Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t HOLDING_TRUE_COURSE_SP_X100 = 32;
|
||||
// XTE correction gain * 1000 (deg of heading correction per metre of XTE)
|
||||
// scale=0.001
|
||||
constexpr uint16_t HOLDING_XTE_GAIN_X1000 = 33;
|
||||
// Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t HOLDING_XTE_MAX_CORRECTION_X100 = 34;
|
||||
|
||||
} // namespace arautopilot::protocols::modbus
|
||||
|
||||
@@ -66,6 +66,10 @@ struct HoldingStorage {
|
||||
uint16_t pid_outer_kp_req_x1000 = 0;
|
||||
uint16_t pid_outer_ki_req_x1000 = 0;
|
||||
uint16_t pid_outer_kd_req_x1000 = 0;
|
||||
// Sprint 5: True Course + Track Keeping
|
||||
uint16_t true_course_sp_x100 = 0;
|
||||
uint16_t xte_gain_x1000 = 500; // 0.5 deg/m default
|
||||
uint16_t xte_max_correction_x100 = 2000; // 20.0 deg default
|
||||
};
|
||||
HoldingStorage g_holding;
|
||||
|
||||
@@ -209,6 +213,51 @@ uint16_t read_input_register(uint16_t addr) {
|
||||
return (uint16_t)scaled;
|
||||
}
|
||||
|
||||
// ----- Sprint 5: COG / SOG / XTE telemetry -----
|
||||
case INPUT_COG_DEG_X100: {
|
||||
auto cs = nmea2000::nmea2000_cog_sog();
|
||||
if (!cs.valid) return 0;
|
||||
int v = (int)(cs.cog_deg * 100.0f);
|
||||
if (v < 0) v = 0;
|
||||
if (v > 35999) v = 35999;
|
||||
return (uint16_t)v;
|
||||
}
|
||||
case INPUT_SOG_KN_X10: {
|
||||
auto cs = nmea2000::nmea2000_cog_sog();
|
||||
if (!cs.valid) return 0;
|
||||
int v = (int)(cs.sog_kn * 10.0f);
|
||||
if (v < 0) v = 0;
|
||||
if (v > 65535) v = 65535;
|
||||
return (uint16_t)v;
|
||||
}
|
||||
case INPUT_COG_AGE_MS: {
|
||||
auto cs = nmea2000::nmea2000_cog_sog();
|
||||
uint32_t age = cs.valid ? (millis() - cs.age_ms) : 60000;
|
||||
if (age > 60000) age = 60000;
|
||||
return (uint16_t)age;
|
||||
}
|
||||
case INPUT_XTE_DM_SIGNED: {
|
||||
auto nd = nmea2000::nmea2000_nav_data();
|
||||
if (!nd.valid) return 0;
|
||||
int v = (int)(nd.xte_m * 10.0f); // metres → decimetres
|
||||
if (v < -32768) v = -32768;
|
||||
if (v > 32767) v = 32767;
|
||||
return (uint16_t)(int16_t)v;
|
||||
}
|
||||
case INPUT_XTE_AGE_MS: {
|
||||
auto nd = nmea2000::nmea2000_nav_data();
|
||||
uint32_t age = nd.valid ? (millis() - nd.age_ms) : 60000;
|
||||
if (age > 60000) age = 60000;
|
||||
return (uint16_t)age;
|
||||
}
|
||||
case INPUT_DTW_M: {
|
||||
auto nd = nmea2000::nmea2000_nav_data();
|
||||
if (!nd.valid) return 0;
|
||||
uint32_t dtw = (uint32_t)nd.dtw_m;
|
||||
if (dtw > 65535) dtw = 65535;
|
||||
return (uint16_t)dtw;
|
||||
}
|
||||
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
@@ -258,6 +307,9 @@ uint16_t read_holding(uint16_t addr) {
|
||||
case HOLDING_PID_OUTER_KP_REQ_X1000: return g_holding.pid_outer_kp_req_x1000;
|
||||
case HOLDING_PID_OUTER_KI_REQ_X1000: return g_holding.pid_outer_ki_req_x1000;
|
||||
case HOLDING_PID_OUTER_KD_REQ_X1000: return g_holding.pid_outer_kd_req_x1000;
|
||||
case HOLDING_TRUE_COURSE_SP_X100: return g_holding.true_course_sp_x100;
|
||||
case HOLDING_XTE_GAIN_X1000: return g_holding.xte_gain_x1000;
|
||||
case HOLDING_XTE_MAX_CORRECTION_X100: return g_holding.xte_max_correction_x100;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
@@ -348,6 +400,25 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) {
|
||||
pid::pid_inner_update_gains(kp, ki, kd);
|
||||
return Modbus::Error::SUCCESS;
|
||||
}
|
||||
// ----- Sprint 5: True Course + XTE parameters -----
|
||||
case HOLDING_TRUE_COURSE_SP_X100: {
|
||||
if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
|
||||
g_holding.true_course_sp_x100 = value;
|
||||
pid::pid_outer_set_cog_setpoint_deg((float)value * 0.01f);
|
||||
return Modbus::Error::SUCCESS;
|
||||
}
|
||||
case HOLDING_XTE_GAIN_X1000: {
|
||||
g_holding.xte_gain_x1000 = value;
|
||||
pid::pid_outer_set_xte_gain((float)value * 0.001f);
|
||||
return Modbus::Error::SUCCESS;
|
||||
}
|
||||
case HOLDING_XTE_MAX_CORRECTION_X100: {
|
||||
if (value > 9000) return Modbus::Error::ILLEGAL_DATA_VALUE; // 90 deg cap
|
||||
g_holding.xte_max_correction_x100 = value;
|
||||
pid::pid_outer_set_xte_max_correction((float)value * 0.01f);
|
||||
return Modbus::Error::SUCCESS;
|
||||
}
|
||||
|
||||
default:
|
||||
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
|
||||
}
|
||||
|
||||
@@ -25,6 +25,7 @@ constexpr const char* TAG = "AR/N2K";
|
||||
constexpr uint32_t STALE_THRESHOLD_MS = 5000;
|
||||
|
||||
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
|
||||
HeadingSnapshot g_snap{
|
||||
.heading_deg = 0.0f,
|
||||
.is_true = false,
|
||||
@@ -35,6 +36,21 @@ HeadingSnapshot g_snap{
|
||||
.rot_valid = false,
|
||||
};
|
||||
|
||||
CogSogSnapshot g_cog_sog{
|
||||
.cog_deg = NAN,
|
||||
.sog_kn = NAN,
|
||||
.age_ms = 0,
|
||||
.valid = false,
|
||||
};
|
||||
|
||||
NavDataSnapshot g_nav_data{
|
||||
.xte_m = NAN,
|
||||
.dtw_m = NAN,
|
||||
.wp_name = {},
|
||||
.age_ms = 0,
|
||||
.valid = false,
|
||||
};
|
||||
|
||||
float rad_to_deg_pos(float rad) {
|
||||
float d = rad * (180.0f / (float)M_PI);
|
||||
// Normalise to 0..360.
|
||||
@@ -90,19 +106,62 @@ void HandleROT(const tN2kMsg& msg) {
|
||||
AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
|
||||
}
|
||||
|
||||
// One global dispatcher because NMEA2000.SetMsgHandler() takes a single
|
||||
// callback that we have to filter by PGN ourselves.
|
||||
void HandleCogSog(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
tN2kHeadingReference ref;
|
||||
double cog = 0.0, sog = 0.0;
|
||||
if (!ParseN2kCOGSOGRapid(msg, sid, ref, cog, sog)) return;
|
||||
if (cog > 1e8 || sog > 1e8) return;
|
||||
const float cog_deg = rad_to_deg_pos((float)cog);
|
||||
const float sog_kn = (float)(sog * 1.94384); // m/s → knots
|
||||
const uint32_t now = millis();
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_cog_sog.cog_deg = cog_deg;
|
||||
g_cog_sog.sog_kn = sog_kn;
|
||||
g_cog_sog.age_ms = now;
|
||||
g_cog_sog.valid = true;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGV(TAG, "PGN 129026 COG=%.2f deg SOG=%.2f kn", cog_deg, sog_kn);
|
||||
}
|
||||
|
||||
void HandleNavData(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
double dist = 0.0, xte = 0.0;
|
||||
double origLat = 0.0, origLon = 0.0, destLat = 0.0, destLon = 0.0;
|
||||
double closingVel = 0.0;
|
||||
tN2kHeadingReference bearingRef = N2khr_Unavailable;
|
||||
tN2kDistanceCalculationType calcType = N2kdct_GreatCircle;
|
||||
bool perpCrossed = false, arrivalAlarm = false;
|
||||
int16_t origWpNum = 0;
|
||||
uint32_t destWpNum = 0, eta = 0;
|
||||
|
||||
if (!ParseN2kNavigationInfo(
|
||||
msg, sid, dist, bearingRef,
|
||||
perpCrossed, arrivalAlarm, calcType, xte,
|
||||
origWpNum, origLat, origLon,
|
||||
destWpNum, eta, destLat, destLon, closingVel)) {
|
||||
return;
|
||||
}
|
||||
if (xte > 1e8 || xte < -1e8) return;
|
||||
const float xte_m = (float)xte;
|
||||
const float dtw_m = (dist < 1e8) ? (float)dist : NAN;
|
||||
const uint32_t now = millis();
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_nav_data.xte_m = xte_m;
|
||||
g_nav_data.dtw_m = dtw_m;
|
||||
g_nav_data.age_ms = now;
|
||||
g_nav_data.valid = true;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGV(TAG, "PGN 129284 XTE=%.2f m DTW=%.0f m", xte_m, dtw_m);
|
||||
}
|
||||
|
||||
void MessageHandler(const tN2kMsg& msg) {
|
||||
switch (msg.PGN) {
|
||||
case 127250L:
|
||||
HandleHeading(msg);
|
||||
break;
|
||||
case 127251L:
|
||||
HandleROT(msg);
|
||||
break;
|
||||
default:
|
||||
// Sprint 1: ignore everything else.
|
||||
break;
|
||||
case 127250L: HandleHeading(msg); break;
|
||||
case 127251L: HandleROT(msg); break;
|
||||
case 129026L: HandleCogSog(msg); break;
|
||||
case 129284L: HandleNavData(msg); break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -110,7 +169,6 @@ void RxTask(void* /*pv*/) {
|
||||
AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
|
||||
for (;;) {
|
||||
NMEA2000.ParseMessages();
|
||||
// Update validity flags based on age.
|
||||
const uint32_t now = millis();
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
|
||||
@@ -120,6 +178,15 @@ void RxTask(void* /*pv*/) {
|
||||
g_snap.rot_valid = false;
|
||||
}
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
// Update COG/SOG and nav-data stale flags.
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
if (g_cog_sog.valid && (now - g_cog_sog.age_ms) > STALE_THRESHOLD_MS) {
|
||||
g_cog_sog.valid = false;
|
||||
}
|
||||
if (g_nav_data.valid && (now - g_nav_data.age_ms) > STALE_THRESHOLD_MS) {
|
||||
g_nav_data.valid = false;
|
||||
}
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
// 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
|
||||
vTaskDelay(pdMS_TO_TICKS(10));
|
||||
}
|
||||
@@ -170,9 +237,29 @@ HeadingSnapshot nmea2000_latest() {
|
||||
return copy;
|
||||
}
|
||||
|
||||
CogSogSnapshot nmea2000_cog_sog() {
|
||||
CogSogSnapshot copy;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
copy = g_cog_sog;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return copy;
|
||||
}
|
||||
|
||||
NavDataSnapshot nmea2000_nav_data() {
|
||||
NavDataSnapshot copy;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
copy = g_nav_data;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return copy;
|
||||
}
|
||||
|
||||
bool nmea2000_is_stale() {
|
||||
const auto s = nmea2000_latest();
|
||||
return !s.heading_valid;
|
||||
}
|
||||
|
||||
bool nmea2000_cog_is_stale() {
|
||||
return !nmea2000_cog_sog().valid;
|
||||
}
|
||||
|
||||
} // namespace arautopilot::protocols::nmea2000
|
||||
|
||||
@@ -2,34 +2,61 @@
|
||||
// nmea2000_consumer.h -- NMEA 2000 backbone consumer
|
||||
// =============================================================================
|
||||
//
|
||||
// Sprint 1 subscribes to PGN 127250 (Vessel Heading) and PGN 127251
|
||||
// (Rate of Turn) from the boat's NMEA 2000 backbone. The latest values
|
||||
// are stashed in a thread-safe snapshot that the Modbus slave (and later
|
||||
// the PID outer loop) read from.
|
||||
// Sprint 1: PGN 127250 (Heading) + 127251 (ROT).
|
||||
// Sprint 5: PGN 129026 (COG/SOG) + 129284 (Navigation Data / XTE).
|
||||
//
|
||||
// Later sprints will also subscribe to PGN 129025/129029 (Position),
|
||||
// 129026 (COG/SOG), 129284 (Navigation Data), 127257 (Attitude).
|
||||
//
|
||||
// This module owns the NMEA2000 instance and its dedicated FreeRTOS task
|
||||
// pinned to Core 0.
|
||||
// All snapshots are updated from the NMEA 2000 task (Core 0) and
|
||||
// read from the PID outer-loop task (Core 1) via a portMUX spinlock.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cmath>
|
||||
|
||||
namespace arautopilot::protocols::nmea2000 {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PGN 127250 / 127251 -- heading + ROT
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
struct HeadingSnapshot {
|
||||
float heading_deg; ///< 0..360, magnetic or true depending on source
|
||||
bool is_true; ///< true if reference is "true north", false if magnetic
|
||||
float rate_of_turn_dps; ///< signed deg/s; positive = turning to starboard
|
||||
uint32_t heading_age_ms; ///< millis() at the last 127250 update
|
||||
uint32_t rot_age_ms; ///< millis() at the last 127251 update
|
||||
bool heading_valid; ///< true if the heading is fresh (<5 s old)
|
||||
bool rot_valid; ///< true if the ROT is fresh (<5 s old)
|
||||
float heading_deg; ///< 0..360, magnetic or true per is_true flag
|
||||
bool is_true; ///< true = true north reference
|
||||
float rate_of_turn_dps; ///< signed deg/s; positive = turning to stbd
|
||||
uint32_t heading_age_ms; ///< millis() at last 127250 update
|
||||
uint32_t rot_age_ms; ///< millis() at last 127251 update
|
||||
bool heading_valid; ///< fresh (<5 s)
|
||||
bool rot_valid; ///< fresh (<5 s)
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PGN 129026 -- Course Over Ground (true) + Speed Over Ground
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
struct CogSogSnapshot {
|
||||
float cog_deg; ///< 0..360, degrees true; NAN until first message
|
||||
float sog_kn; ///< knots; NAN until first message
|
||||
uint32_t age_ms; ///< millis() at last update
|
||||
bool valid; ///< fresh (<5 s) and non-NaN
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PGN 129284 -- Navigation Data (XTE + waypoint)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
struct NavDataSnapshot {
|
||||
float xte_m; ///< cross-track error, metres; +stbd / -port; NAN if unknown
|
||||
float dtw_m; ///< distance to waypoint, metres; NAN if unknown
|
||||
char wp_name[16]; ///< null-terminated waypoint name (truncated at 15 chars)
|
||||
uint32_t age_ms; ///< millis() at last update
|
||||
bool valid; ///< fresh (<5 s) and non-NaN
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Public API
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/// Initialise the NMEA2000 stack with our PGN handlers.
|
||||
void nmea2000_consumer_init();
|
||||
|
||||
@@ -39,8 +66,16 @@ void nmea2000_consumer_start_task();
|
||||
/// Thread-safe read of the latest heading + ROT snapshot.
|
||||
HeadingSnapshot nmea2000_latest();
|
||||
|
||||
/// True if either heading_age_ms or rot_age_ms exceeds the stale-threshold
|
||||
/// (default 5 s, brief section 7).
|
||||
/// Thread-safe read of the latest COG/SOG snapshot (PGN 129026).
|
||||
CogSogSnapshot nmea2000_cog_sog();
|
||||
|
||||
/// Thread-safe read of the latest navigation data snapshot (PGN 129284).
|
||||
NavDataSnapshot nmea2000_nav_data();
|
||||
|
||||
/// True if heading age or ROT age exceeds 5 s.
|
||||
bool nmea2000_is_stale();
|
||||
|
||||
/// True if COG/SOG age exceeds 5 s or data was never received.
|
||||
bool nmea2000_cog_is_stale();
|
||||
|
||||
} // namespace arautopilot::protocols::nmea2000
|
||||
|
||||
Reference in New Issue
Block a user