alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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>
This commit is contained in:
Alvaro Enrique Romero Donado
2026-05-18 18:20:23 -04:00
parent 13a2867ef6
commit 42ee63b776
15 changed files with 1462 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
arautopilot/studio/simulator/vessel_heading.py
+153
View File
@@ -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))