project_inertial-control/imu_module/imu_reader.py

"""
IMU Reader для MPU-9250.
Улучшения относительно прежней версии:
  - реальные timestamp (time.monotonic), а не счётчик
  - блочное чтение 14 байт за один I2C-запрос
  - пачечная запись в SQLite (commit раз в 5 семплов = ~50 мс)
  - WAL-режим для параллельного чтения
  - устойчивость к временным ошибкам I2C
  - точное удержание 100 Гц с компенсацией дрейфа цикла
"""
import smbus2
import time
import sqlite3
import signal
import os

# === Конфигурация ===
MPU_ADDRESS = 0x68
I2C_BUS = 2
DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')

# Регистры MPU-9250
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B

# Масштабные коэффициенты
ACCEL_SCALE = 16384.0  # LSB/g для диапазона ±2g
GYRO_SCALE = 131.0     # LSB/(°/с) для диапазона ±250°/с

# Частота и буферизация
SAMPLE_PERIOD = 0.01   # 10 мс = 100 Гц
COMMIT_EVERY_N = 5     # коммит раз в 5 семплов (~50 мс)

running = True


def shutdown(signum, frame):
    global running
    print("Завершение IMU модуля...", flush=True)
    running = False


signal.signal(signal.SIGTERM, shutdown)
signal.signal(signal.SIGINT, shutdown)


def init_db():
    conn = sqlite3.connect(DB_PATH, timeout=10)
    conn.execute('PRAGMA journal_mode=WAL')
    conn.execute('PRAGMA busy_timeout=5000')
    conn.execute('PRAGMA synchronous=NORMAL')
    conn.execute('''CREATE TABLE IF NOT EXISTS imu_data (
        timestamp REAL,
        ax REAL, ay REAL, az REAL,
        gx REAL, gy REAL, gz REAL
    )''')
    conn.execute('DELETE FROM imu_data')
    conn.commit()
    return conn


def init_mpu(bus):
    # Снимаем sleep-бит, MPU начинает работать
    bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
    time.sleep(0.1)


def to_signed(high, low):
    v = (high << 8) | low
    return v - 65536 if v > 32767 else v


def read_imu(bus):
    # Одним запросом читаем 14 байт: ax/ay/az(6) + TEMP(2) + gx/gy/gz(6)
    data = bus.read_i2c_block_data(MPU_ADDRESS, ACCEL_XOUT_H, 14)
    ax = to_signed(data[0], data[1]) / ACCEL_SCALE
    ay = to_signed(data[2], data[3]) / ACCEL_SCALE
    az = to_signed(data[4], data[5]) / ACCEL_SCALE
    # data[6], data[7] — температура, не используем
    gx = to_signed(data[8], data[9]) / GYRO_SCALE
    gy = to_signed(data[10], data[11]) / GYRO_SCALE
    gz = to_signed(data[12], data[13]) / GYRO_SCALE
    return ax, ay, az, gx, gy, gz


def main():
    global running
    conn = init_db()
    bus = smbus2.SMBus(I2C_BUS)
    init_mpu(bus)

    print(f"IMU запущен. Целевая частота: {1/SAMPLE_PERIOD:.0f} Гц", flush=True)

    start_time = time.monotonic()
    next_tick = start_time + SAMPLE_PERIOD
    batch = []

    try:
        while running:
            # Спим до следующего тика — без дрейфа цикла
            now = time.monotonic()
            sleep_time = next_tick - now
            if sleep_time > 0:
                time.sleep(sleep_time)

            # Читаем датчик с защитой от случайных ошибок I2C
            try:
                ax, ay, az, gx, gy, gz = read_imu(bus)
            except OSError as e:
                print(f"IMU read error: {e}", flush=True)
                next_tick += SAMPLE_PERIOD
                continue

            # Реальное время от старта (монотонные секунды)
            t = time.time()

            batch.append((
                round(t, 4),
                round(ax, 4), round(ay, 4), round(az, 4),
                round(gx, 4), round(gy, 4), round(gz, 4),
            ))

            # Пачечный коммит
            if len(batch) >= COMMIT_EVERY_N:
                conn.executemany(
                    'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
                    batch
                )
                conn.commit()
                batch.clear()

            # Назначаем следующий тик
            next_tick += SAMPLE_PERIOD

            # Если сильно отстали (например, после долгой паузы) — ресинк,
            # чтобы не было «штормового» догоняния.
            if time.monotonic() - next_tick > SAMPLE_PERIOD * 5:
                next_tick = time.monotonic() + SAMPLE_PERIOD
    finally:
        # Сбросим остаток батча
        if batch:
            conn.executemany(
                'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
                batch
            )
            conn.commit()
        bus.close()
        conn.close()
        print("IMU модуль остановлен", flush=True)


if __name__ == '__main__':
    main()