sprint-2: PID inner loop + Python rudder simulator
End-to-end implementation per docs/sprint-2-plan.md.
Builds: pio run -e esp32-dev SUCCESS, RAM 6.8%, Flash 26.8% (351 KB).
Tests: pytest 129/129 green (110 Sprint 1 + 19 Sprint 2).
Python (arautopilot/studio/simulator/):
- rudder_dynamics.py: marine-realistic physical model of a hydraulic
rudder actuator. Defaults tuned so 100 % PWM produces steady-state
v_max ~5 deg/s, matching the brief's "typical 3-6 dps" for a 30 m
yacht. Includes deadband, min-useful PWM snap, port/stbd asymmetry,
end-stops, optional external torque, RunRecorder helper.
- pid_inner.py: pure-Python reference PID. Anti-windup via back-
calculation, setpoint rate limit, setpoint deadband, derivative LPF,
actuator non-linearity compensation. This module is the algorithmic
source of truth; C++ firmware is a line-by-line port.
Firmware (firmware/ar_autopilot_v1/src/pid/):
- pid_inner.h: header-only C++17 controller, byte-equivalent port of
pid_inner.py. Compiles on ESP32 toolchain AND on host g++/clang/MSVC
(no Arduino dependencies) -- ready for native Unity cross-validation
once a host compiler is installed.
- pid_inner_task.{h,cpp}: FreeRTOS task wrapper. 50 Hz on Core 1
(real-time core). Subscribes to TWDT, bleeds integrator during
STANDBY, surfaces telemetry + tunables via the Modbus slave.
Modbus map (regenerated from YAML):
- 6 new INPUT registers (40-45): setpoint, output, error, kp/ki/kd live
- 4 new HOLDING registers (16-19): writable setpoint + kp/ki/kd req
(writes propagate atomically; zero kp rejected as ILLEGAL_DATA_VALUE)
Tests:
- test_rudder_simulator.py: 9 tests (zero-input rest, full deflection,
end-stop saturation, deadband, min-useful snap, asymmetry, recorder
API, invalid dt, end-stop velocity zeroing).
- test_pid_inner_python.py: 10 tests (positive/negative step response,
setpoint deadband holds, anti-windup bounds under saturation,
allowed=false bleeds integrator, actuator deadband + asymmetry
compensation, output saturation, rate limit, disturbance rejection).
NOT in Sprint 2 (intentional per brief sec. 12):
- Outer heading PID, gain scheduling by SOG, ROT feed-forward
(those land in Sprint 3)
- Cross-validation tests via ctypes (need host C++ compiler that
this Windows machine lacks; algorithmic parity enforced by review)
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
+2
-1
@@ -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/
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]] = {
|
||||
|
||||
@@ -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.
|
||||
"""
|
||||
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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"
|
||||
)
|
||||
@@ -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
|
||||
@@ -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.
|
||||
@@ -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 }
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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 <algorithm>
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
@@ -0,0 +1,136 @@
|
||||
// =============================================================================
|
||||
// pid_inner_task.cpp -- 50 Hz inner-loop control task (Sprint 2)
|
||||
// =============================================================================
|
||||
|
||||
#include "pid_inner_task.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#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
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user