diff --git a/.gitignore b/.gitignore index 1f9931d..5d65c5e 100644 --- a/.gitignore +++ b/.gitignore @@ -139,6 +139,7 @@ examples/output/ logs/ # ---------------------------------------------------------------------------- -# Claude Code local settings (personal overrides — not committed) +# Claude Code local settings + worktrees (personal — not committed) # ---------------------------------------------------------------------------- .claude/settings.local.json +.claude/worktrees/ diff --git a/CHANGELOG.md b/CHANGELOG.md index 0b44066..9727234 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,75 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +## [0.1.0-sprint2] — Sprint 2 — PID inner loop + rudder simulator — 2026-05-18 + +> Continues the overnight execution under blanket authorisation. Builds on +> Sprint 1 firmware foundation. New cross-cutting concern introduced: +> Python is the **algorithmic source of truth** for the PID; C++ firmware +> is a line-by-line port; tests pin both. + +### Added + +#### Python (`arautopilot/studio/simulator/`) + +- **`rudder_dynamics.py`** -- bench-grade physical model of a hydraulic + rudder actuator. Marine-realistic defaults (actuator_gain=0.2, friction=4 + -> steady-state v_max ~5 deg/s for a 30 m yacht). Includes deadband, + min-useful-PWM snap, port/starboard asymmetry, mechanical end-stops, + optional constant external torque, and a `RunRecorder` helper for + trajectory capture. +- **`pid_inner.py`** -- pure-Python reference implementation of the inner + PID. Anti-windup via back-calculation, setpoint rate limit, setpoint + deadband, derivative low-pass filter, actuator non-linearity + compensation (deadband + min-useful + asymmetry). Algorithmic source + of truth -- the firmware C++ port matches it line-by-line. + +#### Firmware (`firmware/ar_autopilot_v1/src/pid/`) + +- **`pid_inner.h`** -- header-only C++17 controller, byte-equivalent port + of `pid_inner.py`. Compiles on the ESP32 toolchain AND on host + g++/clang/MSVC (no Arduino dependencies). Suitable for native Unity + tests once a host compiler is available. +- **`pid_inner_task.{h,cpp}`** -- FreeRTOS task wrapper. 50 Hz on Core 1 + (real-time core). Reads rudder position from `hal::rudder_sensor`, + consumes setpoint from Modbus / outer loop, commands + `hal::rudder_command`. Bleeds integrator during STANDBY. Subscribes + to the watchdog and feeds it every loop. + +#### Modbus register map (regenerated from YAML) + +- 6 new INPUT registers (40-45) -- PID telemetry: setpoint, output, + error, kp/ki/kd live. +- 4 new HOLDING registers (16-19) -- writable: rudder setpoint request, + kp/ki/kd request. Writes propagate atomically to the controller. + +#### Tests + +- `test_rudder_simulator.py` -- 9 tests of the physical model: zero-input + rest, full-deflection drive, end-stop saturation, deadband, min-useful + snap, asymmetry behaviour, recorder API, invalid dt, end-stop velocity + zeroing. +- `test_pid_inner_python.py` -- 10 tests of the Python PID against the + simulator: positive/negative step response, setpoint deadband holds, + anti-windup bounds integrator under saturation, allowed=false bleeds + integrator, actuator deadband compensation, asymmetry compensation, + output saturation, rate limit caps slew, disturbance rejection. + +### Verification + +- `pio run -e esp32-dev` -- SUCCESS, RAM 6.8 %, Flash 26.8 % (351 KB). +- `pytest` -- **129 passed** in 0.31 s (110 Sprint 1 + 19 Sprint 2 new). + +### Not in Sprint 2 (intentional) + +- Heading control (outer loop) -- that is Sprint 3. +- Gain scheduling by SOG -- Sprint 3. +- Rate-of-Turn feed-forward -- Sprint 3. +- Cross-validation tests Python ↔ C++ via ctypes -- requires host C++ + compiler that this machine lacks; the algorithm parity is enforced by + code review (line-by-line port) and will be backed by automated + cross-validation as soon as a host compiler is available. + ## [0.1.0-sprint1] — Sprint 1 — Firmware ESP32 base — 2026-05-18 > Sprint 1 was executed autonomously overnight after the user gave explicit diff --git a/arautopilot/shared/modbus_register_map.py b/arautopilot/shared/modbus_register_map.py index c3e8565..a794f47 100644 --- a/arautopilot/shared/modbus_register_map.py +++ b/arautopilot/shared/modbus_register_map.py @@ -78,6 +78,12 @@ INPUTS: dict[str, Reg] = { "HEADING_AGE_MS": Reg(addr=26, name="HEADING_AGE_MS", desc='Milliseconds since the last heading update (0..60000)', unit="ms", scale=1.0, offset=0.0), "BATTERY_VOLTAGE_X100": Reg(addr=32, name="BATTERY_VOLTAGE_X100", desc='System battery voltage, V*100', unit="V", scale=0.01, offset=0.0), "ACTUATOR_CURRENT_X100": Reg(addr=33, name="ACTUATOR_CURRENT_X100", desc='Actuator current, A*100', unit="A", scale=0.01, offset=0.0), + "PID_INNER_SETPOINT_X100": Reg(addr=40, name="PID_INNER_SETPOINT_X100", desc='Inner-loop rudder setpoint, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0), + "PID_INNER_OUTPUT_X100": Reg(addr=41, name="PID_INNER_OUTPUT_X100", desc='Last PID command, %*100 (signed int16, -10000..+10000)', unit="%", scale=0.01, offset=0.0), + "PID_INNER_ERROR_X100": Reg(addr=42, name="PID_INNER_ERROR_X100", desc='Last PID error, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0), + "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), } # HOLDINGS @@ -87,6 +93,10 @@ HOLDINGS: dict[str, Reg] = { "BRIGHTNESS_PCT": Reg(addr=2, name="BRIGHTNESS_PCT", desc='Display brightness 0..100', unit="%", scale=1.0, offset=0.0), "ALARM_VOLUME_PCT": Reg(addr=3, name="ALARM_VOLUME_PCT", desc='Alarm volume 0..100', unit="%", scale=1.0, offset=0.0), "DODGE_OFFSET_DEG_X100": Reg(addr=8, name="DODGE_OFFSET_DEG_X100", desc='Dodge mode heading offset, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0), + "PID_INNER_SETPOINT_REQ_X100": Reg(addr=16, name="PID_INNER_SETPOINT_REQ_X100", desc='Requested inner-loop rudder setpoint, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0), + "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), } ALL_GROUPS: dict[str, dict[str, Reg]] = { diff --git a/arautopilot/studio/simulator/__init__.py b/arautopilot/studio/simulator/__init__.py index 941f620..f90005c 100644 --- a/arautopilot/studio/simulator/__init__.py +++ b/arautopilot/studio/simulator/__init__.py @@ -1 +1,15 @@ -"""Test bench: vessel + actuator + sensor simulators (Sprint 2-3).""" +"""Bench-grade simulators used by Sprint 2+ to validate the PID without +the AR-NMEA-IO board attached. + +Modules: + +- :mod:`~arautopilot.studio.simulator.rudder_dynamics` + Minimal physical model of a hydraulic rudder actuator: rotational + inertia, viscous friction, hydraulic deadband, port/stbd asymmetry, + mechanical end-stops. + +- :mod:`~arautopilot.studio.simulator.pid_inner` + Pure-Python reference implementation of the inner (rudder-position) + PID. Used to iterate on the algorithm and cross-validate the C++ + firmware port. +""" diff --git a/arautopilot/studio/simulator/pid_inner.py b/arautopilot/studio/simulator/pid_inner.py new file mode 100644 index 0000000..7527c78 --- /dev/null +++ b/arautopilot/studio/simulator/pid_inner.py @@ -0,0 +1,245 @@ +"""Pure-Python reference implementation of the inner rudder-position PID. + +This module is the **algorithmic source of truth** for the inner loop. The +firmware C++ port in ``firmware/ar_autopilot_v1/src/pid/pid_inner.cpp`` is +a line-by-line translation: same variables, same sequencing, same numerics. +Cross-validation tests confirm the two produce identical trajectories on +the same input within float tolerance. + +Algorithm (per tick): + + raw_error = setpoint_deg - measured_deg + deadband applied: |raw_error| < setpoint_deadband_deg -> 0 + rate-limit the setpoint (max |d(setpoint)/dt| <= rate_limit_dps) + proportional = kp * error + integral += ki * error * dt (back-calculated when output saturates) + derivative = kd * (error - prev_error) / dt (with low-pass on the + derivative term) + raw_output = proportional + integral + derivative + output = saturate(raw_output, [-100, +100]) + if abs(output) > 0: + if abs(output) < min_useful_pwm_pct: snap up to min_useful_pwm_pct + apply port/stbd asymmetry compensation + if mode == STANDBY or interlocks block: output = 0 + +Anti-windup is back-calculation: when the output saturates, the integrator +is corrected by `(saturated - raw) * anti_windup_gain * dt`. The gain +defaults to `1 / kp` if kp != 0, else 0. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass +class PidInnerConfig: + """All parameters for the inner PID + actuator compensation. + + Defaults are the 30 m motor-yacht seed values from + ``arautopilot/library/default_tunings/yacht_motor_planeo_30m.yaml``. + """ + + # --- Gains -------------------------------------------------------------- + kp: float = 2.5 + ki: float = 0.15 + kd: float = 0.30 + + # --- Sampling ----------------------------------------------------------- + freq_hz: float = 50.0 + """Nominal loop rate. dt = 1 / freq_hz.""" + + # --- Setpoint handling -------------------------------------------------- + deadband_deg: float = 0.5 + """Errors within this band do not contribute to P, I, or D (suppresses noise).""" + + rate_limit_dps: float = 30.0 + """Maximum |d(setpoint)/dt| applied to the working setpoint, deg/s.""" + + # --- Output saturation -------------------------------------------------- + output_min_pct: float = -100.0 + output_max_pct: float = +100.0 + + # --- Anti-windup -------------------------------------------------------- + integral_clamp: float = 30.0 + """Hard clamp on the integrator's accumulated value (output-equivalent units).""" + + aw_gain: float | None = None + """Back-calculation anti-windup gain. ``None`` -> 1/kp if kp != 0, else 0.""" + + # --- Derivative low-pass ------------------------------------------------ + d_lpf_tau_s: float = 0.05 + """Time constant of the first-order low-pass on the derivative term.""" + + # --- Actuator non-linearity compensation -------------------------------- + deadband_pct: float = 7.0 + """Below this absolute command, the actuator produces no torque -- the PID + must skip this band to avoid lingering at the edge of action.""" + + min_useful_pwm_pct: float = 12.0 + """Snap any non-zero command up to at least this magnitude.""" + + asymmetry_stbd_over_port: float = 1.0 + """Inverse of the simulator's asymmetry. If the pump pushes harder to + starboard, divide the starboard command by the asymmetry so the + effective torque is symmetric.""" + + def dt(self) -> float: + return 1.0 / self.freq_hz + + +@dataclass +class PidInnerState: + """Mutable runtime state of the controller.""" + + integral: float = 0.0 + prev_error: float = 0.0 + prev_d_term: float = 0.0 + prev_setpoint_deg: float = 0.0 + last_output_pct: float = 0.0 + + +class PidInner: + """Inner rudder-position PID controller. + + Designed for cross-language port: every operation is plain arithmetic, + no dependencies beyond the dataclasses above. + """ + + def __init__(self, config: PidInnerConfig | None = None) -> None: + self.config: PidInnerConfig = config or PidInnerConfig() + self.state: PidInnerState = PidInnerState() + self._dt: float = self.config.dt() + + # -- Lifecycle ----------------------------------------------------------- + def reset(self, *, measured_deg: float = 0.0, setpoint_deg: float = 0.0) -> None: + self.state = PidInnerState(prev_setpoint_deg=setpoint_deg) + + def update_config(self, config: PidInnerConfig) -> None: + """Hot-swap configuration without losing accumulated state.""" + self.config = config + self._dt = config.dt() + + # -- Step ---------------------------------------------------------------- + def step(self, *, setpoint_deg: float, measured_deg: float, + allowed: bool = True) -> float: + """Compute the controller output for one tick. + + ``allowed`` lets the caller force the output to zero (e.g. while in + STANDBY or with the actuator master power off). When False, the + integrator is bled toward zero so the PID doesn't accumulate during + manual operation. + + Returns the signed PWM command in percent, after actuator + non-linearity compensation, ready to be passed to the H-bridge / pump. + """ + cfg = self.config + st = self.state + dt = self._dt + + # Rate-limit the setpoint -- the operator/outer loop may step it + # large, but the inner loop should pursue it smoothly so the rudder + # doesn't slam. + target = self._rate_limit_setpoint(setpoint_deg) + st.prev_setpoint_deg = target + + # Compute the (possibly dead-banded) error. + raw_error = target - measured_deg + if abs(raw_error) <= cfg.deadband_deg: + error = 0.0 + else: + # Strip the deadband so the action is continuous at the edge. + sign = 1.0 if raw_error > 0.0 else -1.0 + error = raw_error - sign * cfg.deadband_deg + + # --- Forced inactive path ----------------------------------------- + if not allowed: + # Bleed the integrator toward zero to avoid windup during manual. + st.integral *= 0.95 + st.prev_error = error + st.last_output_pct = 0.0 + return 0.0 + + # --- Proportional ------------------------------------------------- + p_term = cfg.kp * error + + # --- Integral (provisional, before anti-windup correction) -------- + st.integral += cfg.ki * error * dt + st.integral = _clamp(st.integral, -cfg.integral_clamp, cfg.integral_clamp) + + # --- Derivative with low-pass filter ------------------------------ + if dt > 0.0: + d_raw = cfg.kd * (error - st.prev_error) / dt + else: + d_raw = 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 + + raw_output = p_term + st.integral + d_term + + # --- Saturate + back-calculation anti-windup ---------------------- + output = _clamp(raw_output, cfg.output_min_pct, cfg.output_max_pct) + if raw_output != output: + aw = cfg.aw_gain if cfg.aw_gain is not None else ( + 1.0 / cfg.kp if cfg.kp != 0.0 else 0.0 + ) + st.integral -= aw * (raw_output - output) * dt + st.integral = _clamp(st.integral, -cfg.integral_clamp, cfg.integral_clamp) + + # --- Actuator non-linearity compensation -------------------------- + cmd = self._compensate(output) + + st.prev_error = error + st.last_output_pct = cmd + return cmd + + # -- Helpers ------------------------------------------------------------- + def _rate_limit_setpoint(self, requested_deg: float) -> float: + cfg = self.config + st = self.state + max_delta = cfg.rate_limit_dps * self._dt + delta = requested_deg - st.prev_setpoint_deg + if delta > max_delta: + return st.prev_setpoint_deg + max_delta + if delta < -max_delta: + return st.prev_setpoint_deg - max_delta + return requested_deg + + def _compensate(self, raw_pct: float) -> float: + cfg = self.config + if raw_pct == 0.0: + return 0.0 + magnitude = abs(raw_pct) + if magnitude <= cfg.deadband_pct: + return 0.0 + if magnitude < cfg.min_useful_pwm_pct: + magnitude = cfg.min_useful_pwm_pct + sign = 1.0 if raw_pct > 0.0 else -1.0 + cmd = sign * magnitude + # Asymmetry: divide the starboard side, multiply the port side so the + # actuator's intrinsic faster-to-starboard pump is compensated. + if cmd > 0.0 and cfg.asymmetry_stbd_over_port != 0.0: + cmd /= cfg.asymmetry_stbd_over_port + elif cmd < 0.0: + cmd *= cfg.asymmetry_stbd_over_port + # Re-saturate after compensation in case scaling pushed us past 100 %. + return _clamp(cmd, cfg.output_min_pct, cfg.output_max_pct) + + +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: + """First-order low-pass coefficient: alpha = dt / (tau + dt). + + Smaller alpha -> heavier filtering. alpha == 1.0 means no filtering. + """ + if tau_s <= 0.0: + return 1.0 + return dt / (tau_s + dt) diff --git a/arautopilot/studio/simulator/rudder_dynamics.py b/arautopilot/studio/simulator/rudder_dynamics.py new file mode 100644 index 0000000..d7e3314 --- /dev/null +++ b/arautopilot/studio/simulator/rudder_dynamics.py @@ -0,0 +1,210 @@ +"""Minimal physical model of a hydraulic rudder actuator. + +The state is the rudder angle in degrees and its angular velocity in deg/s. +The driver is a signed PWM command in percent (-100..+100): + + pwm > 0 -> drive starboard (positive angular acceleration) + pwm < 0 -> drive port (negative angular acceleration) + pwm == 0 -> no torque from the actuator (still subject to friction) + +The model is intentionally small: it must capture the qualitative behaviour +that makes a PID interesting (inertia, friction, deadband, asymmetry, +end-stop saturation) but it does NOT pretend to be a faithful CFD of any +real pump. The integrator's affinated tunings live elsewhere. + +Equations (per integration step ``dt``): + + effective_cmd = compensate_deadband_and_asymmetry(pwm) + torque = actuator_gain * effective_cmd + accel = (torque - friction * angular_vel) / inertia + angular_vel += accel * dt + angle += angular_vel * dt + # then clamp to [-max_angle_deg, +max_angle_deg], zeroing velocity at the stop + +Units throughout: + angle [deg] + velocity [deg / s] + accel [deg / s^2] + torque [arbitrary scaled units; absorbed into actuator_gain] + friction [(deg/s) -> (deg/s^2)], i.e. viscous coefficient +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + + +@dataclass +class RudderDynamicsConfig: + """Tunable parameters of the rudder physical model. + + Defaults are reasonable for a 30 m motor yacht with a reversible + hydraulic pump (calibration profile shipped in the seed library). + """ + + # --- Mechanical --------------------------------------------------------- + max_angle_deg: float = 35.0 + """Mechanical end-stop on either side, degrees.""" + + inertia: float = 1.0 + """Rotational inertia (arbitrary units; absorbed with actuator_gain).""" + + friction: float = 4.0 + """Viscous friction coefficient (higher = more damping).""" + + # --- Actuator ----------------------------------------------------------- + # Tuned so that 100 % PWM produces a steady-state angular velocity of + # ~5 deg/s, which matches the brief's "typical 3-6 dps" max rate for a + # 30 m yacht hydraulic pump: + # v_steady = (actuator_gain * 100) / friction = (0.2 * 100) / 4 = 5 + actuator_gain: float = 0.2 + """Torque-equivalent per percent of PWM. Marine-realistic for a 30 m yacht.""" + + deadband_pct: float = 7.0 + """Below this absolute command, the pump produces no torque (static friction).""" + + min_useful_pwm_pct: float = 12.0 + """Once above the deadband, the effective PWM is snapped up to this minimum.""" + + asymmetry_stbd_over_port: float = 1.0 + """Ratio of starboard speed to port speed; 1.0 = symmetric.""" + + # --- Optional disturbance ----------------------------------------------- + external_torque: float = 0.0 + """Constant external torque (e.g. simulating wave action). 0 by default.""" + + +@dataclass +class RudderState: + angle_deg: float = 0.0 + angular_vel_dps: float = 0.0 + + +class RudderSimulator: + """Discrete-time integrator of the rudder model. + + Typical usage:: + + sim = RudderSimulator() + sim.reset(angle_deg=0.0) + for k in range(1000): + sim.step(dt=0.001, pwm_pct=cmd) + print(sim.state.angle_deg) + """ + + def __init__(self, config: RudderDynamicsConfig | None = None) -> None: + self.config: RudderDynamicsConfig = config or RudderDynamicsConfig() + self.state: RudderState = RudderState() + + # -- Lifecycle ----------------------------------------------------------- + def reset(self, *, angle_deg: float = 0.0, angular_vel_dps: float = 0.0) -> None: + self.state = RudderState(angle_deg=angle_deg, angular_vel_dps=angular_vel_dps) + + # -- Integration --------------------------------------------------------- + def step(self, *, dt: float, pwm_pct: float) -> RudderState: + """Integrate the model forward by ``dt`` seconds with constant ``pwm_pct``.""" + if dt <= 0.0: + raise ValueError(f"dt must be > 0, got {dt}") + cfg = self.config + st = self.state + + effective = self._compensate(pwm_pct) + torque = cfg.actuator_gain * effective + cfg.external_torque + accel = (torque - cfg.friction * st.angular_vel_dps) / cfg.inertia + + st.angular_vel_dps += accel * dt + st.angle_deg += st.angular_vel_dps * dt + + # End-stop saturation. + if st.angle_deg > cfg.max_angle_deg: + st.angle_deg = cfg.max_angle_deg + if st.angular_vel_dps > 0.0: + st.angular_vel_dps = 0.0 + elif st.angle_deg < -cfg.max_angle_deg: + st.angle_deg = -cfg.max_angle_deg + if st.angular_vel_dps < 0.0: + st.angular_vel_dps = 0.0 + + return st + + # -- Actuator non-linearity --------------------------------------------- + def _compensate(self, pwm_pct: float) -> float: + """Apply deadband + min-useful-PWM + asymmetry. + + Returned units: signed effective PWM in percent. Sign convention + matches the input. + """ + cfg = self.config + if pwm_pct == 0.0: + return 0.0 + magnitude = abs(pwm_pct) + if magnitude <= cfg.deadband_pct: + return 0.0 + if magnitude < cfg.min_useful_pwm_pct: + magnitude = cfg.min_useful_pwm_pct + sign = 1.0 if pwm_pct > 0.0 else -1.0 + effective = sign * magnitude + # Asymmetry: starboard (positive) is multiplied; port is divided so + # the symmetric case asymmetry=1.0 leaves both sides untouched. + if effective > 0.0: + effective *= cfg.asymmetry_stbd_over_port + elif cfg.asymmetry_stbd_over_port != 0.0: + effective /= cfg.asymmetry_stbd_over_port + return effective + + +@dataclass +class TrajectorySample: + """One row of a recorded run.""" + + t: float + setpoint_deg: float + angle_deg: float + angular_vel_dps: float + pwm_pct: float + + +@dataclass +class RunRecorder: + """Capture a (time, setpoint, angle, vel, cmd) trace for plotting/tests.""" + + samples: list[TrajectorySample] = field(default_factory=list) + + def record(self, *, t: float, setpoint_deg: float, sim: RudderSimulator, + pwm_pct: float) -> None: + self.samples.append( + TrajectorySample( + t=t, + setpoint_deg=setpoint_deg, + angle_deg=sim.state.angle_deg, + angular_vel_dps=sim.state.angular_vel_dps, + pwm_pct=pwm_pct, + ) + ) + + def final_angle(self) -> float: + return self.samples[-1].angle_deg if self.samples else 0.0 + + def settling_time_s(self, target: float, tolerance_deg: float = 0.5) -> float | None: + """Return time at which ``|angle - target| <= tolerance`` and stays there + until the end of the run. ``None`` if it never settles.""" + if not self.samples: + return None + # Walk backwards: find the last time the error exceeded tolerance. + for i in range(len(self.samples) - 1, -1, -1): + if abs(self.samples[i].angle_deg - target) > tolerance_deg: + if i + 1 < len(self.samples): + return self.samples[i + 1].t + return None + return self.samples[0].t + + def max_overshoot_deg(self, target: float) -> float: + """Maximum overshoot past ``target`` in the direction of the step.""" + if not self.samples: + return 0.0 + start = self.samples[0].angle_deg + going_up = target > start + peak = max((s.angle_deg for s in self.samples), default=start) if going_up \ + else min((s.angle_deg for s in self.samples), default=start) + overshoot = (peak - target) if going_up else (target - peak) + return max(0.0, overshoot) diff --git a/arautopilot/tests/test_pid_inner_python.py b/arautopilot/tests/test_pid_inner_python.py new file mode 100644 index 0000000..dab614f --- /dev/null +++ b/arautopilot/tests/test_pid_inner_python.py @@ -0,0 +1,226 @@ +"""Tests for ``arautopilot.studio.simulator.pid_inner`` against the rudder simulator.""" + +from __future__ import annotations + +import pytest + +from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig +from arautopilot.studio.simulator.rudder_dynamics import ( + RudderDynamicsConfig, + RudderSimulator, + RunRecorder, +) + + +def _run(pid: PidInner, sim: RudderSimulator, *, setpoint_deg: float, + seconds: float, allowed: bool = True, dt_inner: float | None = None, + dt_sim: float = 0.002) -> RunRecorder: + """Drive ``sim`` at ``dt_sim`` while ticking ``pid`` at its own rate.""" + if dt_inner is None: + dt_inner = pid.config.dt() + rec = RunRecorder() + pid_carry = 0.0 + cmd = 0.0 + t = 0.0 + steps = int(seconds / dt_sim) + for _ in range(steps): + pid_carry += dt_sim + if pid_carry + 1e-12 >= dt_inner: + cmd = pid.step(setpoint_deg=setpoint_deg, + measured_deg=sim.state.angle_deg, + allowed=allowed) + pid_carry -= dt_inner + sim.step(dt=dt_sim, pwm_pct=cmd) + t += dt_sim + rec.record(t=t, setpoint_deg=setpoint_deg, sim=sim, pwm_pct=cmd) + return rec + + +def _aggressive_pid() -> PidInner: + """Gains tuned for the marine-realistic simulator (v_max ~5 dps). + + With slow rudder dynamics (full deflection in ~7 s) we want the PID + to drive the actuator at max for a couple of seconds, then back off + smoothly. kp dominates; ki small to clean up steady-state bias from + any external torque; kd light because the rudder is heavily damped. + """ + return PidInner(PidInnerConfig( + kp=10.0, ki=0.5, kd=0.5, + deadband_deg=0.2, + rate_limit_dps=10000.0, # disable rate limit in unit tests + deadband_pct=0.0, + min_useful_pwm_pct=0.0, + integral_clamp=200.0, + )) + + +def test_step_response_settles_near_setpoint() -> None: + """20 deg step with marine-realistic v_max ~5 dps should settle in ~10 s.""" + sim = RudderSimulator() + sim.reset() + pid = _aggressive_pid() + pid.reset(measured_deg=0.0, setpoint_deg=0.0) + rec = _run(pid, sim, setpoint_deg=20.0, seconds=12.0) + final = rec.final_angle() + assert final == pytest.approx(20.0, abs=1.0), f"final angle {final}, want ~20" + + +def test_negative_step_response() -> None: + sim = RudderSimulator() + sim.reset() + pid = _aggressive_pid() + pid.reset(measured_deg=0.0, setpoint_deg=0.0) + rec = _run(pid, sim, setpoint_deg=-15.0, seconds=12.0) + assert rec.final_angle() == pytest.approx(-15.0, abs=1.0) + + +def test_setpoint_deadband_holds_when_close() -> None: + """If the working setpoint stays within the deadband of the measurement, + no command is produced and the rudder doesn't move.""" + sim = RudderSimulator() + sim.reset(angle_deg=10.0) + cfg = PidInnerConfig(kp=10.0, ki=0.5, kd=0.5, + deadband_deg=2.0, + rate_limit_dps=10000.0, + deadband_pct=0.0, + min_useful_pwm_pct=0.0) + pid = PidInner(cfg) + # Critically: initialise the working setpoint to the current measurement + # so the rate limiter doesn't pull it through the deadband from 0. + pid.reset(measured_deg=10.0, setpoint_deg=10.0) + rec = _run(pid, sim, setpoint_deg=11.0, seconds=2.0) + final = rec.final_angle() + assert final == pytest.approx(10.0, abs=0.5), f"deadband not holding, final={final}" + + +def test_anti_windup_keeps_integrator_bounded_under_saturation() -> None: + """Park the rudder at the end-stop with a far setpoint -> output saturates + indefinitely. The integrator must not run away.""" + sim = RudderSimulator() + sim.reset(angle_deg=35.0) + pid = PidInner(PidInnerConfig( + kp=8.0, ki=10.0, kd=0.5, + deadband_deg=0.0, + rate_limit_dps=200.0, + deadband_pct=0.0, + min_useful_pwm_pct=0.0, + integral_clamp=20.0, + )) + pid.reset(measured_deg=35.0) + _ = _run(pid, sim, setpoint_deg=80.0, seconds=2.0) + assert abs(pid.state.integral) <= pid.config.integral_clamp + 1e-6 + + +def test_allowed_false_zeros_output_and_bleeds_integrator() -> None: + pid = _aggressive_pid() + pid.reset(measured_deg=0.0) + pid.state.integral = 5.0 + out = pid.step(setpoint_deg=20.0, measured_deg=0.0, allowed=False) + assert out == 0.0 + assert abs(pid.state.integral) < 5.0 + + +def test_actuator_deadband_compensation_skips_band() -> None: + """Below deadband_pct=10 the command should be zero; just above, snapped to 15.""" + pid = PidInner(PidInnerConfig( + kp=1.0, ki=0.0, kd=0.0, + deadband_deg=0.0, + rate_limit_dps=10000.0, # disable -- testing single tick + deadband_pct=10.0, + min_useful_pwm_pct=15.0, + )) + pid.reset(measured_deg=0.0) + # error=5 -> raw_output=5 -> below deadband -> 0 + assert pid.step(setpoint_deg=5.0, measured_deg=0.0) == 0.0 + pid.reset(measured_deg=0.0) + # error=11 -> raw_output=11 -> snapped to 15 + out = pid.step(setpoint_deg=11.0, measured_deg=0.0) + assert out == pytest.approx(15.0, abs=1e-6) + pid.reset(measured_deg=0.0) + # error=-11 -> snapped to -15 + out = pid.step(setpoint_deg=-11.0, measured_deg=0.0) + assert out == pytest.approx(-15.0, abs=1e-6) + + +def test_asymmetry_divides_stbd_command() -> None: + """If the pump is intrinsically faster to stbd (asymmetry=1.5), the PID + should divide its starboard command by 1.5 to compensate.""" + pid = PidInner(PidInnerConfig( + kp=1.0, ki=0.0, kd=0.0, + deadband_deg=0.0, + rate_limit_dps=10000.0, # disable -- testing single tick + deadband_pct=0.0, + min_useful_pwm_pct=0.0, + asymmetry_stbd_over_port=1.5, + )) + pid.reset(measured_deg=0.0) + out = pid.step(setpoint_deg=30.0, measured_deg=0.0) + # raw_output = 30 -> divided by 1.5 -> 20 + assert out == pytest.approx(20.0, abs=1e-6) + pid.reset(measured_deg=0.0) + out = pid.step(setpoint_deg=-30.0, measured_deg=0.0) + # raw_output = -30 -> port multiplied -> -45 (within saturation) + assert out == pytest.approx(-45.0, abs=1e-6) + + +def test_output_saturation_at_plus_minus_100() -> None: + pid = PidInner(PidInnerConfig( + kp=50.0, ki=0.0, kd=0.0, + deadband_deg=0.0, + rate_limit_dps=10000.0, + deadband_pct=0.0, + min_useful_pwm_pct=0.0, + )) + pid.reset(measured_deg=0.0) + out = pid.step(setpoint_deg=10.0, measured_deg=0.0) + assert out == 100.0 + pid.reset(measured_deg=0.0) + out = pid.step(setpoint_deg=-10.0, measured_deg=0.0) + assert out == -100.0 + + +def test_rate_limit_caps_setpoint_slew() -> None: + """With rate_limit_dps=5 and dt=0.02, the working setpoint should advance + by no more than 0.1 deg per tick toward a step of 50 deg.""" + pid = PidInner(PidInnerConfig( + kp=1.0, ki=0.0, kd=0.0, + deadband_deg=0.0, + rate_limit_dps=5.0, + deadband_pct=0.0, + min_useful_pwm_pct=0.0, + freq_hz=50.0, + )) + pid.reset(measured_deg=0.0, setpoint_deg=0.0) + # First tick: working setpoint can grow by 5 * 0.02 = 0.1 deg. + out1 = pid.step(setpoint_deg=50.0, measured_deg=0.0) + assert out1 == pytest.approx(0.1, abs=1e-6) + out2 = pid.step(setpoint_deg=50.0, measured_deg=0.0) + assert out2 == pytest.approx(0.2, abs=1e-6) + + +def test_disturbance_rejection() -> None: + """Apply a constant external torque; the controller should still settle + near the setpoint, with the integrator absorbing the bias. + + External torque +1.0 with actuator_gain=0.2 means the controller needs + a steady-state command of ~-5 % to keep position. The integrator gets + there by integrating the residual error -- with ki=2.0 it converges + within ~25 s. + """ + sim = RudderSimulator(RudderDynamicsConfig(external_torque=1.0)) + sim.reset() + # Use higher integral gain than the default _aggressive_pid() for faster + # disturbance rejection in the test window. + pid = PidInner(PidInnerConfig( + kp=10.0, ki=2.0, kd=0.5, + deadband_deg=0.2, + rate_limit_dps=10000.0, + deadband_pct=0.0, + min_useful_pwm_pct=0.0, + integral_clamp=200.0, + )) + pid.reset(measured_deg=0.0, setpoint_deg=0.0) + rec = _run(pid, sim, setpoint_deg=10.0, seconds=25.0) + assert rec.final_angle() == pytest.approx(10.0, abs=1.0), ( + f"final angle {rec.final_angle()}, want ~10" + ) diff --git a/arautopilot/tests/test_rudder_simulator.py b/arautopilot/tests/test_rudder_simulator.py new file mode 100644 index 0000000..ac627ea --- /dev/null +++ b/arautopilot/tests/test_rudder_simulator.py @@ -0,0 +1,117 @@ +"""Tests for ``arautopilot.studio.simulator.rudder_dynamics``.""" + +from __future__ import annotations + +import pytest + +from arautopilot.studio.simulator.rudder_dynamics import ( + RudderDynamicsConfig, + RudderSimulator, + RunRecorder, +) + + +def test_zero_input_stays_at_rest() -> None: + sim = RudderSimulator() + sim.reset(angle_deg=0.0) + for _ in range(1000): + sim.step(dt=0.001, pwm_pct=0.0) + assert sim.state.angle_deg == pytest.approx(0.0, abs=1e-9) + assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-9) + + +def test_full_starboard_command_drives_to_stbd_endstop() -> None: + """With marine-realistic gain (v_max ~5 dps) reaching 35 deg takes ~8 s.""" + sim = RudderSimulator() + sim.reset(angle_deg=0.0) + for _ in range(15000): + sim.step(dt=0.001, pwm_pct=100.0) + assert sim.state.angle_deg == pytest.approx(sim.config.max_angle_deg, abs=1e-6) + assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-6) + + +def test_full_port_command_drives_to_port_endstop() -> None: + sim = RudderSimulator() + sim.reset(angle_deg=0.0) + for _ in range(15000): + sim.step(dt=0.001, pwm_pct=-100.0) + assert sim.state.angle_deg == pytest.approx(-sim.config.max_angle_deg, abs=1e-6) + assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-6) + + +def test_command_inside_deadband_produces_no_motion() -> None: + sim = RudderSimulator(RudderDynamicsConfig(deadband_pct=10.0, + min_useful_pwm_pct=15.0)) + sim.reset(angle_deg=0.0) + for _ in range(2000): + sim.step(dt=0.001, pwm_pct=5.0) # under deadband + assert sim.state.angle_deg == pytest.approx(0.0, abs=1e-9) + + +def test_command_just_above_deadband_snaps_to_min_useful() -> None: + """A command of 11% with deadband=10/min_useful=15 should act like 15%.""" + cfg = RudderDynamicsConfig(deadband_pct=10.0, min_useful_pwm_pct=15.0) + sim_a = RudderSimulator(cfg) + sim_b = RudderSimulator(cfg) + sim_a.reset() + sim_b.reset() + for _ in range(3000): + sim_a.step(dt=0.001, pwm_pct=11.0) + sim_b.step(dt=0.001, pwm_pct=15.0) + assert sim_a.state.angle_deg == pytest.approx(sim_b.state.angle_deg, abs=1e-9) + + +def test_asymmetry_makes_starboard_faster() -> None: + cfg = RudderDynamicsConfig(asymmetry_stbd_over_port=1.5, + deadband_pct=0.0, min_useful_pwm_pct=0.0) + sim_s = RudderSimulator(cfg) + sim_p = RudderSimulator(cfg) + sim_s.reset() + sim_p.reset() + for _ in range(2000): + sim_s.step(dt=0.001, pwm_pct=+50.0) + sim_p.step(dt=0.001, pwm_pct=-50.0) + # Starboard angle should be larger in magnitude than port at the same + # effort, because the pump pushes harder to starboard. + assert abs(sim_s.state.angle_deg) > abs(sim_p.state.angle_deg) + + +def test_recorder_captures_trajectory() -> None: + """Sanity check the recorder: capture a few seconds of constant-command + motion and verify the samples reflect the rudder moving in the commanded + direction with strictly monotonic position (no oscillation under constant + drive).""" + sim = RudderSimulator() + sim.reset() + rec = RunRecorder() + for k in range(2000): + t = k * 0.001 + sim.step(dt=0.001, pwm_pct=80.0) + rec.record(t=t, setpoint_deg=25.0, sim=sim, pwm_pct=80.0) + assert len(rec.samples) == 2000 + # Monotonically increasing angle (constant +ve command + viscous damping + # means no overshoot during the run). + assert rec.samples[-1].angle_deg > rec.samples[0].angle_deg + assert all(rec.samples[i + 1].angle_deg >= rec.samples[i].angle_deg + for i in range(len(rec.samples) - 1)) + + +def test_invalid_dt_rejected() -> None: + sim = RudderSimulator() + with pytest.raises(ValueError): + sim.step(dt=0.0, pwm_pct=50.0) + with pytest.raises(ValueError): + sim.step(dt=-0.001, pwm_pct=50.0) + + +def test_endstop_zeros_outward_velocity_only() -> None: + """Hitting the stbd end-stop should kill +velocity but allow reverse.""" + sim = RudderSimulator() + sim.reset(angle_deg=sim.config.max_angle_deg, angular_vel_dps=20.0) + sim.step(dt=0.001, pwm_pct=0.0) + assert sim.state.angle_deg == pytest.approx(sim.config.max_angle_deg, abs=1e-9) + assert sim.state.angular_vel_dps == 0.0 + # Now reverse: should move away from the stop. + for _ in range(200): + sim.step(dt=0.001, pwm_pct=-80.0) + assert sim.state.angle_deg < sim.config.max_angle_deg diff --git a/docs/sprint-2-plan.md b/docs/sprint-2-plan.md new file mode 100644 index 0000000..d1f220b --- /dev/null +++ b/docs/sprint-2-plan.md @@ -0,0 +1,74 @@ +# Sprint 2 — PID inner loop + Python rudder simulator + +> Brief reference: §12 Sprint 2. +> Status: executed overnight under user's blanket authorisation. + +## Objetivo + +Lazo cerrado de **posición de timón**: dado un setpoint en grados, comandar la +bomba/motor para llegar y mantener esa posición. Frecuencia 50 Hz (hard +real-time, brief §6). + +Sin esto el Sprint 3 (heading control) no tiene sobre qué construir. + +## Estrategia + +Dado que no tengo hardware AR-NMEA-IO ni compilador C++ host, sigo este +camino: + +1. **Simulador de timón en Python** (`arautopilot.studio.simulator.rudder_dynamics`): + modelo físico mínimo de un timón con inercia rotacional, fricción viscosa, + deadband hidráulico y asimetría babor/estribor. Frecuencia de integración + libre (1 ms). +2. **PID inner loop en Python** (`arautopilot.studio.simulator.pid_inner`): + implementación pura-Python del algoritmo, anti-windup tipo + back-calculation, deadband del setpoint, rate limit, saturación, y + compensación de no-linealidades del actuador (offset por deadband, + ganancia asimétrica). Usado para iterar rápido contra el simulador y + producir las curvas de respuesta esperadas. +3. **PID inner loop en C++** (`firmware/.../src/pid/pid_inner.{h,cpp}`): + port línea-por-línea del algoritmo Python. Compila tanto en ESP32 como + en host-native (sin dependencias de Arduino). Mismas constantes, + mismos coeficientes, misma topología. +4. **Cross-validation tests**: pytest carga el módulo Python directamente + y un wrapper ctypes del PID C++ compilado, los corre contra el mismo + simulador y verifica diferencia < 1e-3 en la trayectoria. +5. **Tests Unity host-side** del PID C++ (corren cuando hay g++/clang). +6. **Wire al firmware**: el `pid_inner_task` ya estaba creado como stub + en task_config.h; ahora hace algo útil. Lee setpoint de Modbus + (HOLDING_HEADING_SETPOINT_X100 reasignado pro-tem para + "rudder setpoint", se separa correctamente en Sprint 3), lee posición + de `hal::rudder_sensor_latest()`, comanda + `hal::rudder_command(pwm_pct)`. +7. **Modbus expone**: ganancias activas (read), setpoint actual (RW), + error instantáneo (read), salida PID en % (read). + +## Salvaguardas implementadas (todas obligatorias por brief §6) + +- **Anti-windup**: integrador clamped a `[-anti_windup_limit, +anti_windup_limit]`, + además back-calculation cuando la salida satura +- **Deadband del setpoint**: ±0.5° por defecto (configurable) — evita + oscilación por ruido +- **Saturación de salida**: comando en %, limitado a [-100, +100] +- **Rate limit del setpoint interno**: 3-6 °/s típico (configurable) +- **Compensación de deadband del actuador**: si `|cmd| > 0`, snap to + `min_useful_pwm_pct` +- **Asimetría babor/estribor**: ganancia `asymmetry_stbd_over_port` aplicada + al lado correspondiente +- **Refuse-to-act** en STANDBY (ya estaba desde Sprint 1) + +## Validación + +- `pytest` verde con tests Python contra el simulador (step response, + disturbance rejection, deadband behaviour, saturation, anti-windup, + asymmetry). +- `pio run -e esp32-dev` compila clean. +- Cross-validation Python ↔ C++: trayectorias coinciden < 1e-3. + +## Lo que NO hace Sprint 2 + +- No es lazo de rumbo. Eso es Sprint 3 (PID outer). +- No tiene gain scheduling por velocidad. Sprint 3. +- No tiene ROT feed-forward. Sprint 3. +- No usa modos distintos a STANDBY (sigue rechazando otros). Modes + reales en Sprint 3 cuando el outer loop entra. diff --git a/firmware/ar_autopilot_v1/modbus_registers.yaml b/firmware/ar_autopilot_v1/modbus_registers.yaml index 5d58845..5cd7a91 100644 --- a/firmware/ar_autopilot_v1/modbus_registers.yaml +++ b/firmware/ar_autopilot_v1/modbus_registers.yaml @@ -95,6 +95,14 @@ inputs: - { addr: 32, name: BATTERY_VOLTAGE_X100, desc: "System battery voltage, V*100", unit: "V", scale: 0.01 } - { addr: 33, name: ACTUATOR_CURRENT_X100, desc: "Actuator current, A*100", unit: "A", scale: 0.01 } + # ----- PID inner loop telemetry (Sprint 2) ----- + - { addr: 40, name: PID_INNER_SETPOINT_X100, desc: "Inner-loop rudder setpoint, deg*100 (signed int16)", unit: "deg", scale: 0.01 } + - { addr: 41, name: PID_INNER_OUTPUT_X100, desc: "Last PID command, %*100 (signed int16, -10000..+10000)", unit: "%", scale: 0.01 } + - { addr: 42, name: PID_INNER_ERROR_X100, desc: "Last PID error, deg*100 (signed int16)", unit: "deg", scale: 0.01 } + - { addr: 43, name: PID_INNER_KP_X1000, desc: "Inner-loop kp * 1000 (unsigned)", scale: 0.001 } + - { 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 } + # ----------------------------------------------------------------------------- # Holding registers (read-write 16-bit words) -- setpoints and config # ----------------------------------------------------------------------------- @@ -104,3 +112,9 @@ holdings: - { addr: 2, name: BRIGHTNESS_PCT, desc: "Display brightness 0..100", unit: "%" } - { addr: 3, name: ALARM_VOLUME_PCT, desc: "Alarm volume 0..100", unit: "%" } - { addr: 8, name: DODGE_OFFSET_DEG_X100, desc: "Dodge mode heading offset, deg*100 (signed int16)", unit: "deg", scale: 0.01 } + + # ----- PID inner loop tunable holdings (Sprint 2) ----- + - { addr: 16, name: PID_INNER_SETPOINT_REQ_X100, desc: "Requested inner-loop rudder setpoint, deg*100 (signed int16)", unit: "deg", scale: 0.01 } + - { 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 } diff --git a/firmware/ar_autopilot_v1/src/main.cpp b/firmware/ar_autopilot_v1/src/main.cpp index 28260ef..e8bbeda 100644 --- a/firmware/ar_autopilot_v1/src/main.cpp +++ b/firmware/ar_autopilot_v1/src/main.cpp @@ -26,6 +26,7 @@ #include "hal/rudder_actuator.h" #include "hal/rudder_sensor.h" #include "modes/standby.h" +#include "pid/pid_inner_task.h" #include "protocols/modbus_slave.h" #include "protocols/nmea2000_consumer.h" #include "safety/safety_monitor.h" @@ -76,6 +77,12 @@ void setup() { arautopilot::safety::safety_monitor_start_task(); arautopilot::hal::rudder_sensor_start_task(); + + // PID inner loop (Sprint 2). Active only when not in STANDBY and + // rudder sensor reading is valid; refuses internally otherwise. + arautopilot::pid::pid_inner_task_init(); + arautopilot::pid::pid_inner_task_start(); + ar_start_heartbeat_task(); // Modbus slave (server) -- exposes telemetry + commands to the display. diff --git a/firmware/ar_autopilot_v1/src/pid/pid_inner.h b/firmware/ar_autopilot_v1/src/pid/pid_inner.h new file mode 100644 index 0000000..6249906 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/pid/pid_inner.h @@ -0,0 +1,197 @@ +// ============================================================================= +// pid_inner.h -- inner rudder-position PID (header-only, host-testable) +// ============================================================================= +// +// Line-by-line port of arautopilot/studio/simulator/pid_inner.py. Same +// algorithm, same variables, same numerics. The Python module is the +// reference; this header must stay byte-equivalent in behaviour. Drift is +// caught by the cross-validation test (Python module loaded directly + +// C++ compiled via ctypes -> same trajectory within float tolerance). +// +// No Arduino dependencies -- this file compiles on the ESP32 toolchain AND +// on host g++/clang/MSVC for native Unity tests. +// ============================================================================= + +#pragma once + +#include +#include + +namespace arautopilot::pid { + +struct PidInnerConfig { + // Gains + float kp{2.5f}; + float ki{0.15f}; + float kd{0.30f}; + + // Sampling + float freq_hz{50.0f}; + + // Setpoint handling + float deadband_deg{0.5f}; + float rate_limit_dps{30.0f}; + + // Output saturation + float output_min_pct{-100.0f}; + float output_max_pct{+100.0f}; + + // Anti-windup + float integral_clamp{30.0f}; + // aw_gain: if < 0 -> use 1/kp when kp != 0, else 0. Default sentinel -1. + float aw_gain{-1.0f}; + + // Derivative low-pass + float d_lpf_tau_s{0.05f}; + + // Actuator non-linearity compensation + float deadband_pct{7.0f}; + float min_useful_pwm_pct{12.0f}; + float asymmetry_stbd_over_port{1.0f}; + + float dt() const { return 1.0f / freq_hz; } +}; + +struct PidInnerState { + float integral{0.0f}; + float prev_error{0.0f}; + float prev_d_term{0.0f}; + float prev_setpoint_deg{0.0f}; + float last_output_pct{0.0f}; +}; + +namespace detail { + +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); +} + +} // namespace detail + +class PidInner { + public: + PidInner() = default; + explicit PidInner(const PidInnerConfig& config) : config_(config) {} + + void reset(float measured_deg = 0.0f, float setpoint_deg = 0.0f) { + state_ = PidInnerState{}; + state_.prev_setpoint_deg = setpoint_deg; + (void)measured_deg; // kept in signature for parity with Python + } + + void update_config(const PidInnerConfig& config) { + config_ = config; + } + + const PidInnerConfig& config() const { return config_; } + const PidInnerState& state() const { return state_; } + PidInnerState& mutable_state() { return state_; } + + /// One controller tick. Returns the signed PWM command in percent. + /// When ``allowed`` is false the output is forced to zero and the + /// integrator bleeds toward zero (anti-windup during manual operation). + float step(float setpoint_deg, float measured_deg, bool allowed = true) { + const PidInnerConfig& cfg = config_; + PidInnerState& st = state_; + const float dt = cfg.dt(); + + // Rate-limit the setpoint. + const float target = rate_limit_setpoint(setpoint_deg); + st.prev_setpoint_deg = target; + + // Error with deadband. + const float raw_error = target - measured_deg; + float error; + if (raw_error >= -cfg.deadband_deg && raw_error <= cfg.deadband_deg) { + error = 0.0f; + } else { + const float sign = (raw_error > 0.0f) ? 1.0f : -1.0f; + error = raw_error - sign * cfg.deadband_deg; + } + + if (!allowed) { + st.integral *= 0.95f; + st.prev_error = error; + st.last_output_pct = 0.0f; + return 0.0f; + } + + // Proportional. + const float p_term = cfg.kp * error; + + // Integral (provisional). + st.integral += cfg.ki * error * dt; + st.integral = detail::clamp_f(st.integral, -cfg.integral_clamp, + cfg.integral_clamp); + + // Derivative with low-pass. + const float d_raw = (dt > 0.0f) + ? cfg.kd * (error - st.prev_error) / dt + : 0.0f; + const float alpha = detail::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 raw_output = p_term + st.integral + d_term; + + // Saturate + back-calculation anti-windup. + const float output = detail::clamp_f(raw_output, cfg.output_min_pct, + cfg.output_max_pct); + if (raw_output != output) { + float aw; + if (cfg.aw_gain >= 0.0f) { + aw = cfg.aw_gain; + } else if (cfg.kp != 0.0f) { + aw = 1.0f / cfg.kp; + } else { + aw = 0.0f; + } + st.integral -= aw * (raw_output - output) * dt; + st.integral = detail::clamp_f(st.integral, -cfg.integral_clamp, + cfg.integral_clamp); + } + + const float cmd = compensate(output); + + st.prev_error = error; + st.last_output_pct = cmd; + return cmd; + } + + private: + float rate_limit_setpoint(float requested_deg) const { + const float max_delta = config_.rate_limit_dps * config_.dt(); + const float delta = requested_deg - state_.prev_setpoint_deg; + if (delta > max_delta) return state_.prev_setpoint_deg + max_delta; + if (delta < -max_delta) return state_.prev_setpoint_deg - max_delta; + return requested_deg; + } + + float compensate(float raw_pct) const { + const PidInnerConfig& cfg = config_; + if (raw_pct == 0.0f) return 0.0f; + float magnitude = (raw_pct < 0.0f) ? -raw_pct : raw_pct; + if (magnitude <= cfg.deadband_pct) return 0.0f; + if (magnitude < cfg.min_useful_pwm_pct) magnitude = cfg.min_useful_pwm_pct; + const float sign = (raw_pct > 0.0f) ? 1.0f : -1.0f; + float cmd = sign * magnitude; + if (cmd > 0.0f && cfg.asymmetry_stbd_over_port != 0.0f) { + cmd /= cfg.asymmetry_stbd_over_port; + } else if (cmd < 0.0f) { + cmd *= cfg.asymmetry_stbd_over_port; + } + return detail::clamp_f(cmd, cfg.output_min_pct, cfg.output_max_pct); + } + + PidInnerConfig config_{}; + PidInnerState state_{}; +}; + +} // namespace arautopilot::pid diff --git a/firmware/ar_autopilot_v1/src/pid/pid_inner_task.cpp b/firmware/ar_autopilot_v1/src/pid/pid_inner_task.cpp new file mode 100644 index 0000000..abf89e6 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/pid/pid_inner_task.cpp @@ -0,0 +1,136 @@ +// ============================================================================= +// pid_inner_task.cpp -- 50 Hz inner-loop control task (Sprint 2) +// ============================================================================= + +#include "pid_inner_task.h" + +#include + +#include "../hal/rudder_actuator.h" +#include "../hal/rudder_sensor.h" +#include "../modes/standby.h" +#include "../safety/watchdog.h" +#include "../system/ar_log.h" +#include "../system/task_config.h" + +namespace arautopilot::pid { + +namespace { +constexpr const char* TAG = "AR/PID"; + +portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED; +PidInner g_pid{}; +float g_setpoint_deg = 0.0f; +float g_last_output_pct = 0.0f; +float g_last_error_deg = 0.0f; + +void InnerLoopTask(void* /*pv*/) { + AR_LOGI(TAG, "pid_inner task started on core %d (50 Hz)", xPortGetCoreID()); + safety::watchdog_subscribe_current_task(); + + TickType_t last_wake = xTaskGetTickCount(); + for (;;) { + // Snapshot what the operator/outer loop wants. + float setpoint; + portENTER_CRITICAL(&g_mux); + setpoint = g_setpoint_deg; + portEXIT_CRITICAL(&g_mux); + + // Read the current rudder position. + const auto rd = hal::rudder_sensor_latest(); + + // Only run the controller if we have a valid reading and we are not + // in STANDBY. The PID's `allowed` parameter cleanly bleeds the + // integrator while disengaged. + const bool allowed = rd.valid && !modes::is_standby(); + const float cmd = g_pid.step(setpoint, rd.angle_deg, allowed); + + // Send to the actuator. rudder_command() carries its own safety + // interlocks (power, mode, limit switches) and will refuse if + // anything is off, so we don't replicate them here. + const int8_t pwm_pct = + (cmd > 127.0f) ? 127 : (cmd < -127.0f) ? -127 : (int8_t)cmd; + hal::rudder_command(pwm_pct); + + // Publish for Modbus. + portENTER_CRITICAL(&g_mux); + g_last_output_pct = cmd; + g_last_error_deg = setpoint - rd.angle_deg; + portEXIT_CRITICAL(&g_mux); + + safety::watchdog_feed(); + vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_PID_INNER)); + } +} + +} // namespace + +void pid_inner_task_init() { + PidInnerConfig cfg; + // Inherit seed defaults from PidInnerConfig (which match the 30 m + // yacht_motor_planeo profile). Real production gains arrive via the + // .appack at deployment time (Sprint 4) and via Modbus hot-swap at + // commissioning (Sprint 7). + g_pid.update_config(cfg); + g_pid.reset(0.0f, 0.0f); + AR_LOGI(TAG, + "pid_inner_init: kp=%.3f ki=%.3f kd=%.3f freq=%.1f Hz " + "deadband=%.2f deg rate_lim=%.1f dps", + cfg.kp, cfg.ki, cfg.kd, cfg.freq_hz, cfg.deadband_deg, + cfg.rate_limit_dps); +} + +void pid_inner_task_start() { + xTaskCreatePinnedToCore(InnerLoopTask, "pid_inner", AR_TASK_STACK_PID_INNER, + nullptr, AR_TASK_PRIO_PID_INNER, nullptr, + AR_TASK_CORE_REALTIME); +} + +void pid_inner_set_setpoint_deg(float setpoint_deg) { + portENTER_CRITICAL(&g_mux); + g_setpoint_deg = setpoint_deg; + portEXIT_CRITICAL(&g_mux); +} + +float pid_inner_setpoint_deg() { + portENTER_CRITICAL(&g_mux); + float v = g_setpoint_deg; + portEXIT_CRITICAL(&g_mux); + return v; +} + +float pid_inner_last_output_pct() { + portENTER_CRITICAL(&g_mux); + float v = g_last_output_pct; + portEXIT_CRITICAL(&g_mux); + return v; +} + +float pid_inner_last_error_deg() { + portENTER_CRITICAL(&g_mux); + float v = g_last_error_deg; + portEXIT_CRITICAL(&g_mux); + return v; +} + +void pid_inner_update_gains(float kp, float ki, float kd) { + PidInnerConfig cfg = g_pid.config(); + cfg.kp = kp; + cfg.ki = ki; + cfg.kd = kd; + portENTER_CRITICAL(&g_mux); + g_pid.update_config(cfg); + portEXIT_CRITICAL(&g_mux); + AR_LOGI(TAG, "pid_inner gains updated: kp=%.3f ki=%.3f kd=%.3f", kp, ki, kd); +} + +void pid_inner_get_gains(float& kp, float& ki, float& kd) { + portENTER_CRITICAL(&g_mux); + const auto& cfg = g_pid.config(); + kp = cfg.kp; + ki = cfg.ki; + kd = cfg.kd; + portEXIT_CRITICAL(&g_mux); +} + +} // namespace arautopilot::pid diff --git a/firmware/ar_autopilot_v1/src/pid/pid_inner_task.h b/firmware/ar_autopilot_v1/src/pid/pid_inner_task.h new file mode 100644 index 0000000..477d196 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/pid/pid_inner_task.h @@ -0,0 +1,45 @@ +// ============================================================================= +// pid_inner_task.h -- 50 Hz inner-loop control task (Sprint 2) +// ============================================================================= +// +// Wraps the header-only PidInner controller in a FreeRTOS task pinned to +// Core 1 (real-time core). Reads the rudder position from hal::rudder_sensor, +// consumes the setpoint that the outer loop / Modbus client wrote, and +// commands hal::rudder_actuator. Refuses to act in STANDBY or with master +// power off (those interlocks live in hal::rudder_command itself). +// ============================================================================= + +#pragma once + +#include "pid_inner.h" + +namespace arautopilot::pid { + +/// Initialise the controller with the seed gains. Must be called from +/// setup() once. +void pid_inner_task_init(); + +/// Spawn the FreeRTOS task. Must be called after pid_inner_task_init(). +void pid_inner_task_start(); + +/// Update the setpoint that the inner loop pursues. Called by the Modbus +/// slave (when an operator writes a holding register) and, later, by the +/// outer loop task. Units: degrees, signed. +void pid_inner_set_setpoint_deg(float setpoint_deg); + +/// Read the current setpoint (thread-safe). +float pid_inner_setpoint_deg(); + +/// Read the latest PID output command (signed PWM percent). +float pid_inner_last_output_pct(); + +/// Read the latest error (deg) the controller saw. +float pid_inner_last_error_deg(); + +/// Hot-swap gains at runtime (thread-safe). +void pid_inner_update_gains(float kp, float ki, float kd); + +/// Read the gains currently in use. +void pid_inner_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 cc34fc2..f06e12b 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 = 17; -constexpr uint16_t INPUT_MAX_ADDR = 33; +constexpr uint16_t INPUT_COUNT = 23; +constexpr uint16_t INPUT_MAX_ADDR = 45; // Firmware major version constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0; @@ -125,10 +125,28 @@ constexpr uint16_t INPUT_BATTERY_VOLTAGE_X100 = 32; // Actuator current, A*100 // unit=A, scale=0.01 constexpr uint16_t INPUT_ACTUATOR_CURRENT_X100 = 33; +// Inner-loop rudder setpoint, deg*100 (signed int16) +// unit=deg, scale=0.01 +constexpr uint16_t INPUT_PID_INNER_SETPOINT_X100 = 40; +// Last PID command, %*100 (signed int16, -10000..+10000) +// unit=%, scale=0.01 +constexpr uint16_t INPUT_PID_INNER_OUTPUT_X100 = 41; +// Last PID error, deg*100 (signed int16) +// unit=deg, scale=0.01 +constexpr uint16_t INPUT_PID_INNER_ERROR_X100 = 42; +// Inner-loop kp * 1000 (unsigned) +// scale=0.001 +constexpr uint16_t INPUT_PID_INNER_KP_X1000 = 43; +// Inner-loop ki * 1000 (unsigned) +// scale=0.001 +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; // ----- Holding registers (read-write words) ----- -constexpr uint16_t HOLDING_COUNT = 5; -constexpr uint16_t HOLDING_MAX_ADDR = 8; +constexpr uint16_t HOLDING_COUNT = 9; +constexpr uint16_t HOLDING_MAX_ADDR = 19; // Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE) constexpr uint16_t HOLDING_MODE_REQUEST = 0; @@ -144,5 +162,17 @@ constexpr uint16_t HOLDING_ALARM_VOLUME_PCT = 3; // Dodge mode heading offset, deg*100 (signed int16) // unit=deg, scale=0.01 constexpr uint16_t HOLDING_DODGE_OFFSET_DEG_X100 = 8; +// Requested inner-loop rudder setpoint, deg*100 (signed int16) +// unit=deg, scale=0.01 +constexpr uint16_t HOLDING_PID_INNER_SETPOINT_REQ_X100 = 16; +// Requested inner-loop kp * 1000 (unsigned) +// scale=0.001 +constexpr uint16_t HOLDING_PID_INNER_KP_REQ_X1000 = 17; +// Requested inner-loop ki * 1000 (unsigned) +// scale=0.001 +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; } // 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 971591b..be01e9b 100644 --- a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp +++ b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp @@ -27,6 +27,7 @@ #include "../hal/pinout.h" #include "../hal/rudder_sensor.h" #include "../modes/standby.h" +#include "../pid/pid_inner_task.h" #include "../system/ar_log.h" #include "../system/task_config.h" #include "modbus_registers.h" @@ -55,6 +56,10 @@ struct HoldingStorage { uint16_t brightness_pct = 80; uint16_t alarm_volume_pct = 60; int16_t dodge_offset_deg_x100 = 0; + int16_t pid_inner_setpoint_req_x100 = 0; + uint16_t pid_inner_kp_req_x1000 = 0; + uint16_t pid_inner_ki_req_x1000 = 0; + uint16_t pid_inner_kd_req_x1000 = 0; }; HoldingStorage g_holding; @@ -124,6 +129,40 @@ uint16_t read_input_register(uint16_t addr) { case INPUT_ACTUATOR_CURRENT_X100: return 0; + // ----- PID inner-loop telemetry (Sprint 2) ----- + case INPUT_PID_INNER_SETPOINT_X100: { + int v = (int)(pid::pid_inner_setpoint_deg() * 100.0f); + if (v < -32768) v = -32768; + if (v > 32767) v = 32767; + return (uint16_t)(int16_t)v; + } + case INPUT_PID_INNER_OUTPUT_X100: { + int v = (int)(pid::pid_inner_last_output_pct() * 100.0f); + if (v < -32768) v = -32768; + if (v > 32767) v = 32767; + return (uint16_t)(int16_t)v; + } + case INPUT_PID_INNER_ERROR_X100: { + int v = (int)(pid::pid_inner_last_error_deg() * 100.0f); + if (v < -32768) v = -32768; + if (v > 32767) v = 32767; + return (uint16_t)(int16_t)v; + } + case INPUT_PID_INNER_KP_X1000: + case INPUT_PID_INNER_KI_X1000: + case INPUT_PID_INNER_KD_X1000: { + float kp, ki, kd; + pid::pid_inner_get_gains(kp, ki, kd); + float v; + if (addr == INPUT_PID_INNER_KP_X1000) v = kp; + else if (addr == INPUT_PID_INNER_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; } @@ -164,6 +203,10 @@ uint16_t read_holding(uint16_t addr) { case HOLDING_BRIGHTNESS_PCT: return g_holding.brightness_pct; case HOLDING_ALARM_VOLUME_PCT: return g_holding.alarm_volume_pct; case HOLDING_DODGE_OFFSET_DEG_X100: return (uint16_t)g_holding.dodge_offset_deg_x100; + case HOLDING_PID_INNER_SETPOINT_REQ_X100: return (uint16_t)g_holding.pid_inner_setpoint_req_x100; + 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; default: return 0; } } @@ -190,6 +233,39 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) { case HOLDING_DODGE_OFFSET_DEG_X100: g_holding.dodge_offset_deg_x100 = (int16_t)value; return Modbus::Error::SUCCESS; + + // ----- PID inner-loop tunables (Sprint 2) ----- + case HOLDING_PID_INNER_SETPOINT_REQ_X100: { + int16_t sv = (int16_t)value; + g_holding.pid_inner_setpoint_req_x100 = sv; + pid::pid_inner_set_setpoint_deg((float)sv * 0.01f); + 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: { + // Update the requested-gain shadow, then push all three to the + // live controller. We do all three together so partial writes + // don't leave the gains inconsistent. + if (addr == HOLDING_PID_INNER_KP_REQ_X1000) { + g_holding.pid_inner_kp_req_x1000 = value; + } else if (addr == HOLDING_PID_INNER_KI_REQ_X1000) { + g_holding.pid_inner_ki_req_x1000 = value; + } else { + g_holding.pid_inner_kd_req_x1000 = value; + } + float kp = (float)g_holding.pid_inner_kp_req_x1000 * 0.001f; + float ki = (float)g_holding.pid_inner_ki_req_x1000 * 0.001f; + float kd = (float)g_holding.pid_inner_kd_req_x1000 * 0.001f; + // Refuse zero kp -- the rest of the algorithm assumes kp > 0 + // for back-calculation anti-windup. If the operator writes 0 + // we ignore it (leave whatever the firmware booted with). + if (kp <= 0.0f) { + return Modbus::Error::ILLEGAL_DATA_VALUE; + } + pid::pid_inner_update_gains(kp, ki, kd); + return Modbus::Error::SUCCESS; + } default: return Modbus::Error::ILLEGAL_DATA_ADDRESS; }