project_inertial-control/imu_module/imu_reader.py

import smbus2
import time
import sqlite3
import signal
import sys

# Константы
MPU_ADDRESS = 0x68
I2C_BUS = 2
DB_PATH = '../inertial_data.db'

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

# Масштабные коэффициенты
ACCEL_SCALE = 16384.0
GYRO_SCALE = 131.0

# Глобальные переменные для завершения
running = True
conn = None
bus = None

def shutdown(signum, frame):
    """Обработчик сигнала завершения"""
    global running
    print("Завершение IMU модуля...")
    running = False

# Перехватываем сигналы завершения
signal.signal(signal.SIGTERM, shutdown)
signal.signal(signal.SIGINT, shutdown)

def init_db():
    conn = sqlite3.connect(DB_PATH)
    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):
    bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
    time.sleep(0.1)

def read_raw(bus, reg):
    high = bus.read_byte_data(MPU_ADDRESS, reg)
    low = bus.read_byte_data(MPU_ADDRESS, reg + 1)
    value = (high << 8) | low
    if value > 32767:
        value -= 65536
    return value

def read_imu(bus):
    ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE
    ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE
    az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE
    gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE
    gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE
    gz = read_raw(bus, GYRO_XOUT_H + 4) / GYRO_SCALE
    return ax, ay, az, gx, gy, gz

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

    print("Запись данных IMU...")
    t = 0.0
    try:
        while running:
            ax, ay, az, gx, gy, gz = read_imu(bus)
            conn.execute(
                'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
                (round(t, 3), round(ax, 4), round(ay, 4), round(az, 4),
                 round(gx, 4), round(gy, 4), round(gz, 4))
            )
            conn.commit()
            t += 0.01
            time.sleep(0.01)
    finally:
        bus.close()
        conn.close()
        print("IMU модуль остановлен")

save_to_db()