alro65
295efa2d83
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>
118 lines
4.4 KiB
Python
118 lines
4.4 KiB
Python
"""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
|