297 lines
12 KiB
Python
297 lines
12 KiB
Python
"""
|
||
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,
|
||
}
|