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("Готово!")
|