feat: BNO085 IMU integration — SPICE + simulator yaw rate feed-forward

SPICE (6_bno085_imu.cir):
  - BNO085 power supply with 10uF + 100nF decoupling on VDD
  - Power-on reset RC circuit (R=10K, C=1uF, tau=10ms → deasserts at ~12ms)
  - I2C Fast Mode 400kHz bus: 4.7K pull-ups, 50pF bus capacitance model
  - Full I2C transaction: START + address 0x4A + R/W + BNO085 ACK + STOP
  - INT pin (open-drain, 10K pull-up, 100Hz interrupt simulation)
  - .meas directives: reset timing, SCL rise time, VDD stability

Simulator (esp32_sim.py):
  - SimSnapshot.bno085_yaw_rate_dps field added
  - _bno085_enabled / _bno085_noise_std_dps / _bno085_yaw_rate_dps state
  - enable_bno085(noise_std_dps=0.02) public method
  - disable_bno085() public method
  - _run_physics: samples gyro at 50Hz with Gaussian noise model
  - _run_outer_loop: uses BNO085 yaw rate for rot_ff_term when enabled
    (replaces NMEA-derived ROT — lower latency ~4ms vs ~100-200ms)

Usage:
  sim.enable_bno085()          # activate gyro feed-forward
  sim.enable_bno085(noise_std_dps=0.014)  # with BNO085 spec noise

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
2026-05-22 22:46:16 -04:00
parent 3b36f178aa
commit b82ed400bc
2 changed files with 350 additions and 1 deletions
+57 -1
View File
@@ -126,6 +126,7 @@ class SimSnapshot:
alarm_heading_lost: bool
alarm_off_course: bool
alarm_off_course_severe: bool
bno085_yaw_rate_dps: float = 0.0 # medición del giróscopo BNO085 (0 si desactivado)
@dataclass
@@ -295,6 +296,15 @@ class ESP32Simulator:
self._sea_wave_period: float = 8.0
self._sea_rng: random.Random = random.Random(42)
# -- BNO085 IMU (yaw rate de alta frecuencia) --------------------
# Cuando está activo, el rot_ff_term del outer PID usa la medición
# del giróscopo BNO085 (50 Hz, ruido ~0.02 °/s) en lugar del ROT
# derivado del NMEA compass (10 Hz, mayor latencia).
# Esto reduce el overshoot en maniobras y el cabeceo en olas.
self._bno085_enabled: bool = False
self._bno085_noise_std_dps: float = 0.02 # spec BNO085: ~0.014 °/s
self._bno085_yaw_rate_dps: float = 0.0
# -- Registro de telemetría para análisis ------------------------
self.log: list[SimSnapshot] = []
self.events: list[SimEvent] = []
@@ -417,6 +427,36 @@ class ESP32Simulator:
if max_rudder_deg is not None:
self._pid_outer.config.max_rudder_deg = float(max_rudder_deg)
def enable_bno085(self, *, noise_std_dps: float = 0.02) -> None:
"""
Activa el BNO085 como fuente de yaw rate para el outer PID.
Con BNO085 activo el ``rot_ff_term`` del outer PID usa la medición
del giróscopo (50 Hz, ruido típico ~0.014 °/s) en lugar del ROT
derivado del bus NMEA 2000 (10 Hz, mayor latencia de bus + GPS).
Diferencia clave respecto al ROT por NMEA:
- Latencia: BNO085 ≈ 4 ms vs NMEA ROT ≈ 100-200 ms
- Ruido: BNO085 ≈ 0.02 °/s vs NMEA ROT ≈ 0.1-0.5 °/s
- Frecuencia: BNO085 muestrea a 250 Hz (gyro), outer PID consume a 10 Hz
En mal tiempo (B4-B5) el BNO085 permite que el ``rot_ff_term``
reaccione a los picos de guiñada generados por las olas antes de
que el error de heading se acumule — equivale a subir el knob
"COUNTER RUDDER" manteniendo la ganancia proporcional estable.
Args:
noise_std_dps: Desviación estándar del ruido del giróscopo en °/s.
BNO085 spec: ~0.014 °/s típico. Default 0.02 °/s (conservador).
"""
self._bno085_enabled = True
self._bno085_noise_std_dps = float(noise_std_dps)
def disable_bno085(self) -> None:
"""Desactiva el BNO085; el outer PID vuelve a usar ROT del NMEA 2000."""
self._bno085_enabled = False
self._bno085_yaw_rate_dps = 0.0
# -----------------------------------------------------------------------
# API pública — Interfaz Modbus
# -----------------------------------------------------------------------
@@ -648,10 +688,15 @@ class ESP32Simulator:
sp_speed_raw = self._holdings[25] # PID_OUTER_SPEED_KN_REQ_X10
sog_kn = (sp_speed_raw * 0.1) if sp_speed_raw > 0 else self._nmea_sog_kn
# BNO085 activo → usa yaw rate del giróscopo (50 Hz, baja latencia).
# BNO085 inactivo → usa ROT del bus NMEA 2000 (10 Hz, GPS/compass).
rot_for_pid = (self._bno085_yaw_rate_dps if self._bno085_enabled
else self._nmea_rot_dps)
self._outer_rudder_sp = self._pid_outer.step(
heading_setpoint_deg=self._heading_setpoint_deg,
heading_measured_deg=self._nmea_heading_deg,
rate_of_turn_dps=self._nmea_rot_dps,
rate_of_turn_dps=rot_for_pid,
speed_kn=sog_kn,
allowed=True,
)
@@ -715,6 +760,16 @@ class ESP32Simulator:
self._nmea_cog_deg = self._nmea_heading_deg
self._last_nmea_update_t = self._t
# BNO085: muestrea yaw rate a 50 Hz (mismo ritmo que el inner loop).
# El giróscopo del BNO085 tiene latencia ~4ms y ruido ~0.02 °/s,
# mucho mejor que el ROT calculado desde el GPS/compass NMEA (100-200ms).
# En el outer PID (10 Hz) esto significa que rot_ff_term reacciona a
# picos de guiñada por olas ~10x más rápido que con ROT por NMEA.
if self._bno085_enabled:
noise = (self._sea_rng.gauss(0.0, self._bno085_noise_std_dps)
if self._bno085_noise_std_dps > 0.0 else 0.0)
self._bno085_yaw_rate_dps = self._vessel.state.rate_of_turn_dps + noise
# -----------------------------------------------------------------------
# Internos — alarmas
# -----------------------------------------------------------------------
@@ -825,6 +880,7 @@ class ESP32Simulator:
alarm_heading_lost=self._alarm_heading_lost,
alarm_off_course=self._alarm_off_course,
alarm_off_course_severe=self._alarm_off_course_severe,
bno085_yaw_rate_dps=self._bno085_yaw_rate_dps,
))
def _add_event(self, kind: str, detail: str = "") -> None: