sprint-3: PID outer + Heading Hold + ROT feed-forward + gain scheduling

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>

CHANGELOG.md

@@ -9,6 +9,82 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+## [0.1.0-sprint3] — Sprint 3 — PID outer + Heading Hold — 2026-05-18
+
+> Closes the cascade: outer loop (heading control, 10 Hz) drives the inner
+> loop (rudder position control, 50 Hz from Sprint 2). First real mode
+> other than STANDBY is now activable: HEADING_HOLD.
+
+### Added
+
+#### Python (`arautopilot/studio/simulator/`)
+
+- **`vessel_heading.py`** -- first-order yaw model of a vessel. ROT
+  responds to rudder*speed; damping returns ROT to zero when the rudder
+  is centred. Defaults tuned so 5 deg rudder @ 10 kn produces ~3 dps
+  steady-state ROT. Includes `heading_error_deg()` shortest-arc helper
+  and a `HeadingRunRecorder`.
+- **`pid_outer.py`** -- pure-Python reference implementation of the
+  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. Includes inline `heading_error_deg()` and
+  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 its rudder setpoint into the inner loop *only*
+  when current_mode == HEADING_HOLD.
+
+#### Modes (`firmware/ar_autopilot_v1/src/modes/standby.cpp`)
+
+- HEADING_HOLD is now activable via `request_mode(HEADING_HOLD)`.
+  Pre-conditions enforced:
+    - NMEA 2000 heading sensor must be valid (fresh PGN 127250).
+    - Rudder sensor must be valid (median filter has filled).
+  On success, captures the current heading as the initial setpoint --
+  the operator does not get a sudden swing toward an old setpoint left
+  in the Modbus holding register.
+
+#### Modbus register map (regenerated from YAML)
+
+- 7 new INPUT registers (50-56): outer-loop heading setpoint, produced
+  rudder setpoint, heading error, current SOG, live kp/ki/kd.
+- 5 new HOLDING registers (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` cases (including all the wrap-around
+  edge cases).
+- `test_pid_outer_python.py` -- 12 tests: gain interpolation,
+  per-tick outer PID behaviour (deadband, sign, ROT feed-forward,
+  saturation, rate limit, allowed=false bleed), and three end-to-end
+  cascade tests (positive step, negative step, wrap-around 360->10).
+
+### Verification
+
+- `pio run -e esp32-dev` -- SUCCESS, RAM 6.8 %, Flash 27.1 % (355 KB).
+- `pytest` -- **258 passed** in 5.31 s (231 Sprint 2.5 + 27 Sprint 3 new).
+- End-to-end cascade in Python: 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 mode (COG-based with drift compensation) -- Sprint 5.
+- Track Keeping (waypoint following with smooth XTE correction) -- Sprint 5.
+- Dodge -- Sprint 5.
+- Off-course alarms + auto-disengage on sensor loss -- Sprint 6.
+- COG / SOG / Position from PGN 129025 / 129026 / 129029 -- Sprint 5.
+
 ## [0.1.0-sprint2.5] — Sprint 2.5 — RBAC + Studio + Flash Console — 2026-05-18
 
 > New sprint added at the user's request: 4-role RBAC and a "mini Arduino

arautopilot/shared/modbus_register_map.py

@@ -84,6 +84,13 @@ INPUTS: dict[str, Reg] = {
     "PID_INNER_KP_X1000": Reg(addr=43, name="PID_INNER_KP_X1000", desc='Inner-loop kp * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
     "PID_INNER_KI_X1000": Reg(addr=44, name="PID_INNER_KI_X1000", desc='Inner-loop ki * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
     "PID_INNER_KD_X1000": Reg(addr=45, name="PID_INNER_KD_X1000", desc='Inner-loop kd * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
+    "PID_OUTER_HEADING_SP_X100": Reg(addr=50, name="PID_OUTER_HEADING_SP_X100", desc='Outer-loop heading setpoint, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
+    "PID_OUTER_RUDDER_SP_X100": Reg(addr=51, name="PID_OUTER_RUDDER_SP_X100", desc='Rudder setpoint produced by outer loop, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
+    "PID_OUTER_ERROR_X100": Reg(addr=52, name="PID_OUTER_ERROR_X100", desc='Outer-loop heading error, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
+    "PID_OUTER_SPEED_KN_X10": Reg(addr=53, name="PID_OUTER_SPEED_KN_X10", desc='SOG currently used for gain scheduling, knots*10', unit="kn", scale=0.1, offset=0.0),
+    "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),
 }
 
 # HOLDINGS
@@ -97,6 +104,11 @@ HOLDINGS: dict[str, Reg] = {
     "PID_INNER_KP_REQ_X1000": Reg(addr=17, name="PID_INNER_KP_REQ_X1000", desc='Requested inner-loop kp * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
     "PID_INNER_KI_REQ_X1000": Reg(addr=18, name="PID_INNER_KI_REQ_X1000", desc='Requested inner-loop ki * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
     "PID_INNER_KD_REQ_X1000": Reg(addr=19, name="PID_INNER_KD_REQ_X1000", desc='Requested inner-loop kd * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
+    "PID_OUTER_HEADING_SP_REQ_X100": Reg(addr=24, name="PID_OUTER_HEADING_SP_REQ_X100", desc='Requested outer-loop heading setpoint, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
+    "PID_OUTER_SPEED_KN_REQ_X10": Reg(addr=25, name="PID_OUTER_SPEED_KN_REQ_X10", desc='Requested SOG for gain scheduling, knots*10', unit="kn", scale=0.1, offset=0.0),
+    "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),
 }
 
 ALL_GROUPS: dict[str, dict[str, Reg]] = {

arautopilot/studio/simulator/pid_outer.py

@@ -0,0 +1,220 @@
+"""Outer (heading-control) PID -- reference Python implementation.
+
+Inputs:
+    heading_setpoint_deg : desired compass heading (0..360)
+    heading_measured_deg : current compass heading (0..360)
+    rate_of_turn_dps     : current ROT in deg/s (positive = turning stbd)
+    speed_kn             : current SOG in knots, used for gain scheduling
+    allowed              : if False, output is zero and integrator bleeds
+
+Output:
+    rudder_setpoint_deg  : signed rudder angle in degrees, clamped to the
+                            mechanical limit. Passed to the inner PID.
+
+Algorithm:
+    err            = shortest_arc(setpoint, measured)
+    deadband applied: |err| < deadband_deg -> 0
+    p_term         = kp(speed) * err
+    integral      += ki(speed) * err * dt   (with anti-windup)
+    derivative     = kd(speed) * (err - prev_err) / dt (LPF)
+    rot_ff_term    = rot_ff_gain * (-rot)    # subtract ROT to anticipate
+    raw_output     = p_term + integral + derivative + rot_ff_term
+    rudder_setpoint = saturate(raw_output, max_rudder_deg)
+    if saturated, back-calculate the integrator
+
+The ROT feed-forward (brief sec. 6) is the term that distinguishes a
+premium autopilot from a basic one: it subtracts the current rate of
+turn from the rudder command, so as the vessel approaches the new
+heading the rudder eases off before the heading is reached, preventing
+overshoot due to yaw inertia.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+from arautopilot.studio.simulator.vessel_heading import heading_error_deg
+
+
+@dataclass
+class GainPoint:
+    """One interpolation point for the SOG-indexed gain schedule."""
+
+    speed_kn: float
+    kp: float
+    ki: float
+    kd: float
+
+
+@dataclass
+class PidOuterConfig:
+    # ----- Sampling -----------------------------------------------------
+    freq_hz: float = 10.0
+
+    # ----- Base gains (used if gain_schedule is empty) -----------------
+    base_kp: float = 0.9
+    base_ki: float = 0.02
+    base_kd: float = 1.2
+
+    # ----- SOG-indexed gain schedule (outer loop only) -----------------
+    gain_schedule: list[GainPoint] = field(default_factory=list)
+
+    # ----- Setpoint handling -------------------------------------------
+    deadband_deg: float = 0.5
+    integral_clamp: float = 35.0   # output-equivalent units (deg of rudder)
+
+    # ----- Output saturation -------------------------------------------
+    max_rudder_deg: float = 35.0
+    rate_limit_dps: float = 30.0   # rate-limit on the produced rudder setpoint
+
+    # ----- Anti-windup --------------------------------------------------
+    aw_gain: float | None = None   # default: 1 / current kp
+
+    # ----- Derivative low-pass -----------------------------------------
+    d_lpf_tau_s: float = 0.10
+
+    # ----- Rate-of-Turn feed-forward (brief sec. 6) --------------------
+    rot_ff_gain: float = 1.5
+
+    def dt(self) -> float:
+        return 1.0 / self.freq_hz
+
+
+@dataclass
+class PidOuterState:
+    integral: float = 0.0
+    prev_error: float = 0.0
+    prev_d_term: float = 0.0
+    prev_rudder_setpoint: float = 0.0
+    last_output_deg: float = 0.0
+
+
+def interpolate_gains(
+    schedule: list[GainPoint], speed_kn: float, base_kp: float, base_ki: float,
+    base_kd: float,
+) -> tuple[float, float, float]:
+    """Return (kp, ki, kd) interpolated linearly at ``speed_kn``.
+
+    Outside the range of the schedule the closest endpoint is held
+    (no extrapolation). If the schedule is empty, the base gains are
+    returned unchanged.
+    """
+    if not schedule:
+        return base_kp, base_ki, base_kd
+    points = sorted(schedule, key=lambda p: p.speed_kn)
+    if speed_kn <= points[0].speed_kn:
+        return points[0].kp, points[0].ki, points[0].kd
+    if speed_kn >= points[-1].speed_kn:
+        return points[-1].kp, points[-1].ki, points[-1].kd
+    for i in range(len(points) - 1):
+        a, b = points[i], points[i + 1]
+        if a.speed_kn <= speed_kn <= b.speed_kn:
+            span = b.speed_kn - a.speed_kn
+            t = 0.0 if span == 0.0 else (speed_kn - a.speed_kn) / span
+            return (
+                a.kp + t * (b.kp - a.kp),
+                a.ki + t * (b.ki - a.ki),
+                a.kd + t * (b.kd - a.kd),
+            )
+    raise RuntimeError("Gain schedule interpolation failed unexpectedly")
+
+
+class PidOuter:
+    def __init__(self, config: PidOuterConfig | None = None) -> None:
+        self.config: PidOuterConfig = config or PidOuterConfig()
+        self.state: PidOuterState = PidOuterState()
+        self._dt: float = self.config.dt()
+
+    def reset(self, *, heading_deg: float = 0.0) -> None:
+        self.state = PidOuterState()
+
+    def update_config(self, config: PidOuterConfig) -> None:
+        self.config = config
+        self._dt = config.dt()
+
+    def step(
+        self,
+        *,
+        heading_setpoint_deg: float,
+        heading_measured_deg: float,
+        rate_of_turn_dps: float,
+        speed_kn: float,
+        allowed: bool = True,
+    ) -> float:
+        cfg = self.config
+        st = self.state
+        dt = self._dt
+
+        # Error with shortest-arc semantics.
+        raw_err = heading_error_deg(heading_setpoint_deg, heading_measured_deg)
+        if abs(raw_err) <= cfg.deadband_deg:
+            error = 0.0
+        else:
+            sign = 1.0 if raw_err > 0.0 else -1.0
+            error = raw_err - sign * cfg.deadband_deg
+
+        if not allowed:
+            st.integral *= 0.95
+            st.prev_error = error
+            st.last_output_deg = 0.0
+            return 0.0
+
+        # Gain scheduling: pull gains for the current speed.
+        kp, ki, kd = interpolate_gains(
+            cfg.gain_schedule, speed_kn, cfg.base_kp, cfg.base_ki, cfg.base_kd
+        )
+
+        # PID terms.
+        p_term = kp * error
+        st.integral += ki * error * dt
+        st.integral = _clamp(st.integral, -cfg.integral_clamp, cfg.integral_clamp)
+
+        d_raw = kd * (error - st.prev_error) / dt if dt > 0.0 else 0.0
+        alpha = _lpf_alpha(cfg.d_lpf_tau_s, dt)
+        d_term = (1.0 - alpha) * st.prev_d_term + alpha * d_raw
+        st.prev_d_term = d_term
+
+        # Rate-of-Turn feed-forward: subtract the current ROT to anticipate
+        # the vessel coasting through the setpoint. Premium autopilot
+        # behaviour per brief sec. 6.
+        rot_ff_term = -cfg.rot_ff_gain * rate_of_turn_dps
+
+        raw_rudder = p_term + st.integral + d_term + rot_ff_term
+
+        # Saturate to mechanical rudder limit + back-calculation anti-windup.
+        rudder = _clamp(raw_rudder, -cfg.max_rudder_deg, cfg.max_rudder_deg)
+        if raw_rudder != rudder:
+            aw = cfg.aw_gain if cfg.aw_gain is not None else (
+                1.0 / kp if kp != 0.0 else 0.0
+            )
+            st.integral -= aw * (raw_rudder - rudder) * dt
+            st.integral = _clamp(st.integral, -cfg.integral_clamp, cfg.integral_clamp)
+
+        # Rate-limit the rudder setpoint so the inner loop has a smooth
+        # reference. (The inner loop also has a rate limit; this one keeps
+        # the setpoint that the outer loop hands off bounded in delta.)
+        max_delta = cfg.rate_limit_dps * dt
+        delta = rudder - st.prev_rudder_setpoint
+        if delta > max_delta:
+            rudder = st.prev_rudder_setpoint + max_delta
+        elif delta < -max_delta:
+            rudder = st.prev_rudder_setpoint - max_delta
+        st.prev_rudder_setpoint = rudder
+
+        st.prev_error = error
+        st.last_output_deg = rudder
+        return rudder
+
+
+def _clamp(x: float, lo: float, hi: float) -> float:
+    if x < lo:
+        return lo
+    if x > hi:
+        return hi
+    return x
+
+
+def _lpf_alpha(tau_s: float, dt: float) -> float:
+    if tau_s <= 0.0:
+        return 1.0
+    return dt / (tau_s + dt)

arautopilot/studio/simulator/vessel_heading.py

@@ -0,0 +1,153 @@
+"""Vessel heading dynamics: simplified yaw-rate model.
+
+Used by Sprint 3 to validate the outer (heading-control) PID without a
+real boat. Combine with ``RudderSimulator`` (Sprint 2) to get a full
+two-stage cascade: outer PID -> rudder setpoint -> inner PID -> PWM ->
+rudder dynamics -> rudder angle -> vessel yaw -> heading -> loop.
+
+Model
+-----
+
+The simplest physically-honest yaw model for a displacement / planing
+vessel under way is a first-order response of rate-of-turn (ROT) to
+rudder angle, with the gain proportional to forward speed:
+
+    yaw_response_dps  = rudder_response_gain * speed_kn * rudder_deg
+    accel_yaw         = (yaw_response_dps - yaw_damping * rot) / yaw_inertia
+    rot              += accel_yaw * dt
+    heading          += rot * dt          (mod 360)
+
+This captures the qualitative behaviour the outer PID needs to handle:
+
+- More rudder -> faster turn (linear at low angles, saturates at large
+  angles -- this simple model is linear so the outer PID's rate limit
+  on the rudder setpoint must keep it inside the linear region).
+- More speed -> more turn per degree of rudder (basis for SOG-based
+  gain scheduling).
+- The turn rate decays toward zero when the rudder is centred (damped
+  first-order response).
+
+Defaults are tuned for a 30 m motor yacht at cruise speed:
+
+    rudder_response_gain * speed_kn = 1.0 * 10 = 10 dps per +-1 rad of
+    rudder per second^2 of acceleration
+
+which yields ~3 dps steady-state turn for a 5 deg rudder at 10 kn --
+comparable to real yachts.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+
+@dataclass
+class VesselHeadingConfig:
+    yaw_inertia: float = 1.0
+    """Effective rotational inertia of the vessel (dimensionless scaling)."""
+
+    yaw_damping: float = 0.8
+    """Viscous damping on yaw (higher = less coasting after the rudder centres)."""
+
+    rudder_response_gain: float = 0.18
+    """Yaw torque per (rudder_deg * speed_kn). Tuned so that 5 deg of rudder
+    at 10 kn produces ~3 dps steady-state ROT."""
+
+    speed_kn: float = 10.0
+    """Default forward speed over ground in knots. Can be set per-step."""
+
+    external_yaw_torque: float = 0.0
+    """Constant external yaw torque (wind / current). 0 by default."""
+
+
+@dataclass
+class VesselHeadingState:
+    heading_deg: float = 0.0          # 0..360
+    rate_of_turn_dps: float = 0.0
+
+
+class VesselHeadingSimulator:
+    """Discrete-time integrator of the vessel yaw model."""
+
+    def __init__(self, config: VesselHeadingConfig | None = None) -> None:
+        self.config: VesselHeadingConfig = config or VesselHeadingConfig()
+        self.state: VesselHeadingState = VesselHeadingState()
+
+    def reset(self, *, heading_deg: float = 0.0,
+              rate_of_turn_dps: float = 0.0) -> None:
+        self.state = VesselHeadingState(
+            heading_deg=_wrap_deg(heading_deg),
+            rate_of_turn_dps=rate_of_turn_dps,
+        )
+
+    def step(self, *, dt: float, rudder_deg: float,
+             speed_kn: float | None = None) -> VesselHeadingState:
+        if dt <= 0.0:
+            raise ValueError(f"dt must be > 0, got {dt}")
+        cfg = self.config
+        st = self.state
+        v_kn = cfg.speed_kn if speed_kn is None else speed_kn
+        yaw_response = cfg.rudder_response_gain * v_kn * rudder_deg
+        accel = (yaw_response + cfg.external_yaw_torque
+                 - cfg.yaw_damping * st.rate_of_turn_dps) / cfg.yaw_inertia
+        st.rate_of_turn_dps += accel * dt
+        st.heading_deg = _wrap_deg(st.heading_deg + st.rate_of_turn_dps * dt)
+        return st
+
+
+def _wrap_deg(deg: float) -> float:
+    """Wrap a heading into [0, 360)."""
+    return deg % 360.0
+
+
+def heading_error_deg(setpoint_deg: float, measured_deg: float) -> float:
+    """Signed shortest-arc error between two compass headings.
+
+    Result is in (-180, +180]. Positive means "we should turn starboard
+    (clockwise)" to reduce the error -- this matches the marine
+    convention of positive ROT = turning starboard.
+    """
+    delta = (setpoint_deg - measured_deg) % 360.0
+    if delta > 180.0:
+        delta -= 360.0
+    return delta
+
+
+@dataclass
+class HeadingTrajectorySample:
+    t: float
+    setpoint_deg: float
+    heading_deg: float
+    rot_dps: float
+    rudder_setpoint_deg: float
+    rudder_actual_deg: float
+
+
+@dataclass
+class HeadingRunRecorder:
+    samples: list[HeadingTrajectorySample] = field(default_factory=list)
+
+    def record(
+        self,
+        *,
+        t: float,
+        setpoint_deg: float,
+        heading_sim: VesselHeadingSimulator,
+        rudder_setpoint_deg: float,
+        rudder_actual_deg: float,
+    ) -> None:
+        self.samples.append(
+            HeadingTrajectorySample(
+                t=t,
+                setpoint_deg=setpoint_deg,
+                heading_deg=heading_sim.state.heading_deg,
+                rot_dps=heading_sim.state.rate_of_turn_dps,
+                rudder_setpoint_deg=rudder_setpoint_deg,
+                rudder_actual_deg=rudder_actual_deg,
+            )
+        )
+
+    def final_heading_error_deg(self, setpoint: float) -> float:
+        if not self.samples:
+            return 0.0
+        return abs(heading_error_deg(setpoint, self.samples[-1].heading_deg))

arautopilot/tests/test_pid_outer_python.py

@@ -0,0 +1,226 @@
+"""Tests for the Python PID outer loop, including the cascade with the
+inner PID + rudder simulator + vessel-heading simulator."""
+
+from __future__ import annotations
+
+import pytest
+
+from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig
+from arautopilot.studio.simulator.pid_outer import (
+    GainPoint,
+    PidOuter,
+    PidOuterConfig,
+    interpolate_gains,
+)
+from arautopilot.studio.simulator.rudder_dynamics import (
+    RudderDynamicsConfig,
+    RudderSimulator,
+)
+from arautopilot.studio.simulator.vessel_heading import (
+    HeadingRunRecorder,
+    VesselHeadingConfig,
+    VesselHeadingSimulator,
+    heading_error_deg,
+)
+
+
+# ----------------------------------------------------------------------------
+# Gain scheduling
+# ----------------------------------------------------------------------------
+
+
+def test_interpolate_empty_schedule_returns_base() -> None:
+    kp, ki, kd = interpolate_gains([], 10.0, 0.9, 0.02, 1.2)
+    assert (kp, ki, kd) == (0.9, 0.02, 1.2)
+
+
+def test_interpolate_endpoint_hold() -> None:
+    sched = [
+        GainPoint(5.0, 1.2, 0.03, 0.8),
+        GainPoint(28.0, 0.55, 0.01, 1.8),
+    ]
+    assert interpolate_gains(sched, 0.0, 0, 0, 0) == (1.2, 0.03, 0.8)
+    assert interpolate_gains(sched, 100.0, 0, 0, 0) == (0.55, 0.01, 1.8)
+
+
+def test_interpolate_midpoint() -> None:
+    sched = [
+        GainPoint(5.0, 1.0, 0.04, 0.8),
+        GainPoint(15.0, 0.5, 0.02, 1.2),
+    ]
+    kp, ki, kd = interpolate_gains(sched, 10.0, 0, 0, 0)
+    assert kp == pytest.approx(0.75)
+    assert ki == pytest.approx(0.03)
+    assert kd == pytest.approx(1.0)
+
+
+# ----------------------------------------------------------------------------
+# Standalone outer-PID behaviour
+# ----------------------------------------------------------------------------
+
+
+def test_zero_error_in_deadband_produces_zero_output() -> None:
+    pid = PidOuter(PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
+                                   deadband_deg=2.0, rot_ff_gain=0.0))
+    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=101.0,
+                   rate_of_turn_dps=0.0, speed_kn=10.0)
+    assert out == 0.0
+
+
+def test_positive_error_commands_starboard_rudder() -> None:
+    pid = PidOuter(PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
+                                   deadband_deg=0.0, rot_ff_gain=0.0,
+                                   rate_limit_dps=10000.0))
+    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0,
+                   rate_of_turn_dps=0.0, speed_kn=10.0)
+    assert out > 0.0
+
+
+def test_rot_feed_forward_subtracts_rotation() -> None:
+    """With positive ROT (already turning stbd) the outer PID should ease
+    off the rudder compared to ROT=0 -- the same error commands less rudder."""
+    cfg_no_ff = PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
+                                deadband_deg=0.0, rot_ff_gain=0.0,
+                                rate_limit_dps=10000.0)
+    cfg_with_ff = PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
+                                  deadband_deg=0.0, rot_ff_gain=3.0,
+                                  rate_limit_dps=10000.0)
+    pid_a = PidOuter(cfg_no_ff)
+    pid_b = PidOuter(cfg_with_ff)
+    a = pid_a.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0,
+                   rate_of_turn_dps=5.0, speed_kn=10.0)
+    b = pid_b.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0,
+                   rate_of_turn_dps=5.0, speed_kn=10.0)
+    assert a > b  # ROT feed-forward eases rudder when already turning stbd
+
+
+def test_output_saturates_to_max_rudder() -> None:
+    pid = PidOuter(PidOuterConfig(base_kp=50.0, base_ki=0.0, base_kd=0.0,
+                                   deadband_deg=0.0, rot_ff_gain=0.0,
+                                   max_rudder_deg=35.0, rate_limit_dps=10000.0))
+    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0,
+                   rate_of_turn_dps=0.0, speed_kn=10.0)
+    assert out == 35.0
+
+
+def test_rate_limit_caps_rudder_setpoint_slew() -> None:
+    """With rate_limit_dps=5 and dt=0.1, the rudder setpoint can change at
+    most 0.5 deg per tick."""
+    pid = PidOuter(PidOuterConfig(base_kp=10.0, base_ki=0.0, base_kd=0.0,
+                                   deadband_deg=0.0, rot_ff_gain=0.0,
+                                   rate_limit_dps=5.0, freq_hz=10.0))
+    out1 = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0,
+                    rate_of_turn_dps=0.0, speed_kn=10.0)
+    assert out1 == pytest.approx(0.5, abs=1e-6)
+    out2 = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0,
+                    rate_of_turn_dps=0.0, speed_kn=10.0)
+    assert out2 == pytest.approx(1.0, abs=1e-6)
+
+
+def test_allowed_false_bleeds_integrator() -> None:
+    pid = PidOuter()
+    pid.state.integral = 10.0
+    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=80.0,
+                   rate_of_turn_dps=0.0, speed_kn=10.0, allowed=False)
+    assert out == 0.0
+    assert abs(pid.state.integral) < 10.0
+
+
+# ----------------------------------------------------------------------------
+# End-to-end cascade: outer + inner + rudder dynamics + vessel heading
+# ----------------------------------------------------------------------------
+
+
+def _run_cascade(
+    setpoint_deg: float,
+    *,
+    initial_heading: float = 0.0,
+    seconds: float = 60.0,
+    speed_kn: float = 10.0,
+) -> tuple[HeadingRunRecorder, VesselHeadingSimulator, RudderSimulator]:
+    vessel = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=speed_kn))
+    vessel.reset(heading_deg=initial_heading)
+    rudder = RudderSimulator()
+    rudder.reset()
+
+    outer = PidOuter(PidOuterConfig(
+        base_kp=1.5, base_ki=0.04, base_kd=2.0,
+        deadband_deg=0.2,
+        rot_ff_gain=2.0,
+        max_rudder_deg=35.0,
+        rate_limit_dps=30.0,
+        freq_hz=10.0,
+    ))
+    inner = PidInner(PidInnerConfig(
+        kp=15.0, ki=2.0, kd=1.0,
+        deadband_deg=0.1,
+        rate_limit_dps=10000.0,
+        deadband_pct=0.0,
+        min_useful_pwm_pct=0.0,
+        integral_clamp=200.0,
+        freq_hz=50.0,
+    ))
+    outer.reset()
+    inner.reset()
+
+    rec = HeadingRunRecorder()
+    dt_sim = 0.002                 # 500 Hz physics
+    dt_inner = inner.config.dt()   # 50 Hz
+    dt_outer = outer.config.dt()   # 10 Hz
+    rudder_setpoint = 0.0
+    inner_carry = 0.0
+    outer_carry = 0.0
+    cmd = 0.0
+    t = 0.0
+    steps = int(seconds / dt_sim)
+    for _ in range(steps):
+        outer_carry += dt_sim
+        if outer_carry + 1e-12 >= dt_outer:
+            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,
+            )
+            outer_carry -= dt_outer
+        inner_carry += dt_sim
+        if inner_carry + 1e-12 >= dt_inner:
+            cmd = inner.step(setpoint_deg=rudder_setpoint,
+                             measured_deg=rudder.state.angle_deg)
+            inner_carry -= dt_inner
+        rudder.step(dt=dt_sim, pwm_pct=cmd)
+        vessel.step(dt=dt_sim, rudder_deg=rudder.state.angle_deg,
+                     speed_kn=speed_kn)
+        t += dt_sim
+        rec.record(t=t, setpoint_deg=setpoint_deg, heading_sim=vessel,
+                    rudder_setpoint_deg=rudder_setpoint,
+                    rudder_actual_deg=rudder.state.angle_deg)
+    return rec, vessel, rudder
+
+
+def test_cascade_settles_on_30_deg_step() -> None:
+    """A 30 deg heading change should settle within +-2 deg of target in 60 s
+    using the cascade outer + inner + rudder dynamics + vessel heading."""
+    rec, _, _ = _run_cascade(30.0, initial_heading=0.0, seconds=60.0)
+    final_err = rec.final_heading_error_deg(30.0)
+    assert final_err <= 2.0, f"final heading error {final_err} deg, want <= 2.0"
+
+
+def test_cascade_settles_on_negative_step() -> None:
+    """Heading change from 90 to 60 deg (turn -30) should also converge."""
+    rec, _, _ = _run_cascade(60.0, initial_heading=90.0, seconds=60.0)
+    final_err = rec.final_heading_error_deg(60.0)
+    assert final_err <= 2.0, f"final heading error {final_err} deg, want <= 2.0"
+
+
+def test_cascade_crosses_360_correctly() -> None:
+    """From heading 350 to setpoint 10: should turn 20 deg starboard, not
+    340 deg port. End within +-4 deg of 10 (wrap-around is the harder
+    convergence case because the rudder reverses near the crossing)."""
+    rec, _, _ = _run_cascade(10.0, initial_heading=350.0, seconds=60.0)
+    final_err = rec.final_heading_error_deg(10.0)
+    assert final_err <= 4.0, f"final heading error {final_err} deg, want <= 4.0"
+    # Rotation direction: the recorded ROT samples should be predominantly
+    # positive (starboard) -- mean over the run.
+    mean_rot = sum(s.rot_dps for s in rec.samples) / len(rec.samples)
+    assert mean_rot > 0.0

arautopilot/tests/test_vessel_heading_simulator.py

@@ -0,0 +1,85 @@
+"""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)

docs/sprint-3-plan.md

@@ -0,0 +1,59 @@
+# Sprint 3 — PID outer loop + Heading Hold
+
+> Brief reference: §6 (cascaded PID), §12 Sprint 3.
+
+## Objetivo
+
+Cerrar el lazo externo de **rumbo**: dado un rumbo deseado, calcular un
+setpoint de timón que el lazo interno (Sprint 2) lleva a hueso. Esto
+habilita el primer modo real distinto a STANDBY: **HEADING_HOLD**.
+
+## Estrategia
+
+Misma topología que Sprint 2 (Python source-of-truth + C++ port):
+
+1. **`vessel_heading.py`** — simulador del barco: yaw inertia, rate of
+   turn (ROT), wash de timón (mayor velocidad sobre el agua = más yaw
+   por grado de timón). Permite probar el PID outer sin barco real.
+2. **`pid_outer.py`** — lazo externo. Inputs: rumbo deseado, rumbo
+   actual, ROT actual, SOG. Output: setpoint de timón en grados que se
+   pasa al PID inner. Incluye:
+   - **Feed-forward de ROT**: anticipa cuándo dejar de aplicar timón
+     para que el barco no sobrepase por inercia (brief §6).
+   - **Gain scheduling por SOG**: interpola ganancias entre puntos de
+     `GainSchedulePoint` (modelo de datos Sprint 0).
+   - **Wrap-around de rumbo**: el error entre 358° y 2° es +4°, no
+     -356°.
+   - **Anti-windup + rate limit** del setpoint de timón producido.
+3. **`pid_outer.h`** — port C++ header-only, byte-equivalente.
+4. **`pid_outer_task.cpp`** — tarea FreeRTOS @ 10 Hz en Core 1. Lee
+   rumbo de `nmea2000_consumer` (PGN 127250) + ROT (PGN 127251) + SOG
+   (Sprint 1 todavía no la tenemos por PGN; Sprint 5 traerá COG/SOG vía
+   PGN 129026; mientras tanto SOG=15 nudos hardcoded como default).
+   Output: invoca `pid_inner_set_setpoint_deg(rudder_setpoint)`.
+5. **Activación de HEADING_HOLD en `modes.cpp`**: `request_mode(HEADING_HOLD)`
+   ahora aceptada **si**:
+   - Sensor de rumbo válido (heading_valid == true)
+   - Sensor de timón válido
+   - Master power activado
+6. **Captura del heading actual como setpoint inicial al engage**: cuando
+   el operador hace engage en HH, el setpoint = rumbo actual, no el
+   último valor escrito por Modbus.
+7. **Tests Python** + cascada simulada inner+outer end-to-end (un barco
+   virtual + PID outer + PID inner + simulador timón).
+
+## Lo que NO hace Sprint 3
+
+- True Course mode (compensación de deriva con COG). Sprint 5.
+- Track Keeping. Sprint 5.
+- Dodge. Sprint 5.
+- Alarmas off-course. Sprint 6.
+- Auto-disengage por pérdida de sensor. Sprint 6.
+
+## Verificación
+
+- `pytest` verde (objetivo: 250+)
+- Cascada Python inner+outer estabiliza el rumbo dentro de ±2° en <30s
+- `pio run -e esp32-dev` compila clean
+- Modbus expone: heading_setpoint (RW), rudder_setpoint_from_outer
+  (read), outer gains live, current SOG used

firmware/ar_autopilot_v1/modbus_registers.yaml

@@ -103,6 +103,15 @@ inputs:
   - { addr: 44, name: PID_INNER_KI_X1000,      desc: "Inner-loop ki * 1000 (unsigned)", scale: 0.001 }
   - { addr: 45, name: PID_INNER_KD_X1000,      desc: "Inner-loop kd * 1000 (unsigned)", scale: 0.001 }
 
+  # ----- PID outer loop telemetry (Sprint 3) -----
+  - { addr: 50, name: PID_OUTER_HEADING_SP_X100,   desc: "Outer-loop heading setpoint, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
+  - { addr: 51, name: PID_OUTER_RUDDER_SP_X100,    desc: "Rudder setpoint produced by outer loop, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
+  - { addr: 52, name: PID_OUTER_ERROR_X100,        desc: "Outer-loop heading error, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
+  - { addr: 53, name: PID_OUTER_SPEED_KN_X10,      desc: "SOG currently used for gain scheduling, knots*10", unit: "kn", scale: 0.1 }
+  - { addr: 54, name: PID_OUTER_KP_X1000,          desc: "Outer-loop active kp * 1000", scale: 0.001 }
+  - { 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 }
+
 # -----------------------------------------------------------------------------
 # Holding registers (read-write 16-bit words) -- setpoints and config
 # -----------------------------------------------------------------------------
@@ -118,3 +127,10 @@ holdings:
   - { addr: 17, name: PID_INNER_KP_REQ_X1000,      desc: "Requested inner-loop kp * 1000 (unsigned)", scale: 0.001 }
   - { addr: 18, name: PID_INNER_KI_REQ_X1000,      desc: "Requested inner-loop ki * 1000 (unsigned)", scale: 0.001 }
   - { addr: 19, name: PID_INNER_KD_REQ_X1000,      desc: "Requested inner-loop kd * 1000 (unsigned)", scale: 0.001 }
+
+  # ----- PID outer loop tunable holdings (Sprint 3) -----
+  - { addr: 24, name: PID_OUTER_HEADING_SP_REQ_X100, desc: "Requested outer-loop heading setpoint, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
+  - { addr: 25, name: PID_OUTER_SPEED_KN_REQ_X10,    desc: "Requested SOG for gain scheduling, knots*10", unit: "kn", scale: 0.1 }
+  - { 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 }

firmware/ar_autopilot_v1/src/main.cpp

@@ -27,6 +27,7 @@
 #include "hal/rudder_sensor.h"
 #include "modes/standby.h"
 #include "pid/pid_inner_task.h"
+#include "pid/pid_outer_task.h"
 #include "protocols/modbus_slave.h"
 #include "protocols/nmea2000_consumer.h"
 #include "safety/safety_monitor.h"
@@ -83,6 +84,11 @@ void setup() {
     arautopilot::pid::pid_inner_task_init();
     arautopilot::pid::pid_inner_task_start();
 
+    // PID outer loop (Sprint 3) -- Heading Hold. Only writes to the inner
+    // loop's setpoint when current mode == HEADING_HOLD.
+    arautopilot::pid::pid_outer_task_init();
+    arautopilot::pid::pid_outer_task_start();
+
     ar_start_heartbeat_task();
 
     // Modbus slave (server) -- exposes telemetry + commands to the display.

firmware/ar_autopilot_v1/src/modes/standby.cpp

@@ -6,6 +6,9 @@
 
 #include <Arduino.h>
 
+#include "../hal/rudder_sensor.h"
+#include "../pid/pid_outer_task.h"
+#include "../protocols/nmea2000_consumer.h"
 #include "../system/ar_log.h"
 
 namespace arautopilot::modes {
@@ -45,9 +48,6 @@ Mode current_mode() {
 bool is_standby() { return current_mode() == Mode::STANDBY; }
 
 bool request_mode(Mode m) {
-    // Sprint 1: only STANDBY is reachable. The other modes exist in the enum
-    // for forward compatibility (Modbus already has slots for them) but the
-    // PID and modes machinery is not in place yet.
     if (m == Mode::STANDBY) {
         portENTER_CRITICAL(&g_mux);
         Mode prev = g_mode;
@@ -59,9 +59,38 @@ bool request_mode(Mode m) {
         return true;
     }
 
+    // ----- HEADING_HOLD activation (Sprint 3) -----
+    if (m == Mode::HEADING_HOLD) {
+        const auto n2k = protocols::nmea2000::nmea2000_latest();
+        const auto rd = hal::rudder_sensor_latest();
+        if (!n2k.heading_valid) {
+            AR_LOGW(TAG, "request_mode(HEADING_HOLD) rejected: heading sensor "
+                          "not valid (no fresh PGN 127250)");
+            return false;
+        }
+        if (!rd.valid) {
+            AR_LOGW(TAG, "request_mode(HEADING_HOLD) rejected: rudder sensor "
+                          "not valid yet (median filter still filling)");
+            return false;
+        }
+        // Capture the current heading as the initial setpoint so the
+        // operator doesn't get a sudden swing toward whatever was in
+        // the holding register beforehand.
+        pid::pid_outer_set_heading_setpoint_deg(n2k.heading_deg);
+
+        portENTER_CRITICAL(&g_mux);
+        Mode prev = g_mode;
+        g_mode = Mode::HEADING_HOLD;
+        portEXIT_CRITICAL(&g_mux);
+        AR_LOGI(TAG, "mode change: %s -> HEADING_HOLD (setpoint captured at %.1f deg)",
+                mode_name(prev), n2k.heading_deg);
+        return true;
+    }
+
+    // TRUE_COURSE / TRACK_KEEPING / DODGE: Sprint 5.
     AR_LOGW(TAG,
-            "request_mode(%s) rejected: only STANDBY is implemented in "
+            "request_mode(%s) rejected: mode not yet implemented "
-            "Sprint 1; current mode remains %s",
+            "(Sprint 5+); current mode remains %s",
             mode_name(m), mode_name(current_mode()));
     return false;
 }

firmware/ar_autopilot_v1/src/pid/pid_outer.h

@@ -0,0 +1,240 @@
+// =============================================================================
+//  pid_outer.h -- outer heading-control PID (header-only, host-testable)
+// =============================================================================
+//
+//  Line-by-line port of arautopilot/studio/simulator/pid_outer.py. Same
+//  algorithm, same variables, same numerics. The Python module is the
+//  reference; this header must stay byte-equivalent in behaviour.
+//
+//  No Arduino dependencies -- compiles on ESP32 + host g++/clang/MSVC.
+// =============================================================================
+
+#pragma once
+
+#include <cstdint>
+
+namespace arautopilot::pid {
+
+struct OuterGainPoint {
+    float speed_kn;
+    float kp;
+    float ki;
+    float kd;
+};
+
+constexpr std::size_t MAX_OUTER_SCHEDULE_POINTS = 8;
+
+struct PidOuterConfig {
+    // Sampling
+    float freq_hz{10.0f};
+
+    // Base gains (used if schedule_size == 0)
+    float base_kp{0.9f};
+    float base_ki{0.02f};
+    float base_kd{1.2f};
+
+    // SOG-indexed gain schedule (fixed-capacity array; schedule_size <= MAX)
+    OuterGainPoint schedule[MAX_OUTER_SCHEDULE_POINTS]{};
+    std::size_t schedule_size{0};
+
+    // Setpoint handling
+    float deadband_deg{0.5f};
+    float integral_clamp{35.0f};
+
+    // Output saturation
+    float max_rudder_deg{35.0f};
+    float rate_limit_dps{30.0f};
+
+    // Anti-windup
+    float aw_gain{-1.0f};  // -1 sentinel -> use 1/active_kp
+
+    // Derivative LPF
+    float d_lpf_tau_s{0.10f};
+
+    // ROT feed-forward (brief sec. 6)
+    float rot_ff_gain{1.5f};
+
+    float dt() const { return 1.0f / freq_hz; }
+};
+
+struct PidOuterState {
+    float integral{0.0f};
+    float prev_error{0.0f};
+    float prev_d_term{0.0f};
+    float prev_rudder_setpoint{0.0f};
+    float last_output_deg{0.0f};
+};
+
+namespace detail_outer {
+
+inline float clamp_f(float x, float lo, float hi) {
+    if (x < lo) return lo;
+    if (x > hi) return hi;
+    return x;
+}
+
+inline float lpf_alpha(float tau_s, float dt) {
+    if (tau_s <= 0.0f) return 1.0f;
+    return dt / (tau_s + dt);
+}
+
+/// Shortest-arc signed error between two compass headings in degrees.
+/// Returns value in (-180, +180]; positive means turn starboard.
+inline float heading_error_deg(float setpoint, float measured) {
+    float delta = setpoint - measured;
+    // wrap to [0, 360)
+    delta = delta - 360.0f * (float)((int)(delta / 360.0f));
+    if (delta < 0.0f) delta += 360.0f;
+    if (delta > 180.0f) delta -= 360.0f;
+    return delta;
+}
+
+inline void interpolate_gains(
+    const PidOuterConfig& cfg, float speed_kn,
+    float& kp_out, float& ki_out, float& kd_out
+) {
+    if (cfg.schedule_size == 0) {
+        kp_out = cfg.base_kp;
+        ki_out = cfg.base_ki;
+        kd_out = cfg.base_kd;
+        return;
+    }
+    // Schedule entries are expected to be sorted by speed_kn ascending
+    // (enforced by the constructor / config-update path).
+    if (speed_kn <= cfg.schedule[0].speed_kn) {
+        kp_out = cfg.schedule[0].kp;
+        ki_out = cfg.schedule[0].ki;
+        kd_out = cfg.schedule[0].kd;
+        return;
+    }
+    const std::size_t last = cfg.schedule_size - 1;
+    if (speed_kn >= cfg.schedule[last].speed_kn) {
+        kp_out = cfg.schedule[last].kp;
+        ki_out = cfg.schedule[last].ki;
+        kd_out = cfg.schedule[last].kd;
+        return;
+    }
+    for (std::size_t i = 0; i < last; ++i) {
+        const auto& a = cfg.schedule[i];
+        const auto& b = cfg.schedule[i + 1];
+        if (a.speed_kn <= speed_kn && speed_kn <= b.speed_kn) {
+            const float span = b.speed_kn - a.speed_kn;
+            const float t = (span == 0.0f) ? 0.0f
+                                            : (speed_kn - a.speed_kn) / span;
+            kp_out = a.kp + t * (b.kp - a.kp);
+            ki_out = a.ki + t * (b.ki - a.ki);
+            kd_out = a.kd + t * (b.kd - a.kd);
+            return;
+        }
+    }
+    // Unreachable.
+    kp_out = cfg.base_kp;
+    ki_out = cfg.base_ki;
+    kd_out = cfg.base_kd;
+}
+
+}  // namespace detail_outer
+
+class PidOuter {
+   public:
+    PidOuter() = default;
+    explicit PidOuter(const PidOuterConfig& config) : config_(config) {}
+
+    void reset() { state_ = PidOuterState{}; }
+
+    void update_config(const PidOuterConfig& config) { config_ = config; }
+
+    const PidOuterConfig& config() const { return config_; }
+    const PidOuterState& state() const { return state_; }
+
+    /// One controller tick. Returns the signed rudder setpoint in degrees,
+    /// already clamped to [-max_rudder_deg, +max_rudder_deg] and
+    /// rate-limited.
+    float step(
+        float heading_setpoint_deg,
+        float heading_measured_deg,
+        float rate_of_turn_dps,
+        float speed_kn,
+        bool allowed = true
+    ) {
+        const PidOuterConfig& cfg = config_;
+        PidOuterState& st = state_;
+        const float dt = cfg.dt();
+
+        // Error with shortest-arc + deadband.
+        const float raw_err = detail_outer::heading_error_deg(
+            heading_setpoint_deg, heading_measured_deg
+        );
+        float error;
+        if (raw_err >= -cfg.deadband_deg && raw_err <= cfg.deadband_deg) {
+            error = 0.0f;
+        } else {
+            const float sign = (raw_err > 0.0f) ? 1.0f : -1.0f;
+            error = raw_err - sign * cfg.deadband_deg;
+        }
+
+        if (!allowed) {
+            st.integral *= 0.95f;
+            st.prev_error = error;
+            st.last_output_deg = 0.0f;
+            return 0.0f;
+        }
+
+        float kp = 0.0f, ki = 0.0f, kd = 0.0f;
+        detail_outer::interpolate_gains(cfg, speed_kn, kp, ki, kd);
+
+        const float p_term = kp * error;
+
+        st.integral += ki * error * dt;
+        st.integral = detail_outer::clamp_f(
+            st.integral, -cfg.integral_clamp, cfg.integral_clamp
+        );
+
+        const float d_raw = (dt > 0.0f) ? kd * (error - st.prev_error) / dt
+                                          : 0.0f;
+        const float alpha = detail_outer::lpf_alpha(cfg.d_lpf_tau_s, dt);
+        const float d_term = (1.0f - alpha) * st.prev_d_term + alpha * d_raw;
+        st.prev_d_term = d_term;
+
+        const float rot_ff_term = -cfg.rot_ff_gain * rate_of_turn_dps;
+
+        const float raw_rudder = p_term + st.integral + d_term + rot_ff_term;
+
+        float rudder = detail_outer::clamp_f(raw_rudder, -cfg.max_rudder_deg,
+                                              cfg.max_rudder_deg);
+        if (raw_rudder != rudder) {
+            float aw;
+            if (cfg.aw_gain >= 0.0f) {
+                aw = cfg.aw_gain;
+            } else if (kp != 0.0f) {
+                aw = 1.0f / kp;
+            } else {
+                aw = 0.0f;
+            }
+            st.integral -= aw * (raw_rudder - rudder) * dt;
+            st.integral = detail_outer::clamp_f(
+                st.integral, -cfg.integral_clamp, cfg.integral_clamp
+            );
+        }
+
+        // Rate-limit the produced rudder setpoint.
+        const float max_delta = cfg.rate_limit_dps * dt;
+        const float delta = rudder - st.prev_rudder_setpoint;
+        if (delta > max_delta) {
+            rudder = st.prev_rudder_setpoint + max_delta;
+        } else if (delta < -max_delta) {
+            rudder = st.prev_rudder_setpoint - max_delta;
+        }
+        st.prev_rudder_setpoint = rudder;
+
+        st.prev_error = error;
+        st.last_output_deg = rudder;
+        return rudder;
+    }
+
+   private:
+    PidOuterConfig config_{};
+    PidOuterState state_{};
+};
+
+}  // namespace arautopilot::pid

firmware/ar_autopilot_v1/src/pid/pid_outer_task.cpp

@@ -0,0 +1,173 @@
+// =============================================================================
+//  pid_outer_task.cpp -- 10 Hz outer-loop (heading control) task
+// =============================================================================
+
+#include "pid_outer_task.h"
+
+#include <Arduino.h>
+
+#include "../modes/standby.h"
+#include "../protocols/nmea2000_consumer.h"
+#include "../safety/watchdog.h"
+#include "../system/ar_log.h"
+#include "../system/task_config.h"
+#include "pid_inner_task.h"
+
+namespace arautopilot::pid {
+
+namespace {
+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_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)
+
+void OuterLoopTask(void* /*pv*/) {
+    AR_LOGI(TAG, "pid_outer task started on core %d (10 Hz)", xPortGetCoreID());
+    safety::watchdog_subscribe_current_task();
+
+    TickType_t last_wake = xTaskGetTickCount();
+    for (;;) {
+        // Snapshot inputs we need atomically.
+        float setpoint;
+        float speed_kn;
+        portENTER_CRITICAL(&g_mux);
+        setpoint = g_heading_setpoint_deg;
+        speed_kn = g_speed_kn;
+        portEXIT_CRITICAL(&g_mux);
+
+        const auto n2k = protocols::nmea2000::nmea2000_latest();
+
+        // 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 float rudder_sp = g_outer.step(
+            setpoint,
+            n2k.heading_deg,
+            n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
+            speed_kn,
+            allowed
+        );
+
+        // 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) {
+            pid_inner_set_setpoint_deg(rudder_sp);
+        }
+
+        const float err =
+            n2k.heading_valid
+                ? (setpoint - n2k.heading_deg)
+                : 0.0f;
+
+        portENTER_CRITICAL(&g_mux);
+        g_last_rudder_setpoint_deg = rudder_sp;
+        g_last_error_deg = err;
+        portEXIT_CRITICAL(&g_mux);
+
+        safety::watchdog_feed();
+        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_PID_OUTER));
+    }
+}
+
+}  // namespace
+
+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;
+    g_outer.update_config(cfg);
+    g_outer.reset();
+    AR_LOGI(TAG,
+            "pid_outer_init: schedule={5kn,15kn,28kn} rot_ff=%.2f freq=%.1fHz",
+            cfg.rot_ff_gain, cfg.freq_hz);
+}
+
+void pid_outer_task_start() {
+    xTaskCreatePinnedToCore(OuterLoopTask, "pid_outer", AR_TASK_STACK_PID_OUTER,
+                            nullptr, AR_TASK_PRIO_PID_OUTER, nullptr,
+                            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;
+    while (sp >= 360.0f) sp -= 360.0f;
+    portENTER_CRITICAL(&g_mux);
+    g_heading_setpoint_deg = sp;
+    portEXIT_CRITICAL(&g_mux);
+}
+
+float pid_outer_heading_setpoint_deg() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_heading_setpoint_deg;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+float pid_outer_last_rudder_setpoint_deg() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_last_rudder_setpoint_deg;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+float pid_outer_last_error_deg() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_last_error_deg;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+void pid_outer_set_speed_kn(float speed_kn) {
+    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;
+    portEXIT_CRITICAL(&g_mux);
+}
+
+float pid_outer_speed_kn() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_speed_kn;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+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
+    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);
+}
+
+void pid_outer_get_gains(float& kp, float& ki, float& kd) {
+    portENTER_CRITICAL(&g_mux);
+    const auto& cfg = g_outer.config();
+    kp = cfg.base_kp;
+    ki = cfg.base_ki;
+    kd = cfg.base_kd;
+    portEXIT_CRITICAL(&g_mux);
+}
+
+}  // namespace arautopilot::pid

firmware/ar_autopilot_v1/src/pid/pid_outer_task.h

@@ -0,0 +1,40 @@
+// =============================================================================
+//  pid_outer_task.h -- 10 Hz outer-loop (heading control) task (Sprint 3)
+// =============================================================================
+//
+//  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.
+// =============================================================================
+
+#pragma once
+
+#include "pid_outer.h"
+
+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).
+float pid_outer_heading_setpoint_deg();
+
+/// Read the last rudder setpoint the outer loop produced.
+float pid_outer_last_rudder_setpoint_deg();
+
+/// Read the last heading error the controller saw.
+float pid_outer_last_error_deg();
+
+/// 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);
+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);
+
+}  // namespace arautopilot::pid

firmware/ar_autopilot_v1/src/protocols/modbus_registers.h

@@ -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 = 23;
+constexpr uint16_t INPUT_COUNT = 30;
-constexpr uint16_t INPUT_MAX_ADDR = 45;
+constexpr uint16_t INPUT_MAX_ADDR = 56;
 
 // Firmware major version
 constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
@@ -143,10 +143,31 @@ constexpr uint16_t INPUT_PID_INNER_KI_X1000 = 44;
 // Inner-loop kd * 1000 (unsigned)
 // scale=0.001
 constexpr uint16_t INPUT_PID_INNER_KD_X1000 = 45;
+// Outer-loop heading setpoint, deg*100 (0..35999)
+// unit=deg, scale=0.01
+constexpr uint16_t INPUT_PID_OUTER_HEADING_SP_X100 = 50;
+// Rudder setpoint produced by outer loop, deg*100 (signed int16)
+// unit=deg, scale=0.01
+constexpr uint16_t INPUT_PID_OUTER_RUDDER_SP_X100 = 51;
+// Outer-loop heading error, deg*100 (signed int16)
+// unit=deg, scale=0.01
+constexpr uint16_t INPUT_PID_OUTER_ERROR_X100 = 52;
+// SOG currently used for gain scheduling, knots*10
+// unit=kn, scale=0.1
+constexpr uint16_t INPUT_PID_OUTER_SPEED_KN_X10 = 53;
+// Outer-loop active kp * 1000
+// scale=0.001
+constexpr uint16_t INPUT_PID_OUTER_KP_X1000 = 54;
+// Outer-loop active ki * 1000
+// scale=0.001
+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;
 
 // ----- Holding registers (read-write words) -----
-constexpr uint16_t HOLDING_COUNT = 9;
+constexpr uint16_t HOLDING_COUNT = 14;
-constexpr uint16_t HOLDING_MAX_ADDR = 19;
+constexpr uint16_t HOLDING_MAX_ADDR = 28;
 
 // Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
 constexpr uint16_t HOLDING_MODE_REQUEST = 0;
@@ -174,5 +195,20 @@ constexpr uint16_t HOLDING_PID_INNER_KI_REQ_X1000 = 18;
 // Requested inner-loop kd * 1000 (unsigned)
 // scale=0.001
 constexpr uint16_t HOLDING_PID_INNER_KD_REQ_X1000 = 19;
+// Requested outer-loop heading setpoint, deg*100 (0..35999)
+// unit=deg, scale=0.01
+constexpr uint16_t HOLDING_PID_OUTER_HEADING_SP_REQ_X100 = 24;
+// Requested SOG for gain scheduling, knots*10
+// unit=kn, scale=0.1
+constexpr uint16_t HOLDING_PID_OUTER_SPEED_KN_REQ_X10 = 25;
+// Requested outer-loop base kp * 1000
+// scale=0.001
+constexpr uint16_t HOLDING_PID_OUTER_KP_REQ_X1000 = 26;
+// Requested outer-loop base ki * 1000
+// scale=0.001
+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;
 
 }  // namespace arautopilot::protocols::modbus

firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp

@@ -28,6 +28,7 @@
 #include "../hal/rudder_sensor.h"
 #include "../modes/standby.h"
 #include "../pid/pid_inner_task.h"
+#include "../pid/pid_outer_task.h"
 #include "../system/ar_log.h"
 #include "../system/task_config.h"
 #include "modbus_registers.h"
@@ -60,6 +61,11 @@ struct HoldingStorage {
     uint16_t pid_inner_kp_req_x1000 = 0;
     uint16_t pid_inner_ki_req_x1000 = 0;
     uint16_t pid_inner_kd_req_x1000 = 0;
+    uint16_t pid_outer_heading_sp_req_x100 = 0;
+    uint16_t pid_outer_speed_kn_req_x10 = 150;  // 15.0 kn default
+    uint16_t pid_outer_kp_req_x1000 = 0;
+    uint16_t pid_outer_ki_req_x1000 = 0;
+    uint16_t pid_outer_kd_req_x1000 = 0;
 };
 HoldingStorage g_holding;
 
@@ -163,6 +169,46 @@ uint16_t read_input_register(uint16_t addr) {
             return (uint16_t)scaled;
         }
 
+        // ----- PID outer-loop telemetry (Sprint 3) -----
+        case INPUT_PID_OUTER_HEADING_SP_X100: {
+            int v = (int)(pid::pid_outer_heading_setpoint_deg() * 100.0f);
+            if (v < 0) v = 0;
+            if (v > 35999) v = 35999;
+            return (uint16_t)v;
+        }
+        case INPUT_PID_OUTER_RUDDER_SP_X100: {
+            int v = (int)(pid::pid_outer_last_rudder_setpoint_deg() * 100.0f);
+            if (v < -32768) v = -32768;
+            if (v >  32767) v =  32767;
+            return (uint16_t)(int16_t)v;
+        }
+        case INPUT_PID_OUTER_ERROR_X100: {
+            int v = (int)(pid::pid_outer_last_error_deg() * 100.0f);
+            if (v < -32768) v = -32768;
+            if (v >  32767) v =  32767;
+            return (uint16_t)(int16_t)v;
+        }
+        case INPUT_PID_OUTER_SPEED_KN_X10: {
+            int v = (int)(pid::pid_outer_speed_kn() * 10.0f);
+            if (v < 0) v = 0;
+            if (v > 65535) v = 65535;
+            return (uint16_t)v;
+        }
+        case INPUT_PID_OUTER_KP_X1000:
+        case INPUT_PID_OUTER_KI_X1000:
+        case INPUT_PID_OUTER_KD_X1000: {
+            float kp, ki, kd;
+            pid::pid_outer_get_gains(kp, ki, kd);
+            float v;
+            if (addr == INPUT_PID_OUTER_KP_X1000)      v = kp;
+            else if (addr == INPUT_PID_OUTER_KI_X1000) v = ki;
+            else                                        v = kd;
+            int scaled = (int)(v * 1000.0f);
+            if (scaled < 0) scaled = 0;
+            if (scaled > 65535) scaled = 65535;
+            return (uint16_t)scaled;
+        }
+
         default:
             return 0;
     }
@@ -207,6 +253,11 @@ uint16_t read_holding(uint16_t addr) {
         case HOLDING_PID_INNER_KP_REQ_X1000: return g_holding.pid_inner_kp_req_x1000;
         case HOLDING_PID_INNER_KI_REQ_X1000: return g_holding.pid_inner_ki_req_x1000;
         case HOLDING_PID_INNER_KD_REQ_X1000: return g_holding.pid_inner_kd_req_x1000;
+        case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: return g_holding.pid_outer_heading_sp_req_x100;
+        case HOLDING_PID_OUTER_SPEED_KN_REQ_X10:    return g_holding.pid_outer_speed_kn_req_x10;
+        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;
         default:                             return 0;
     }
 }
@@ -241,6 +292,37 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) {
             pid::pid_inner_set_setpoint_deg((float)sv * 0.01f);
             return Modbus::Error::SUCCESS;
         }
+        case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: {
+            if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
+            g_holding.pid_outer_heading_sp_req_x100 = value;
+            pid::pid_outer_set_heading_setpoint_deg((float)value * 0.01f);
+            return Modbus::Error::SUCCESS;
+        }
+        case HOLDING_PID_OUTER_SPEED_KN_REQ_X10: {
+            if (value > 800) return Modbus::Error::ILLEGAL_DATA_VALUE;  // 80 kn cap
+            g_holding.pid_outer_speed_kn_req_x10 = value;
+            pid::pid_outer_set_speed_kn((float)value * 0.1f);
+            return Modbus::Error::SUCCESS;
+        }
+        case HOLDING_PID_OUTER_KP_REQ_X1000:
+        case HOLDING_PID_OUTER_KI_REQ_X1000:
+        case HOLDING_PID_OUTER_KD_REQ_X1000: {
+            if (addr == HOLDING_PID_OUTER_KP_REQ_X1000) {
+                g_holding.pid_outer_kp_req_x1000 = value;
+            } else if (addr == HOLDING_PID_OUTER_KI_REQ_X1000) {
+                g_holding.pid_outer_ki_req_x1000 = value;
+            } else {
+                g_holding.pid_outer_kd_req_x1000 = value;
+            }
+            float kp = (float)g_holding.pid_outer_kp_req_x1000 * 0.001f;
+            float ki = (float)g_holding.pid_outer_ki_req_x1000 * 0.001f;
+            float kd = (float)g_holding.pid_outer_kd_req_x1000 * 0.001f;
+            if (kp <= 0.0f) {
+                return Modbus::Error::ILLEGAL_DATA_VALUE;
+            }
+            pid::pid_outer_update_gains(kp, ki, kd);
+            return Modbus::Error::SUCCESS;
+        }
         case HOLDING_PID_INNER_KP_REQ_X1000:
         case HOLDING_PID_INNER_KI_REQ_X1000:
         case HOLDING_PID_INNER_KD_REQ_X1000: {