From 42ee63b776118a541c1fd5ad9b23f1721283296e Mon Sep 17 00:00:00 2001 From: alro1965 Date: Mon, 18 May 2026 18:20:23 -0400 Subject: [PATCH] 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) --- CHANGELOG.md | 76 ++++++ arautopilot/shared/modbus_register_map.py | 12 + arautopilot/studio/simulator/pid_outer.py | 220 ++++++++++++++++ .../studio/simulator/vessel_heading.py | 153 +++++++++++ arautopilot/tests/test_pid_outer_python.py | 226 +++++++++++++++++ .../tests/test_vessel_heading_simulator.py | 85 +++++++ docs/sprint-3-plan.md | 59 +++++ .../ar_autopilot_v1/modbus_registers.yaml | 16 ++ firmware/ar_autopilot_v1/src/main.cpp | 6 + .../ar_autopilot_v1/src/modes/standby.cpp | 39 ++- firmware/ar_autopilot_v1/src/pid/pid_outer.h | 240 ++++++++++++++++++ .../src/pid/pid_outer_task.cpp | 173 +++++++++++++ .../ar_autopilot_v1/src/pid/pid_outer_task.h | 40 +++ .../src/protocols/modbus_registers.h | 44 +++- .../src/protocols/modbus_slave.cpp | 82 ++++++ 15 files changed, 1462 insertions(+), 9 deletions(-) create mode 100644 arautopilot/studio/simulator/pid_outer.py create mode 100644 arautopilot/studio/simulator/vessel_heading.py create mode 100644 arautopilot/tests/test_pid_outer_python.py create mode 100644 arautopilot/tests/test_vessel_heading_simulator.py create mode 100644 docs/sprint-3-plan.md create mode 100644 firmware/ar_autopilot_v1/src/pid/pid_outer.h create mode 100644 firmware/ar_autopilot_v1/src/pid/pid_outer_task.cpp create mode 100644 firmware/ar_autopilot_v1/src/pid/pid_outer_task.h diff --git a/CHANGELOG.md b/CHANGELOG.md index 75d6de7..3bf78d3 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/arautopilot/shared/modbus_register_map.py b/arautopilot/shared/modbus_register_map.py index a794f47..66f4fe7 100644 --- a/arautopilot/shared/modbus_register_map.py +++ b/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]] = { diff --git a/arautopilot/studio/simulator/pid_outer.py b/arautopilot/studio/simulator/pid_outer.py new file mode 100644 index 0000000..ed0fd11 --- /dev/null +++ b/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) diff --git a/arautopilot/studio/simulator/vessel_heading.py b/arautopilot/studio/simulator/vessel_heading.py new file mode 100644 index 0000000..f3b443e --- /dev/null +++ b/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)) diff --git a/arautopilot/tests/test_pid_outer_python.py b/arautopilot/tests/test_pid_outer_python.py new file mode 100644 index 0000000..dfe581e --- /dev/null +++ b/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 diff --git a/arautopilot/tests/test_vessel_heading_simulator.py b/arautopilot/tests/test_vessel_heading_simulator.py new file mode 100644 index 0000000..883ed94 --- /dev/null +++ b/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) diff --git a/docs/sprint-3-plan.md b/docs/sprint-3-plan.md new file mode 100644 index 0000000..8130bf7 --- /dev/null +++ b/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 diff --git a/firmware/ar_autopilot_v1/modbus_registers.yaml b/firmware/ar_autopilot_v1/modbus_registers.yaml index 5cd7a91..550c490 100644 --- a/firmware/ar_autopilot_v1/modbus_registers.yaml +++ b/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 } diff --git a/firmware/ar_autopilot_v1/src/main.cpp b/firmware/ar_autopilot_v1/src/main.cpp index e8bbeda..b52bcef 100644 --- a/firmware/ar_autopilot_v1/src/main.cpp +++ b/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. diff --git a/firmware/ar_autopilot_v1/src/modes/standby.cpp b/firmware/ar_autopilot_v1/src/modes/standby.cpp index 7301511..b1bd338 100644 --- a/firmware/ar_autopilot_v1/src/modes/standby.cpp +++ b/firmware/ar_autopilot_v1/src/modes/standby.cpp @@ -6,6 +6,9 @@ #include +#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 " - "Sprint 1; current mode remains %s", + "request_mode(%s) rejected: mode not yet implemented " + "(Sprint 5+); current mode remains %s", mode_name(m), mode_name(current_mode())); return false; } diff --git a/firmware/ar_autopilot_v1/src/pid/pid_outer.h b/firmware/ar_autopilot_v1/src/pid/pid_outer.h new file mode 100644 index 0000000..b48c5cd --- /dev/null +++ b/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 + +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 diff --git a/firmware/ar_autopilot_v1/src/pid/pid_outer_task.cpp b/firmware/ar_autopilot_v1/src/pid/pid_outer_task.cpp new file mode 100644 index 0000000..49007f4 --- /dev/null +++ b/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 + +#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 diff --git a/firmware/ar_autopilot_v1/src/pid/pid_outer_task.h b/firmware/ar_autopilot_v1/src/pid/pid_outer_task.h new file mode 100644 index 0000000..2077cb3 --- /dev/null +++ b/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 diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_registers.h b/firmware/ar_autopilot_v1/src/protocols/modbus_registers.h index f06e12b..b798998 100644 --- a/firmware/ar_autopilot_v1/src/protocols/modbus_registers.h +++ b/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_MAX_ADDR = 45; +constexpr uint16_t INPUT_COUNT = 30; +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_MAX_ADDR = 19; +constexpr uint16_t HOLDING_COUNT = 14; +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 diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp index be01e9b..70734eb 100644 --- a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp +++ b/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: {