92 lines
2.7 KiB
Python
92 lines
2.7 KiB
Python
import serial
|
|
import struct
|
|
from datetime import datetime, timedelta
|
|
|
|
# ================= CONFIG =================
|
|
PORT = "COM17" # <-- cambia
|
|
BAUD = 2000000
|
|
OUT_FILE = "imu_log.txt"
|
|
|
|
# Formato frame ESP32 (raw)
|
|
FRAME_FMT = "<H I 6h 6h H" # magic, ts_us, imu1 6x int16, imu2 6x int16, crc
|
|
FRAME_SIZE = struct.calcsize(FRAME_FMT)
|
|
MAGIC_BYTES = b'\xCD\xAB'
|
|
|
|
# Conversione accelerometro e giroscopio
|
|
ACC_LSB_PER_G = 2048.0
|
|
G_CONST = 9.80665
|
|
GYRO_LSB_PER_DPS = 16.4
|
|
DEG2RAD = 0.017453292519943295
|
|
|
|
# ================= CRC ====================
|
|
def crc16_xor(data: bytes) -> int:
|
|
c = 0
|
|
for b in data:
|
|
c ^= b
|
|
return c & 0xFFFF
|
|
|
|
# ================= SERIAL =================
|
|
ser = serial.Serial(PORT, BAUD, timeout=1)
|
|
buffer = bytearray()
|
|
|
|
start_wall_time = None
|
|
start_ts_us = None
|
|
|
|
print("Listening...")
|
|
|
|
def convert_acc(raw):
|
|
return round(((raw / ACC_LSB_PER_G) * G_CONST)/10, 2)
|
|
|
|
def convert_gyro(raw):
|
|
return round((raw / GYRO_LSB_PER_DPS) * DEG2RAD, 2)
|
|
|
|
with open(OUT_FILE, "w", encoding="utf-8") as f:
|
|
while True:
|
|
buffer += ser.read(1024)
|
|
|
|
while True:
|
|
idx = buffer.find(MAGIC_BYTES)
|
|
if idx == -1:
|
|
buffer.clear()
|
|
break
|
|
if idx > 0:
|
|
buffer = buffer[idx:]
|
|
if len(buffer) < FRAME_SIZE:
|
|
break
|
|
|
|
frame = buffer[:FRAME_SIZE]
|
|
buffer = buffer[FRAME_SIZE:]
|
|
|
|
magic, ts_us, *imu_raw, crc = struct.unpack(FRAME_FMT, frame)
|
|
|
|
if crc16_xor(frame[:-2]) != crc:
|
|
print("CRC error (resync)")
|
|
continue
|
|
|
|
# Base timestamp
|
|
if start_wall_time is None:
|
|
start_wall_time = datetime.now()
|
|
start_ts_us = ts_us
|
|
|
|
delta_us = ts_us - start_ts_us
|
|
ts = start_wall_time + timedelta(microseconds=delta_us)
|
|
|
|
# Split raw dati
|
|
imu1_raw = imu_raw[:6]
|
|
imu2_raw = imu_raw[6:]
|
|
|
|
# Conversione in m/s² e rad/s
|
|
imu1 = [convert_acc(imu1_raw[0]), convert_acc(imu1_raw[1]), convert_acc(imu1_raw[2]),
|
|
convert_gyro(imu1_raw[3]), convert_gyro(imu1_raw[4]), convert_gyro(imu1_raw[5])]
|
|
imu2 = [convert_acc(imu2_raw[0]), convert_acc(imu2_raw[1]), convert_acc(imu2_raw[2]),
|
|
convert_gyro(imu2_raw[3]), convert_gyro(imu2_raw[4]), convert_gyro(imu2_raw[5])]
|
|
|
|
line = (
|
|
f"{ts.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]}"
|
|
f"|1,{','.join(f'{v:.2f}' for v in imu1)}"
|
|
f"|2,{','.join(f'{v:.2f}' for v in imu2)}\n"
|
|
)
|
|
|
|
f.write(line)
|
|
f.flush()
|
|
print(line.strip()) |