154 lines
5.0 KiB
Python
154 lines
5.0 KiB
Python
"""
|
||
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()
|