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:
@@ -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]] = {
|
||||
|
||||
@@ -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)
|
||||
@@ -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))
|
||||
@@ -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
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user