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>
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user