68 lines
2.2 KiB
Python
68 lines
2.2 KiB
Python
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("Готово!") |