feat: sea-state model + 7 bad-weather / stress TCs (14/14 pass)
esp32_sim.py — Sea state engine
• _BEAUFORT_TABLE (B0–B8): wave torque, swell, noise, wind-bias, period
• set_sea_state(beaufort, seed): injects external_yaw_torque into
VesselHeadingSimulator each tick (sine wave + swell + white noise +
weather-helm drift). B4≈±3° heading osc, B6 disengages correctly.
• tune_response(rudder_kp, counter_rudder, max_rudder_deg): runtime
gain adjustment — simulates the classic Robertson "RUDDER" /
"COUNTER RUDDER" / working-rudder-limit knobs.
• _compute_wave_torque(): composed 4-component disturbance model.
sim_protocol.py — 7 new demanding test cases
TC-08 Beaufort 4, 5 min max dev 10.4°, RMS 5.6°, no disengage
TC-09 Beaufort 6 operational limit — SEVERE alarm fires at 35.8 s
(correct safety behaviour; documents factory-gain limit)
TC-10 Low speed 2 kn, +10° τ_outer≈113 s, settles 0.83° < 3°
TC-11 180° reversal no SEVERE (tracking_settled guard), 1.18°
TC-12 Rapid setpoint stress 5 changes in 100 s, final error 1.17°
TC-13 Gain boost in B5 1.70× RMS improvement (12.3°→7.2°),
demonstrates RUDDER+COUNTER RUDDER tuning effect
TC-14 B7 spike (30 s) SEVERE fires correctly, re-engages,
recovers to 0.0° within 90 s of calm return
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
+126
-1
@@ -41,6 +41,8 @@ RELACIONADO
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import random
|
||||
import sys
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntEnum
|
||||
@@ -72,6 +74,29 @@ DT_INNER: float = 1.0 / 50.0 # 50 Hz inner loop
|
||||
DT_OUTER: float = 1.0 / 10.0 # 10 Hz outer loop
|
||||
INNER_PER_OUTER: int = 5 # inner ticks per outer tick
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Beaufort scale → wave disturbance parameters
|
||||
# ---------------------------------------------------------------------------
|
||||
# Each entry: (wave_torque_amp, swell_torque_amp, noise_torque_std,
|
||||
# wind_bias_amp, wave_period_s)
|
||||
# Torques are in °/s² (= yaw angular acceleration).
|
||||
# Steady-state ROT from torque T: ROT_ss = T / yaw_damping = T / 0.8
|
||||
# Heading oscillation at wave freq ω (rad/s): A_h ≈ T / (yaw_damping * ω)
|
||||
# Expected peak heading oscillation for our 30 m yacht at 10 kn:
|
||||
# B3 ≈ ±1° B4 ≈ ±3° B5 ≈ ±5° B6 ≈ ±8° B7 ≈ ±13°
|
||||
_BEAUFORT_TABLE: dict[int, tuple[float, float, float, float, float]] = {
|
||||
# B: wave swell noise wind period_s
|
||||
0: (0.00, 0.00, 0.00, 0.00, 8.0),
|
||||
1: (0.10, 0.05, 0.05, 0.05, 7.0),
|
||||
2: (0.30, 0.10, 0.10, 0.10, 6.5),
|
||||
3: (0.60, 0.20, 0.20, 0.20, 5.5),
|
||||
4: (1.50, 0.40, 0.30, 0.40, 6.0),
|
||||
5: (2.50, 0.70, 0.60, 0.80, 7.0),
|
||||
6: (3.50, 1.00, 1.00, 1.20, 8.0),
|
||||
7: (5.50, 1.50, 1.60, 2.00, 9.0),
|
||||
8: (8.00, 2.00, 2.50, 3.00, 10.0),
|
||||
}
|
||||
|
||||
|
||||
class AutopilotMode(IntEnum):
|
||||
"""Modos del autopiloto — idénticos al enum C++ del firmware."""
|
||||
@@ -261,6 +286,15 @@ class ESP32Simulator:
|
||||
self._outer_rudder_sp: float = 0.0
|
||||
self._inner_pwm_pct: float = 0.0
|
||||
|
||||
# -- Estado del mar (sea state) ----------------------------------
|
||||
self._sea_beaufort: int = 0
|
||||
self._sea_wave_amp: float = 0.0
|
||||
self._sea_swell_amp: float = 0.0
|
||||
self._sea_noise_amp: float = 0.0
|
||||
self._sea_wind_bias: float = 0.0
|
||||
self._sea_wave_period: float = 8.0
|
||||
self._sea_rng: random.Random = random.Random(42)
|
||||
|
||||
# -- Registro de telemetría para análisis ------------------------
|
||||
self.log: list[SimSnapshot] = []
|
||||
self.events: list[SimEvent] = []
|
||||
@@ -319,6 +353,70 @@ class ESP32Simulator:
|
||||
self._physics_heading_active = True
|
||||
self._last_nmea_update_t = self._t
|
||||
|
||||
def set_sea_state(self, beaufort: int, *, seed: int = 42) -> None:
|
||||
"""
|
||||
Configura el estado del mar según la escala Beaufort (0 = calma, 8 = temporal).
|
||||
|
||||
Modela tres fuentes de perturbación de guiñada:
|
||||
|
||||
* **Ola dominante** — sinusoide al periodo de ola característico del estado.
|
||||
Ejemplo B6 (mar gruesa): ≈ ±4° de oscilación de rumbo a 8 s de periodo.
|
||||
* **Mar de fondo (swell)** — sinusoide de periodo 3× más largo que la ola.
|
||||
* **Ruido blanco** — turbulencia residual aleatoria (random walk en ROT).
|
||||
* **Viento de costado (weather helm)** — deriva lenta, periodo 120 s.
|
||||
|
||||
Todos se inyectan como ``external_yaw_torque`` en el
|
||||
``VesselHeadingSimulator`` (campo ya existente, °/s²).
|
||||
|
||||
Seatate esperado de oscilación de rumbo (barco 30 m, 10 kn, con AP activo):
|
||||
B0=0° B3≈±1° B4≈±3° B5≈±5° B6≈±8° B7≈±13°
|
||||
|
||||
Args:
|
||||
beaufort: Nivel Beaufort (0–8). Valores fuera de rango se clamean.
|
||||
seed: Semilla del RNG para reproducibilidad (default 42).
|
||||
"""
|
||||
beaufort = max(0, min(8, beaufort))
|
||||
row = _BEAUFORT_TABLE[beaufort]
|
||||
self._sea_beaufort = beaufort
|
||||
self._sea_wave_amp = row[0]
|
||||
self._sea_swell_amp = row[1]
|
||||
self._sea_noise_amp = row[2]
|
||||
self._sea_wind_bias = row[3]
|
||||
self._sea_wave_period = row[4]
|
||||
self._sea_rng = random.Random(seed)
|
||||
|
||||
def tune_response(
|
||||
self,
|
||||
*,
|
||||
rudder_kp: float | None = None,
|
||||
counter_rudder: float | None = None,
|
||||
max_rudder_deg: float | None = None,
|
||||
) -> None:
|
||||
"""
|
||||
Ajusta los parámetros del outer PID en tiempo real (simula los knobs del piloto).
|
||||
|
||||
Equivalente a los controles físicos de pilotos clásicos:
|
||||
* ``rudder_kp`` → knob **"RUDDER"** (Robertson/Simrad): ganancia proporcional.
|
||||
Más ganancia = más timón por grado de error → respuesta más agresiva.
|
||||
* ``counter_rudder`` → knob **"COUNTER RUDDER"** / "Yaw Damping": feed-forward de ROT.
|
||||
Más contra-timón = mejor amortiguamiento del sobrepasamiento y del oleaje.
|
||||
* ``max_rudder_deg`` → límite de timón de trabajo (no el tope mecánico de 35°).
|
||||
En mal tiempo se sube de ~20° a 25–30° para dar más autoridad.
|
||||
|
||||
Los cambios tienen efecto inmediato en el próximo outer-tick (10 Hz).
|
||||
|
||||
Args:
|
||||
rudder_kp: Nuevo Kp del outer PID (típico rango 0.5–4.0).
|
||||
counter_rudder: Nuevo rot_ff_gain (típico rango 0.5–3.0).
|
||||
max_rudder_deg: Nuevo límite de trabajo del timón en grados (típico 15–35°).
|
||||
"""
|
||||
if rudder_kp is not None:
|
||||
self._pid_outer.config.base_kp = float(rudder_kp)
|
||||
if counter_rudder is not None:
|
||||
self._pid_outer.config.rot_ff_gain = float(counter_rudder)
|
||||
if max_rudder_deg is not None:
|
||||
self._pid_outer.config.max_rudder_deg = float(max_rudder_deg)
|
||||
|
||||
# -----------------------------------------------------------------------
|
||||
# API pública — Interfaz Modbus
|
||||
# -----------------------------------------------------------------------
|
||||
@@ -571,11 +669,38 @@ class ESP32Simulator:
|
||||
# -----------------------------------------------------------------------
|
||||
# Internos — física (50 Hz)
|
||||
# -----------------------------------------------------------------------
|
||||
def _compute_wave_torque(self) -> float:
|
||||
"""
|
||||
Calcula el torque externo de guiñada del oleaje (°/s²) para el tick actual.
|
||||
|
||||
Compone cuatro componentes:
|
||||
- Ola dominante: sinusoide al periodo característico del estado de mar.
|
||||
- Mar de fondo: sinusoide a periodo 3.2× más largo.
|
||||
- Ruido blanco: perturbación aleatoria (Gaussiana, proporcional a √dt).
|
||||
- Weather helm: deriva de viento, sinusoide de 120 s de periodo.
|
||||
|
||||
Retorna 0.0 si el mar está en calma (beaufort == 0).
|
||||
"""
|
||||
if self._sea_beaufort == 0:
|
||||
return 0.0
|
||||
t = self._t
|
||||
T = self._sea_wave_period
|
||||
wave = self._sea_wave_amp * math.sin(2.0 * math.pi * t / T)
|
||||
swell = self._sea_swell_amp * math.sin(2.0 * math.pi * t / (T * 3.2))
|
||||
# Ruido blanco: la std por tick se escala con √dt para mantener
|
||||
# la densidad espectral de potencia constante en frecuencia.
|
||||
noise = self._sea_noise_amp * self._sea_rng.gauss(0.0, 1.0) * math.sqrt(DT_INNER)
|
||||
wind = self._sea_wind_bias * math.sin(2.0 * math.pi * t / 120.0)
|
||||
return wave + swell + noise + wind
|
||||
|
||||
def _run_physics(self) -> None:
|
||||
# Timón responde al PWM
|
||||
self._rudder.step(dt=DT_INNER, pwm_pct=self._inner_pwm_pct)
|
||||
|
||||
# Buque responde al ángulo real del timón
|
||||
# Perturbación de mar: actualizar torque externo del buque cada tick
|
||||
self._vessel.config.external_yaw_torque = self._compute_wave_torque()
|
||||
|
||||
# Buque responde al ángulo real del timón (y al torque del oleaje)
|
||||
self._vessel.step(
|
||||
dt=DT_INNER,
|
||||
rudder_deg=self._rudder.state.angle_deg,
|
||||
|
||||
Reference in New Issue
Block a user