alro65
2b574b57f6
tools/esp32_sim.py – Full software ESP32 simulator with cascade PID (outer 10 Hz / inner 50 Hz), vessel yaw physics, Modbus register banks, mode state-machine (STANDBY/HEADING_HOLD/DODGE), alarm engine (HEADING_LOST, OFF_COURSE with _tracking_settled guard). Sim-specific parameter tuning vs. firmware defaults: • outer: kd=0, ki=0, aw_gain=0, deadband=0 → pure P+ROT-FF, τ≈24 s • inner: kp=20, deadband=0, min_useful=0 → τ_cl=1 s, no bang-bang • vessel: rudder_response_gain=0.004 → 30 m yacht dynamics tools/sim_protocol.py – 7 automated test cases (TC-01…TC-07) with heading-trace charts and HTML report. All 7 PASS: TC-02 settle 49.8 s, error 0.488° (crit <60 s, <1°) TC-03 settle 134 s, error 0.985° (crit <180 s, <2°) TC-04 settle 56.5 s, error 0.570° (crit <90 s, <1°) TC-07 dodge 1.73°, return 0.527° (crit <2°, <1°) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
953 lines
32 KiB
Python
953 lines
32 KiB
Python
"""
|
||
PROPÓSITO
|
||
---------
|
||
Protocolo de pruebas de simulación para AR-Autopilot.
|
||
|
||
POR QUÉ EXISTE
|
||
--------------
|
||
Valida el comportamiento del autopiloto en 7 casos de prueba canónicos
|
||
antes de conectar el ESP32 real. Cada TC crea un simulador limpio, ejecuta
|
||
un escenario, evalúa criterios de aceptación y reporta PASS / FAIL con
|
||
métricas cuantitativas.
|
||
|
||
Al ejecutarse produce dos salidas:
|
||
- Resumen en consola (tabla ASCII)
|
||
- Informe HTML completo con gráficas interactivas (Chart.js)
|
||
|
||
CÓMO USARLO
|
||
-----------
|
||
python tools/sim_protocol.py # todos los TCs
|
||
python tools/sim_protocol.py TC-02 TC-04 # solo esos TCs
|
||
python tools/sim_protocol.py --out mi_reporte.html
|
||
|
||
RELACIONADO
|
||
-----------
|
||
tools/esp32_sim.py — simulador ESP32 en Python
|
||
docs/TEST_PROTOCOL.md — especificación del protocolo
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import html
|
||
import json
|
||
import sys
|
||
import textwrap
|
||
from dataclasses import dataclass
|
||
from datetime import datetime
|
||
from pathlib import Path
|
||
from typing import Callable
|
||
|
||
# Añadir raíz del proyecto al path
|
||
sys.path.insert(0, str(Path(__file__).parent.parent))
|
||
|
||
from arautopilot.studio.simulator.vessel_heading import heading_error_deg
|
||
from tools.esp32_sim import AutopilotMode, ESP32Simulator, SimSnapshot
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Estructuras de datos del protocolo
|
||
# ---------------------------------------------------------------------------
|
||
@dataclass
|
||
class TCSpec:
|
||
id: str
|
||
name: str
|
||
description: str
|
||
acceptance: str # Criterios de aceptación en lenguaje natural
|
||
|
||
|
||
@dataclass
|
||
class TCResult:
|
||
spec: TCSpec
|
||
passed: bool
|
||
metrics: dict[str, float | str]
|
||
message: str
|
||
snapshots: list[SimSnapshot]
|
||
events: list
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Casos de prueba
|
||
# ---------------------------------------------------------------------------
|
||
|
||
def tc01_power_on_engage() -> TCResult:
|
||
"""
|
||
TC-01 — Arranque y enganche
|
||
===========================
|
||
Verifica que el autopiloto parte en STANDBY y transita correctamente a
|
||
HEADING_HOLD cuando hay rumbo NMEA válido y se envía CMD_ENGAGE_REQUEST.
|
||
|
||
Criterios de aceptación:
|
||
• Arranca en STANDBY
|
||
• Transita a HEADING_HOLD tras engage
|
||
• Mantiene HEADING_HOLD durante 10 s sin alarmas
|
||
• Error final < 1°
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-01",
|
||
name="Arranque y enganche en HEADING_HOLD",
|
||
description="El piloto arranca en STANDBY y engancha correctamente con rumbo NMEA válido.",
|
||
acceptance="Modo final HEADING_HOLD, error < 1°, sin alarmas en 10 s.",
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
|
||
# Verificar estado inicial STANDBY
|
||
started_in_standby = sim.mode == AutopilotMode.STANDBY
|
||
|
||
# Inyectar rumbo válido y velocidad
|
||
sim.inject_nmea(heading_deg=90.0, sog_kn=10.0)
|
||
sim.step(0.5) # deja que el NMEA se estabilice
|
||
|
||
# Configurar holdings y enganchar
|
||
sim.write_holding(0, 1) # MODE_REQUEST = HEADING_HOLD
|
||
sim.write_holding(1, 9000) # HEADING_SETPOINT_X100 = 90.00°
|
||
sim.write_coil(0, 1) # CMD_ENGAGE_REQUEST
|
||
|
||
sim.step(0.1) # procesar el engage
|
||
|
||
engaged_in_hh = sim.mode == AutopilotMode.HEADING_HOLD
|
||
|
||
# Correr 10 segundos manteniendo el rumbo
|
||
sim.step(10.0)
|
||
|
||
final_error = abs(heading_error_deg(90.0, sim.heading))
|
||
any_alarm = sim.any_alarm
|
||
final_mode = sim.mode
|
||
|
||
passed = (
|
||
started_in_standby
|
||
and engaged_in_hh
|
||
and final_mode == AutopilotMode.HEADING_HOLD
|
||
and final_error < 1.0
|
||
and not any_alarm
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"started_in_standby": "Sí" if started_in_standby else "No",
|
||
"engaged_correctly": "Sí" if engaged_in_hh else "No",
|
||
"final_mode": final_mode.name,
|
||
"final_error_deg": round(final_error, 3),
|
||
"any_alarm": "No" if not any_alarm else "Sí",
|
||
},
|
||
message=(
|
||
f"Inicio en STANDBY: {'OK' if started_in_standby else 'NO'} | "
|
||
f"Enganche: {'OK' if engaged_in_hh else 'NO'} | "
|
||
f"Error final: {final_error:.3f}° | "
|
||
f"Alarmas: {'ninguna' if not any_alarm else 'PRESENTES'}"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
def tc02_small_heading_change() -> TCResult:
|
||
"""
|
||
TC-02 — Cambio de rumbo pequeño (10°)
|
||
======================================
|
||
El autopiloto está enganchado en 90°. Se ordena virar a 100°.
|
||
Mide tiempo de asentamiento, sobrepasamiento y error residual.
|
||
|
||
Criterios de aceptación:
|
||
• Asentamiento (|error| < 1°) en < 60 s
|
||
• Sobrepasamiento < 5°
|
||
• Error final < 1°
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-02",
|
||
name="Cambio de rumbo pequeño +10°",
|
||
description="Maniobra de 90° → 100°. Evalúa tiempo de asentamiento y sobrepasamiento.",
|
||
acceptance="Asentamiento < 60 s, sobrepasamiento < 5°, error final < 1°.",
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
TARGET = 100.0
|
||
INITIAL = 90.0
|
||
|
||
sim.inject_nmea(heading_deg=INITIAL, sog_kn=10.0)
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, int(INITIAL * 100))
|
||
sim.write_coil(0, 1)
|
||
sim.step(5.0) # estabilizar en 90°
|
||
|
||
# Nuevo setpoint: 100°
|
||
sim.write_holding(1, int(TARGET * 100))
|
||
t_step = sim.t
|
||
sim.step(120.0)
|
||
|
||
snapshots_after = [s for s in sim.log if s.t >= t_step]
|
||
|
||
# Tiempo de asentamiento: último instante con |error| > 1°
|
||
settling_time = _settling_time(snapshots_after, TARGET, tolerance=1.0)
|
||
|
||
# Sobrepasamiento: cruce al otro lado del setpoint
|
||
overshoot = _overshoot(snapshots_after, INITIAL, TARGET)
|
||
|
||
final_error = abs(heading_error_deg(TARGET, sim.heading))
|
||
|
||
passed = (
|
||
sim.mode == AutopilotMode.HEADING_HOLD
|
||
and final_error < 1.0
|
||
and settling_time is not None
|
||
and settling_time < 60.0
|
||
and overshoot < 5.0
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"final_error_deg": round(final_error, 3),
|
||
"settling_time_s": round(settling_time, 1) if settling_time else ">120",
|
||
"overshoot_deg": round(overshoot, 2),
|
||
"final_mode": sim.mode.name,
|
||
},
|
||
message=(
|
||
f"Error: {final_error:.3f}° | "
|
||
f"Asentamiento: {settling_time:.1f}s | "
|
||
f"Sobrepasamiento: {overshoot:.2f}°"
|
||
if settling_time else
|
||
f"Error: {final_error:.3f}° | NO ASENTÓ en 120 s"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
def tc03_large_heading_change() -> TCResult:
|
||
"""
|
||
TC-03 — Cambio de rumbo grande (90°)
|
||
=====================================
|
||
Maniobra agresiva de 90°. Verifica que el ROT feed-forward limita el
|
||
sobrepasamiento y que el cascada PID completa la maniobra sin oscilar.
|
||
|
||
Criterios de aceptación:
|
||
• Asentamiento (|error| < 2°) en < 180 s
|
||
• Sobrepasamiento < 10°
|
||
• Error final < 2°
|
||
• Sin alarma OFF_COURSE (error no supera 30°)
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-03",
|
||
name="Cambio de rumbo grande +90°",
|
||
description="Maniobra de 90° → 180°. Valida el ROT feed-forward y la cascada completa.",
|
||
acceptance="Asentamiento < 180 s, sobrepasamiento < 10°, error final < 2°.",
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
TARGET = 180.0
|
||
INITIAL = 90.0
|
||
|
||
sim.inject_nmea(heading_deg=INITIAL, sog_kn=10.0)
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, int(INITIAL * 100))
|
||
sim.write_coil(0, 1)
|
||
sim.step(5.0)
|
||
|
||
t_step = sim.t
|
||
sim.write_holding(1, int(TARGET * 100))
|
||
sim.step(240.0)
|
||
|
||
snapshots_after = [s for s in sim.log if s.t >= t_step]
|
||
|
||
settling_time = _settling_time(snapshots_after, TARGET, tolerance=2.0)
|
||
overshoot = _overshoot(snapshots_after, INITIAL, TARGET)
|
||
final_error = abs(heading_error_deg(TARGET, sim.heading))
|
||
alarm_severe = sim.read_discrete(17) or sim._alarm_off_course_severe
|
||
|
||
passed = (
|
||
sim.mode == AutopilotMode.HEADING_HOLD
|
||
and final_error < 2.0
|
||
and settling_time is not None
|
||
and settling_time < 180.0
|
||
and overshoot < 10.0
|
||
and not alarm_severe
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"final_error_deg": round(final_error, 3),
|
||
"settling_time_s": round(settling_time, 1) if settling_time else ">240",
|
||
"overshoot_deg": round(overshoot, 2),
|
||
"alarm_off_course_severe": "No" if not alarm_severe else "Sí",
|
||
},
|
||
message=(
|
||
f"Error: {final_error:.3f}° | "
|
||
f"Asentamiento: {settling_time:.1f}s | "
|
||
f"Sobrepasamiento: {overshoot:.2f}°"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
def tc04_boundary_crossing() -> TCResult:
|
||
"""
|
||
TC-04 — Cruce de frontera 0°/360°
|
||
==================================
|
||
El buque va a 355° y se ordena virar a 010°. La lógica de arco más
|
||
corto debe elegir +15° (a estribor) en vez de -345° (a babor).
|
||
|
||
Criterios de aceptación:
|
||
• El timón gira a ESTRIBOR (positivo) durante la maniobra
|
||
• Asentamiento en 010° en < 90 s
|
||
• Error final < 1°
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-04",
|
||
name="Cruce de frontera 355° → 010°",
|
||
description=(
|
||
"Maniobra que cruza el meridiano 0°/360°. "
|
||
"Verifica que el shortest-arc elige el camino de +15° (estribor)."
|
||
),
|
||
acceptance="Timón a estribor, asentamiento < 90 s, error final < 1°.",
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
INITIAL = 355.0
|
||
TARGET = 10.0
|
||
|
||
sim.inject_nmea(heading_deg=INITIAL, sog_kn=10.0)
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, int(INITIAL * 100))
|
||
sim.write_coil(0, 1)
|
||
sim.step(5.0)
|
||
|
||
t_step = sim.t
|
||
sim.write_holding(1, int(TARGET * 100))
|
||
sim.step(120.0)
|
||
|
||
snapshots_after = [s for s in sim.log if s.t >= t_step]
|
||
|
||
# Verificar que el timón fue a estribor (positivo)
|
||
max_rudder_stbd = max(s.rudder_angle_deg for s in snapshots_after[:30])
|
||
went_starboard = max_rudder_stbd > 2.0
|
||
|
||
settling_time = _settling_time(snapshots_after, TARGET, tolerance=1.0)
|
||
final_error = abs(heading_error_deg(TARGET, sim.heading))
|
||
|
||
passed = (
|
||
went_starboard
|
||
and final_error < 1.0
|
||
and settling_time is not None
|
||
and settling_time < 90.0
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"went_starboard": "Sí" if went_starboard else "No",
|
||
"max_rudder_stbd_deg": round(max_rudder_stbd, 2),
|
||
"final_error_deg": round(final_error, 3),
|
||
"settling_time_s": round(settling_time, 1) if settling_time else ">120",
|
||
},
|
||
message=(
|
||
f"Timón a estribor: {'OK' if went_starboard else 'NO'} "
|
||
f"(máx={max_rudder_stbd:.1f}°) | "
|
||
f"Error: {final_error:.3f}° | "
|
||
f"Asentamiento: {settling_time:.1f}s" if settling_time else
|
||
f"Timón a estribor: {'OK' if went_starboard else 'NO'} | NO ASENTÓ"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
def tc05_manual_disengage() -> TCResult:
|
||
"""
|
||
TC-05 — Desenganche manual
|
||
==========================
|
||
El autopiloto está activo. El operador pulsa DISENGAGE.
|
||
El timón debe dejar de ser controlado (PWM = 0) inmediatamente.
|
||
|
||
Criterios de aceptación:
|
||
• El modo pasa a STANDBY en ≤ 1 tick (0.02 s)
|
||
• El PWM cae a 0 tras el desenganche
|
||
• El timón no recibe nuevas órdenes después del desenganche
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-05",
|
||
name="Desenganche manual (DISENGAGE)",
|
||
description=(
|
||
"Pulsar CMD_DISENGAGE_REQUEST mientras el autopiloto está en HEADING_HOLD. "
|
||
"El sistema debe pasar a STANDBY y soltar el timón."
|
||
),
|
||
acceptance="STANDBY en ≤ 0.02 s, PWM = 0, sin nuevas órdenes al timón.",
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
|
||
sim.inject_nmea(heading_deg=90.0, sog_kn=10.0)
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, int(100.0 * 100)) # setpoint 100° (activo, generando timón)
|
||
sim.write_coil(0, 1)
|
||
sim.step(0.1)
|
||
sim.step(10.0) # dejar que el timón esté aplicando corrección
|
||
|
||
rudder_before = sim.rudder
|
||
pwm_before = sim._inner_pwm_pct
|
||
mode_before_engaged = sim.mode == AutopilotMode.HEADING_HOLD
|
||
|
||
# DISENGAGE
|
||
sim.write_coil(1, 1)
|
||
sim.step(0.1) # un tick para procesar
|
||
|
||
mode_after = sim.mode
|
||
pwm_after = sim._inner_pwm_pct
|
||
disengage_ok = mode_after == AutopilotMode.STANDBY
|
||
|
||
# Correr 5 s más: el timón no debe recibir nuevas órdenes
|
||
rudder_samples_after = []
|
||
for _ in range(50):
|
||
sim.step(0.1)
|
||
rudder_samples_after.append(sim.rudder)
|
||
rudder_still_moving = (
|
||
max(rudder_samples_after) - min(rudder_samples_after) > 0.5
|
||
)
|
||
|
||
passed = (
|
||
mode_before_engaged
|
||
and disengage_ok
|
||
and pwm_after == 0.0
|
||
and not rudder_still_moving
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"was_engaged": "Sí" if mode_before_engaged else "No",
|
||
"mode_after": mode_after.name,
|
||
"pwm_after_pct": round(pwm_after, 2),
|
||
"rudder_drift_after_deg": round(
|
||
max(rudder_samples_after) - min(rudder_samples_after), 3
|
||
),
|
||
},
|
||
message=(
|
||
f"Antes: {mode_before_engaged and 'HH' or 'N/A'} | "
|
||
f"Después: {mode_after.name} | "
|
||
f"PWM post-disengage: {pwm_after:.2f}% | "
|
||
f"Timón comandado post-disengage: {'No' if not rudder_still_moving else 'Sí (FALLO)'}"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
def tc06_heading_lost_alarm() -> TCResult:
|
||
"""
|
||
TC-06 — Alarma HEADING_LOST (pérdida de sensor)
|
||
================================================
|
||
El compass NMEA 2000 deja de enviar datos. Tras 5 s el firmware
|
||
debe disparar ALARM_HEADING_LOST y desengranarse automáticamente.
|
||
|
||
Criterios de aceptación:
|
||
• ALARM_HEADING_LOST activa en 5.0–6.0 s desde la pérdida
|
||
• El modo pasa a STANDBY automáticamente
|
||
• El discrete[19] (ALARM_HEADING_LOST) = 1
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-06",
|
||
name="Alarma HEADING_LOST y auto-desenganche",
|
||
description=(
|
||
"Simulación de pérdida del sensor NMEA 2000. "
|
||
"El autopiloto debe detectarla y disenganchar automáticamente."
|
||
),
|
||
acceptance=(
|
||
"Alarma activa en 5–6 s, modo STANDBY, discrete[19]=1."
|
||
),
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
|
||
sim.inject_nmea(heading_deg=90.0, sog_kn=10.0)
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, 9000)
|
||
sim.write_coil(0, 1)
|
||
sim.step(5.0) # estabilizar
|
||
|
||
assert sim.mode == AutopilotMode.HEADING_HOLD, "Precondición: debe estar enganchado"
|
||
|
||
t_loss = sim.t
|
||
sim.disconnect_nmea_heading()
|
||
|
||
# Avanzar en pequeños pasos registrando cuándo dispara la alarma
|
||
alarm_fired_at: float | None = None
|
||
disengage_at: float | None = None
|
||
for _ in range(100): # hasta 10 s
|
||
sim.step(0.1)
|
||
if sim.read_discrete(19) and alarm_fired_at is None:
|
||
alarm_fired_at = sim.t
|
||
if sim.mode == AutopilotMode.STANDBY and disengage_at is None:
|
||
disengage_at = sim.t
|
||
|
||
time_to_alarm = (alarm_fired_at - t_loss) if alarm_fired_at else None
|
||
|
||
passed = (
|
||
alarm_fired_at is not None
|
||
and 4.5 <= (alarm_fired_at - t_loss) <= 7.0
|
||
and sim.mode == AutopilotMode.STANDBY
|
||
and sim.read_discrete(19) == 1
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"time_to_alarm_s": round(time_to_alarm, 2) if time_to_alarm else "nunca",
|
||
"final_mode": sim.mode.name,
|
||
"discrete_19": sim.read_discrete(19),
|
||
},
|
||
message=(
|
||
f"Alarma a los {time_to_alarm:.2f}s post-pérdida | "
|
||
f"Modo: {sim.mode.name} | "
|
||
f"Discrete[19]: {sim.read_discrete(19)}"
|
||
if time_to_alarm else
|
||
"ALARMA NO DISPARADA en 10 s — FALLO"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
def tc07_dodge_mode() -> TCResult:
|
||
"""
|
||
TC-07 — Modo DODGE (esquiva temporal)
|
||
======================================
|
||
El operador activa DODGE con un offset de +20° para esquivar un
|
||
obstáculo. El autopiloto vira 20° a estribor. Luego el operador
|
||
cancela el DODGE volviendo a HEADING_HOLD en el rumbo original.
|
||
|
||
Criterios de aceptación:
|
||
• Modo cambia a DODGE
|
||
• El buque vira hacia el setpoint de DODGE (original + 20°)
|
||
• Tras volver a HEADING_HOLD, el error respecto al rumbo original < 1°
|
||
"""
|
||
spec = TCSpec(
|
||
id="TC-07",
|
||
name="Modo DODGE (+20°) y retorno",
|
||
description=(
|
||
"El operador esquiva con offset +20° y luego vuelve al rumbo original. "
|
||
"Verifica la transición HEADING_HOLD ↔ DODGE ↔ HEADING_HOLD."
|
||
),
|
||
acceptance=(
|
||
"DODGE activo, buque vira al setpoint DODGE, retorno sin error > 1°."
|
||
),
|
||
)
|
||
|
||
sim = ESP32Simulator()
|
||
ORIGINAL_SP = 90.0
|
||
DODGE_OFFSET = 20.0
|
||
DODGE_SP = (ORIGINAL_SP + DODGE_OFFSET) % 360.0
|
||
|
||
sim.inject_nmea(heading_deg=ORIGINAL_SP, sog_kn=10.0)
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, int(ORIGINAL_SP * 100))
|
||
sim.write_coil(0, 1)
|
||
sim.step(10.0) # estabilizar en 90°
|
||
|
||
initial_heading_error = abs(heading_error_deg(ORIGINAL_SP, sim.heading))
|
||
|
||
# Activar DODGE con offset +20°
|
||
sim.write_holding(0, 4) # MODE_REQUEST = DODGE
|
||
sim.write_holding(8, int(DODGE_OFFSET * 100)) # DODGE_OFFSET_DEG_X100
|
||
sim.write_coil(0, 1) # CMD_ENGAGE_REQUEST
|
||
sim.step(0.1)
|
||
|
||
dodge_engaged = sim.mode == AutopilotMode.DODGE
|
||
dodge_setpoint = sim._heading_setpoint_deg
|
||
|
||
# Dejar que el buque vire hacia el setpoint DODGE
|
||
sim.step(90.0)
|
||
|
||
heading_at_dodge = sim.heading
|
||
dodge_error = abs(heading_error_deg(DODGE_SP, heading_at_dodge))
|
||
|
||
# Volver a HEADING_HOLD con el rumbo original
|
||
sim.write_holding(0, 1)
|
||
sim.write_holding(1, int(ORIGINAL_SP * 100))
|
||
sim.write_coil(0, 1)
|
||
sim.step(0.1)
|
||
returned_to_hh = sim.mode == AutopilotMode.HEADING_HOLD
|
||
sim.step(120.0)
|
||
|
||
final_error = abs(heading_error_deg(ORIGINAL_SP, sim.heading))
|
||
|
||
passed = (
|
||
dodge_engaged
|
||
and abs(dodge_setpoint - DODGE_SP) < 0.5
|
||
and dodge_error < 2.0
|
||
and returned_to_hh
|
||
and final_error < 1.0
|
||
)
|
||
|
||
return TCResult(
|
||
spec=spec,
|
||
passed=passed,
|
||
metrics={
|
||
"dodge_engaged": "Sí" if dodge_engaged else "No",
|
||
"dodge_setpoint_deg": round(dodge_setpoint, 2),
|
||
"heading_at_dodge_end_deg": round(heading_at_dodge, 2),
|
||
"dodge_error_deg": round(dodge_error, 3),
|
||
"returned_to_hh": "Sí" if returned_to_hh else "No",
|
||
"final_error_vs_original_deg": round(final_error, 3),
|
||
},
|
||
message=(
|
||
f"DODGE: {'OK' if dodge_engaged else 'NO'} sp={dodge_setpoint:.1f}° | "
|
||
f"Error en DODGE: {dodge_error:.2f}° | "
|
||
f"Retorno HH: {'OK' if returned_to_hh else 'NO'} | "
|
||
f"Error final vs original: {final_error:.3f}°"
|
||
),
|
||
snapshots=sim.log,
|
||
events=sim.events,
|
||
)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Registro de todos los TCs
|
||
# ---------------------------------------------------------------------------
|
||
ALL_TCS: dict[str, Callable[[], TCResult]] = {
|
||
"TC-01": tc01_power_on_engage,
|
||
"TC-02": tc02_small_heading_change,
|
||
"TC-03": tc03_large_heading_change,
|
||
"TC-04": tc04_boundary_crossing,
|
||
"TC-05": tc05_manual_disengage,
|
||
"TC-06": tc06_heading_lost_alarm,
|
||
"TC-07": tc07_dodge_mode,
|
||
}
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Helpers de métricas
|
||
# ---------------------------------------------------------------------------
|
||
def _settling_time(
|
||
snapshots: list[SimSnapshot], target: float, tolerance: float = 1.0
|
||
) -> float | None:
|
||
"""
|
||
Retorna el instante en que |error| entra en la banda de tolerancia
|
||
y ya no vuelve a salir. None si nunca asienta.
|
||
"""
|
||
if not snapshots:
|
||
return None
|
||
# Buscar el último instante donde |error| > tolerance
|
||
last_out = None
|
||
for s in snapshots:
|
||
if abs(heading_error_deg(target, s.heading_deg)) > tolerance:
|
||
last_out = s.t
|
||
if last_out is None:
|
||
return snapshots[0].t # siempre estuvo dentro
|
||
# Verificar que después de last_out permanece dentro
|
||
after = [s for s in snapshots if s.t > last_out]
|
||
if all(abs(heading_error_deg(target, s.heading_deg)) <= tolerance for s in after):
|
||
return last_out
|
||
return None # salió de la banda después del último out
|
||
|
||
|
||
def _overshoot(
|
||
snapshots: list[SimSnapshot], initial: float, target: float
|
||
) -> float:
|
||
"""
|
||
Sobrepasamiento en grados respecto al setpoint en la dirección de giro.
|
||
Positivo = cruzó el setpoint en la dirección del movimiento.
|
||
"""
|
||
if not snapshots:
|
||
return 0.0
|
||
going_right = heading_error_deg(target, initial) > 0 # girando a estribor
|
||
peak = 0.0
|
||
for s in snapshots[3:]: # ignorar los primeros muestras (transitorio inicial)
|
||
err = heading_error_deg(target, s.heading_deg)
|
||
if going_right:
|
||
# Sobrepasamiento = haber ido más allá del setpoint (error negativo)
|
||
if err < 0:
|
||
peak = max(peak, abs(err))
|
||
else:
|
||
if err > 0:
|
||
peak = max(peak, abs(err))
|
||
return peak
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Salida en consola
|
||
# ---------------------------------------------------------------------------
|
||
def _print_summary(results: list[TCResult]) -> None:
|
||
print()
|
||
print("=" * 72)
|
||
print(" AR-AUTOPILOT - Protocolo de Simulacion HIL")
|
||
print(f" {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
|
||
print("=" * 72)
|
||
print(f" {'ID':<8} {'Nombre':<35} {'Resultado':<8} {'Nota'}")
|
||
print(" " + "-" * 68)
|
||
for r in results:
|
||
badge = "[PASS]" if r.passed else "[FAIL]"
|
||
name = r.spec.name[:35]
|
||
note = r.message[:40] if len(r.message) > 40 else r.message
|
||
print(f" {r.spec.id:<8} {name:<35} {badge:<8} {note}")
|
||
print(" " + "-" * 68)
|
||
n_pass = sum(1 for r in results if r.passed)
|
||
n_fail = len(results) - n_pass
|
||
print(f" Total: {len(results)} PASS: {n_pass} FAIL: {n_fail}")
|
||
print("=" * 72)
|
||
print()
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Generador de informe HTML
|
||
# ---------------------------------------------------------------------------
|
||
def generate_html_report(results: list[TCResult], out_path: Path) -> None:
|
||
"""Genera un informe HTML autocontenido con gráficas Chart.js."""
|
||
|
||
n_pass = sum(1 for r in results if r.passed)
|
||
n_fail = len(results) - n_pass
|
||
overall_badge = "PASS" if n_fail == 0 else "FAIL"
|
||
overall_color = "#22c55e" if n_fail == 0 else "#ef4444"
|
||
|
||
tc_cards_html = ""
|
||
for r in results:
|
||
tc_cards_html += _render_tc_card(r)
|
||
|
||
html_content = f"""<!DOCTYPE html>
|
||
<html lang="es">
|
||
<head>
|
||
<meta charset="UTF-8">
|
||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||
<title>AR-Autopilot — Protocolo de Simulación</title>
|
||
<script src="https://cdn.jsdelivr.net/npm/chart.js@4.4.0/dist/chart.umd.min.js"></script>
|
||
<style>
|
||
:root {{
|
||
--bg: #0d1822; --bg-mid: #0f172a; --panel: #14202e;
|
||
--border: rgba(56,189,248,.22); --text: #e0f2fe; --muted: #64748b;
|
||
--accent: #38bdf8; --ok: #4ade80; --fail: #ef4444;
|
||
--warn: #fbbf24; --set: #fbbf24;
|
||
}}
|
||
* {{ box-sizing: border-box; margin: 0; padding: 0; }}
|
||
body {{ background: var(--bg); color: var(--text); font-family: 'Courier New', monospace; padding: 24px; }}
|
||
h1 {{ color: var(--accent); font-size: 1.4rem; margin-bottom: 4px; }}
|
||
h2 {{ color: var(--accent); font-size: 1.05rem; margin-bottom: 12px; }}
|
||
.meta {{ color: var(--muted); font-size: .8rem; margin-bottom: 24px; }}
|
||
.overall {{ display: flex; align-items: center; gap: 16px; margin-bottom: 28px; }}
|
||
.badge {{ padding: 6px 18px; border-radius: 4px; font-weight: bold; font-size: 1.1rem; }}
|
||
.badge-pass {{ background: var(--ok); color: #0a1a0a; }}
|
||
.badge-fail {{ background: var(--fail); color: #fff; }}
|
||
.stats {{ color: var(--muted); font-size: .9rem; }}
|
||
.tc-card {{ background: var(--panel); border: 1px solid var(--border);
|
||
border-radius: 6px; padding: 20px; margin-bottom: 20px; }}
|
||
.tc-header {{ display: flex; align-items: baseline; gap: 12px; margin-bottom: 8px; }}
|
||
.tc-id {{ color: var(--accent); font-size: 1rem; font-weight: bold; }}
|
||
.tc-name {{ font-size: .95rem; color: var(--text); }}
|
||
.tc-badge {{ padding: 2px 10px; border-radius: 3px; font-size: .85rem; font-weight: bold; }}
|
||
.pass {{ background: var(--ok); color: #0a1a0a; }}
|
||
.fail {{ background: var(--fail); color: #fff; }}
|
||
.tc-desc {{ color: var(--muted); font-size: .8rem; margin-bottom: 10px; }}
|
||
.tc-accept {{ color: var(--warn); font-size: .8rem; margin-bottom: 12px; }}
|
||
.metrics {{ display: flex; flex-wrap: wrap; gap: 8px; margin-bottom: 14px; }}
|
||
.metric {{ background: #0a1220; border: 1px solid var(--border); border-radius: 4px;
|
||
padding: 5px 10px; font-size: .8rem; }}
|
||
.metric-key {{ color: var(--muted); }}
|
||
.metric-val {{ color: var(--accent); font-weight: bold; }}
|
||
.tc-msg {{ color: var(--text); font-size: .82rem; margin-bottom: 14px; opacity: .85; }}
|
||
.chart-wrap {{ position: relative; height: 240px; }}
|
||
.events {{ margin-top: 10px; font-size: .75rem; color: var(--muted); }}
|
||
.event {{ margin-bottom: 2px; }}
|
||
.ev-engage {{ color: var(--ok); }}
|
||
.ev-disengage {{ color: var(--warn); }}
|
||
.ev-alarm {{ color: var(--fail); }}
|
||
.ev-ack {{ color: var(--accent); }}
|
||
</style>
|
||
</head>
|
||
<body>
|
||
<h1>AR-Autopilot · Protocolo de Simulación HIL</h1>
|
||
<div class="meta">Generado: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} · Simulador Python puro · Sin hardware ESP32</div>
|
||
<div class="overall">
|
||
<span class="badge badge-{'pass' if n_fail==0 else 'fail'}">{overall_badge}</span>
|
||
<span class="stats">{n_pass}/{len(results)} casos superados | {n_fail} fallos</span>
|
||
</div>
|
||
{tc_cards_html}
|
||
</body>
|
||
</html>"""
|
||
|
||
out_path.write_text(html_content, encoding="utf-8")
|
||
print(f" → Informe HTML: {out_path.resolve()}")
|
||
|
||
|
||
def _render_tc_card(r: TCResult) -> str:
|
||
"""Genera el HTML de una tarjeta de caso de prueba."""
|
||
passed_class = "pass" if r.passed else "fail"
|
||
passed_label = "PASS" if r.passed else "FAIL"
|
||
|
||
# Métricas
|
||
metrics_html = ""
|
||
for k, v in r.metrics.items():
|
||
metrics_html += (
|
||
f'<span class="metric">'
|
||
f'<span class="metric-key">{html.escape(str(k))}</span>: '
|
||
f'<span class="metric-val">{html.escape(str(v))}</span>'
|
||
f'</span>'
|
||
)
|
||
|
||
# Datos del gráfico (subsamplear si hay muchos puntos)
|
||
snaps = r.snapshots
|
||
if len(snaps) > 600:
|
||
step = len(snaps) // 600
|
||
snaps = snaps[::step]
|
||
|
||
times = [round(s.t, 2) for s in snaps]
|
||
headings = [round(s.heading_deg, 2) for s in snaps]
|
||
setpoints = [round(s.heading_setpoint_deg, 2) for s in snaps]
|
||
rudders = [round(s.rudder_angle_deg, 2) for s in snaps]
|
||
rots = [round(s.rot_dps, 3) for s in snaps]
|
||
|
||
chart_id = r.spec.id.replace("-", "_")
|
||
|
||
# Eventos
|
||
events_html = ""
|
||
for ev in r.events:
|
||
ev_class = {
|
||
"engage": "ev-engage", "engage_refused": "ev-alarm",
|
||
"disengage": "ev-disengage", "alarm": "ev-alarm", "ack": "ev-ack",
|
||
}.get(ev.kind, "")
|
||
events_html += (
|
||
f'<div class="event {ev_class}">'
|
||
f't={ev.t:.2f}s [{ev.kind}] {html.escape(ev.detail)}'
|
||
f'</div>'
|
||
)
|
||
|
||
return f"""
|
||
<div class="tc-card">
|
||
<div class="tc-header">
|
||
<span class="tc-id">{html.escape(r.spec.id)}</span>
|
||
<span class="tc-name">{html.escape(r.spec.name)}</span>
|
||
<span class="tc-badge {passed_class}">{passed_label}</span>
|
||
</div>
|
||
<div class="tc-desc">{html.escape(r.spec.description)}</div>
|
||
<div class="tc-accept">* Aceptación: {html.escape(r.spec.acceptance)}</div>
|
||
<div class="metrics">{metrics_html}</div>
|
||
<div class="tc-msg">{html.escape(r.message)}</div>
|
||
<div class="chart-wrap">
|
||
<canvas id="chart_{chart_id}"></canvas>
|
||
</div>
|
||
<div class="events">{events_html}</div>
|
||
</div>
|
||
<script>
|
||
(function() {{
|
||
var ctx = document.getElementById('chart_{chart_id}').getContext('2d');
|
||
new Chart(ctx, {{
|
||
type: 'line',
|
||
data: {{
|
||
labels: {json.dumps(times)},
|
||
datasets: [
|
||
{{
|
||
label: 'Rumbo (°)',
|
||
data: {json.dumps(headings)},
|
||
borderColor: '#38bdf8', borderWidth: 1.5,
|
||
pointRadius: 0, tension: 0.1, yAxisID: 'y'
|
||
}},
|
||
{{
|
||
label: 'Setpoint (°)',
|
||
data: {json.dumps(setpoints)},
|
||
borderColor: '#fbbf24', borderWidth: 1.5, borderDash: [4, 3],
|
||
pointRadius: 0, tension: 0, yAxisID: 'y'
|
||
}},
|
||
{{
|
||
label: 'Timón (°)',
|
||
data: {json.dumps(rudders)},
|
||
borderColor: '#4ade80', borderWidth: 1,
|
||
pointRadius: 0, tension: 0.1, yAxisID: 'y2'
|
||
}},
|
||
{{
|
||
label: 'ROT (°/s)',
|
||
data: {json.dumps(rots)},
|
||
borderColor: '#fb923c', borderWidth: 1,
|
||
pointRadius: 0, tension: 0.1, yAxisID: 'y2'
|
||
}}
|
||
]
|
||
}},
|
||
options: {{
|
||
animation: false,
|
||
interaction: {{ mode: 'index', intersect: false }},
|
||
plugins: {{
|
||
legend: {{ labels: {{ color: '#e0f2fe', font: {{ size: 11 }} }} }},
|
||
tooltip: {{ backgroundColor: '#0d1822', titleColor: '#38bdf8', bodyColor: '#e0f2fe' }}
|
||
}},
|
||
scales: {{
|
||
x: {{
|
||
ticks: {{ color: '#64748b', font: {{ size: 10 }}, maxTicksLimit: 12 }},
|
||
grid: {{ color: 'rgba(56,189,248,.08)' }},
|
||
title: {{ display: true, text: 'Tiempo (s)', color: '#64748b', font: {{ size: 11 }} }}
|
||
}},
|
||
y: {{
|
||
ticks: {{ color: '#38bdf8', font: {{ size: 10 }} }},
|
||
grid: {{ color: 'rgba(56,189,248,.08)' }},
|
||
title: {{ display: true, text: 'Rumbo / Setpoint (°)', color: '#38bdf8', font: {{ size: 11 }} }}
|
||
}},
|
||
y2: {{
|
||
position: 'right',
|
||
ticks: {{ color: '#4ade80', font: {{ size: 10 }} }},
|
||
grid: {{ drawOnChartArea: false }},
|
||
title: {{ display: true, text: 'Timón (°) / ROT (°/s)', color: '#4ade80', font: {{ size: 11 }} }}
|
||
}}
|
||
}}
|
||
}}
|
||
}});
|
||
}})();
|
||
</script>
|
||
"""
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Entry point
|
||
# ---------------------------------------------------------------------------
|
||
def main() -> int:
|
||
parser = argparse.ArgumentParser(
|
||
description="AR-Autopilot: protocolo de simulación HIL",
|
||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||
epilog=textwrap.dedent("""
|
||
Ejemplos:
|
||
python tools/sim_protocol.py
|
||
python tools/sim_protocol.py TC-02 TC-04
|
||
python tools/sim_protocol.py --out resultado.html
|
||
"""),
|
||
)
|
||
parser.add_argument(
|
||
"tests", nargs="*", metavar="TC-NN",
|
||
help="IDs de casos a ejecutar (default: todos)",
|
||
)
|
||
parser.add_argument(
|
||
"--out", default="tools/sim_report.html",
|
||
metavar="ARCHIVO",
|
||
help="Ruta del informe HTML (default: tools/sim_report.html)",
|
||
)
|
||
args = parser.parse_args()
|
||
|
||
selected = args.tests or list(ALL_TCS.keys())
|
||
unknown = [tc for tc in selected if tc not in ALL_TCS]
|
||
if unknown:
|
||
print(f"Error: TCs desconocidos: {', '.join(unknown)}")
|
||
print(f"Disponibles: {', '.join(ALL_TCS)}")
|
||
return 1
|
||
|
||
results: list[TCResult] = []
|
||
for tc_id in selected:
|
||
print(f" Ejecutando {tc_id} …", end="", flush=True)
|
||
result = ALL_TCS[tc_id]()
|
||
badge = "PASS" if result.passed else "FAIL"
|
||
print(f" {badge}")
|
||
results.append(result)
|
||
|
||
_print_summary(results)
|
||
|
||
out_path = Path(args.out)
|
||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||
generate_html_report(results, out_path)
|
||
|
||
return 0 if all(r.passed for r in results) else 1
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|