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:
+57
-1
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user