""" modulo_imu.py — Sistema Vestibular del Robot ============================================= Interfaz con el sensor MPU6050 via I2C en Raspberry Pi 5. Responsabilidades: - Leer velocidad angular del eje X (Yaw) del giroscopio - Integrar numéricamente para obtener ángulo absoluto - Detectar y compensar el drift del giroscopio (bias) - Exponer theta_rad (radianes) para fusión de sensores en SLAM Conexión hardware: MPU6050 VCC → Pin 1 (3.3V) MPU6050 GND → Pin 6 (GND) MPU6050 SDA → Pin 3 (GPIO2, I2C1 SDA) MPU6050 SCL → Pin 5 (GPIO3, I2C1 SCL) MPU6050 AD0 → GND (dirección I2C = 0x68) Dependencias: pip install smbus2 """ import smbus2 import time import threading import logging import math logger = logging.getLogger(__name__) # ── Registros del MPU6050 ───────────────────────────────────────────────────── MPU6050_ADDR = 0x68 # AD0 en GND → 0x68; AD0 en VCC → 0x69 REG_PWR_MGMT_1 = 0x6B REG_SMPLRT_DIV = 0x19 REG_CONFIG = 0x1A REG_GYRO_CONFIG = 0x1B REG_ACCEL_CONFIG = 0x1C REG_GYRO_XOUT_H = 0x43 REG_GYRO_YOUT_H = 0x45 REG_GYRO_ZOUT_H = 0x47 REG_ACCEL_ZOUT_H = 0x3F REG_WHO_AM_I = 0x75 # ── Escalas del giroscopio ──────────────────────────────────────────────────── # FS_SEL: 0 = ±250°/s, 1 = ±500°/s, 2 = ±1000°/s, 3 = ±2000°/s GYRO_FS_SEL = 0 # ±250°/s → mejor resolución para robot lento GYRO_SCALE = {0: 131.0, 1: 65.5, 2: 32.8, 3: 16.4} # LSB/(°/s) ACCEL_FS_SEL = 0 # ±2g ACCEL_SCALE = {0: 16384.0, 1: 8192.0, 2: 4096.0, 3: 2048.0} # LSB/g # ── Configuración del filtro DLPF ───────────────────────────────────────────── # DLPF_CFG 3 → Ancho de banda 44 Hz, delay 4.9ms — buen balance ruido/respuesta DLPF_CFG = 3 # ── Número de muestras para calibración de bias ─────────────────────────────── CALIBRATION_SAMPLES = 500 class IMU: """ Interfaz de alto nivel con el MPU6050. Uso básico: imu = IMU(bus=1) imu.iniciar() ... theta = imu.theta_rad # ángulo Yaw integrado en radianes imu.detener() """ def __init__(self, bus: int = 1, direccion: int = MPU6050_ADDR, frecuencia_hz: float = 200.0): """ Args: bus: Número del bus I2C (1 para RPi 5, pines 3/5). direccion: Dirección I2C del MPU6050 (0x68 o 0x69). frecuencia_hz: Frecuencia de muestreo del bucle de integración. """ self._bus_num = bus self._addr = direccion self._frecuencia = frecuencia_hz self._dt = 1.0 / frecuencia_hz self._bus: smbus2.SMBus | None = None # Estado angular (acceso thread-safe) self._lock = threading.Lock() self._theta_rad = 0.0 # Yaw integrado self._omega_x_rads = 0.0 # Velocidad angular actual (rad/s) self._accel_z_ms2 = 0.0 # Aceleración lineal eje Z (m/s²) # Calibración (bias en reposo) self._bias_x_deg_s = 0.0 self._bias_accel_z_ms2 = 0.0 # Escala activa self._escala = GYRO_SCALE[GYRO_FS_SEL] self._escala_accel = ACCEL_SCALE[ACCEL_FS_SEL] # Hilo de lectura self._hilo: threading.Thread | None = None self._activo = False # ── Propiedades públicas (thread-safe) ──────────────────────────────────── @property def theta_rad(self) -> float: """Ángulo Yaw acumulado en radianes. Positivo = giro anti-horario.""" with self._lock: return self._theta_rad @property def theta_deg(self) -> float: """Ángulo Yaw acumulado en grados.""" return math.degrees(self.theta_rad) @property def omega_x_rads(self) -> float: """Velocidad angular instantánea en el eje X (rad/s).""" with self._lock: return self._omega_x_rads @property def accel_z_ms2(self) -> float: """Aceleración lineal instantánea en el eje Z (m/s²).""" with self._lock: return self._accel_z_ms2 # ── Inicialización ──────────────────────────────────────────────────────── def iniciar(self) -> None: """ Abre el bus I2C, configura el MPU6050, calibra el bias y lanza el hilo de integración en segundo plano. """ logger.info("IMU: Abriendo bus I2C-%d, dirección 0x%02X", self._bus_num, self._addr) self._bus = smbus2.SMBus(self._bus_num) self._verificar_chip() self._configurar_chip() logger.info("IMU: Calibrando bias de giroscopio y acelerómetro (%d muestras)...", CALIBRATION_SAMPLES) self._calibrar_bias() logger.info("IMU: Bias Eje-X = %.4f °/s | Bias Accel Z = %.4f m/s²", self._bias_x_deg_s, self._bias_accel_z_ms2) self._activo = True self._hilo = threading.Thread(target=self._bucle_integracion, name="hilo-imu", daemon=True) self._hilo.start() logger.info("IMU: Hilo de integración iniciado a %.0f Hz", self._frecuencia) def detener(self) -> None: """Detiene el hilo de lectura y cierra el bus I2C.""" self._activo = False if self._hilo: self._hilo.join(timeout=2.0) if self._bus: self._bus.close() logger.info("IMU: Detenida.") def resetear_angulo(self, valor_rad: float = 0.0) -> None: """ Reestablece el ángulo Yaw integrado a un valor arbitrario. Útil cuando el SLAM cierra un loop y corrige la pose. """ with self._lock: self._theta_rad = valor_rad # ── Configuración del chip ──────────────────────────────────────────────── def _verificar_chip(self) -> None: who = self._bus.read_byte_data(self._addr, REG_WHO_AM_I) if who != 0x68: raise RuntimeError( f"IMU: WHO_AM_I=0x{who:02X} — ¿MPU6050 conectado y AD0 en GND?" ) logger.debug("IMU: WHO_AM_I OK (0x68)") def _configurar_chip(self) -> None: # Despertar el chip (sale del modo sleep) self._bus.write_byte_data(self._addr, REG_PWR_MGMT_1, 0x00) time.sleep(0.1) # Seleccionar reloj del giroscopio eje X (más estable que el oscilador interno) self._bus.write_byte_data(self._addr, REG_PWR_MGMT_1, 0x01) # Sample Rate Divider: SMPLRT_DIV=0 → Fsample = 8 kHz / (1+0) = 8 kHz # (El hilo de Python muestrea a 200 Hz; el chip siempre está listo) self._bus.write_byte_data(self._addr, REG_SMPLRT_DIV, 0x00) # Filtro pasa-bajas digital (DLPF) self._bus.write_byte_data(self._addr, REG_CONFIG, DLPF_CFG) # Rango del giroscopio y acelerómetro self._bus.write_byte_data(self._addr, REG_GYRO_CONFIG, GYRO_FS_SEL << 3) self._bus.write_byte_data(self._addr, REG_ACCEL_CONFIG, ACCEL_FS_SEL << 3) logger.debug("IMU: Chip configurado. Rango=±%s°/s, DLPF=%d", {0: 250, 1: 500, 2: 1000, 3: 2000}[GYRO_FS_SEL], DLPF_CFG) # ── Lectura de registros ────────────────────────────────────────────────── def _leer_gyro_x_raw(self) -> int: """Lee los 2 bytes del giroscopio eje X y devuelve el entero con signo.""" high = self._bus.read_byte_data(self._addr, REG_GYRO_XOUT_H) low = self._bus.read_byte_data(self._addr, REG_GYRO_XOUT_H + 1) valor = (high << 8) | low # Convertir a complemento a dos if valor >= 0x8000: valor -= 0x10000 return valor def _leer_gyro_x_deg_s(self) -> float: """Devuelve la velocidad angular del eje X en grados/segundo.""" return self._leer_gyro_x_raw() / self._escala def _leer_accel_z_raw(self) -> int: """Lee los 2 bytes del acelerómetro eje Z y devuelve el entero con signo.""" high = self._bus.read_byte_data(self._addr, REG_ACCEL_ZOUT_H) low = self._bus.read_byte_data(self._addr, REG_ACCEL_ZOUT_H + 1) valor = (high << 8) | low if valor >= 0x8000: valor -= 0x10000 return valor def _leer_accel_z_ms2_crudo(self) -> float: """Devuelve la aceleración cruda del eje Z en m/s².""" g_force = self._leer_accel_z_raw() / self._escala_accel return g_force * 9.80665 # ── Calibración ─────────────────────────────────────────────────────────── def _calibrar_bias(self) -> None: """ Estima el drift (offset) del giroscopio y la gravedad estática del acelerómetro en reposo. El robot DEBE estar completamente inmóvil durante este proceso. """ acumulador_gyro = 0.0 acumulador_accel = 0.0 for _ in range(CALIBRATION_SAMPLES): acumulador_gyro += self._leer_gyro_x_deg_s() acumulador_accel += self._leer_accel_z_ms2_crudo() time.sleep(0.002) # ~500 Hz durante calibración self._bias_x_deg_s = acumulador_gyro / CALIBRATION_SAMPLES self._bias_accel_z_ms2 = acumulador_accel / CALIBRATION_SAMPLES # ── Bucle de integración ────────────────────────────────────────────────── def _bucle_integracion(self) -> None: """ Hilo principal de integración. Lee ω_x, resta el bias, integra numéricamente y actualiza theta_rad. Δθ = -(ω_x − bias) × Δt θ_actual = θ_anterior + Δθ """ intervalo = self._dt siguiente_tick = time.perf_counter() + intervalo while self._activo: ahora = time.perf_counter() try: # Giroscopio (sentido del ángulo invertido) omega_x_deg_s = self._leer_gyro_x_deg_s() - self._bias_x_deg_s omega_x_rads = -math.radians(omega_x_deg_s) delta_theta = omega_x_rads * intervalo # Acelerómetro accel_z = self._leer_accel_z_ms2_crudo() - self._bias_accel_z_ms2 with self._lock: self._theta_rad += delta_theta self._omega_x_rads = omega_x_rads self._accel_z_ms2 = accel_z except Exception as e: logger.warning("IMU: Error de lectura I2C: %s", e) # Espera precisa hasta el siguiente tick siguiente_tick += intervalo pausa = siguiente_tick - time.perf_counter() if pausa > 0: time.sleep(pausa) # ── Diagnóstico ─────────────────────────────────────────────────────────── def diagnostico(self) -> dict: """Devuelve un snapshot del estado actual para logging/debug.""" with self._lock: return { "theta_deg": math.degrees(self._theta_rad), "theta_rad": self._theta_rad, "omega_x_rads": self._omega_x_rads, "bias_x_deg_s": self._bias_x_deg_s, "activo": self._activo, }