import smbus2 #библиотека Python для работы с I2C шиной
import time
import csv
import struct

# Константы
MPU_ADDRESS = 0x68
I2C_BUS = 1

# Регистры MPU-9250
PWR_MGMT_1 = 0x6B      # управление питанием
ACCEL_XOUT_H = 0x3B    # начало данных акселерометра
GYRO_XOUT_H = 0x43     # начало данных гироскопа

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

def init_mpu(bus):
    """Разбудить датчик"""
    bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
    time.sleep(0.1)

def read_raw(bus, reg):
    """Читать 16-битное значение из двух регистров"""
    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_csv(filename, duration_seconds):
    """Записать данные в CSV файл"""
    bus = smbus2.SMBus(I2C_BUS)
    init_mpu(bus)

    with open(filename, 'w', newline='') as f:
        writer = csv.writer(f)
        writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz'])

        t = 0.0
        while t < duration_seconds:
            ax, ay, az, gx, gy, gz = read_imu(bus)
            writer.writerow([round(t, 3), 
                           round(ax, 4), round(ay, 4), round(az, 4),
                           round(gx, 4), round(gy, 4), round(gz, 4)])
            t += 0.01
            time.sleep(0.01)

    bus.close()

# Запуск — записываем 5 секунд
save_to_csv('imu.csv', 5)
print("Готово!")