70 lines
1.7 KiB
Python
70 lines
1.7 KiB
Python
import serial
|
|
import struct
|
|
from datetime import datetime, timedelta
|
|
|
|
# ================= CONFIG =================
|
|
PORT = "COM17" # <-- cambia
|
|
BAUD = 2000000
|
|
OUT_FILE = "imu_log.txt"
|
|
|
|
FRAME_FMT = "<H I 6f 6f H"
|
|
# FRAME_FMT = "<H I 6h 6h H"
|
|
FRAME_SIZE = struct.calcsize(FRAME_FMT)
|
|
MAGIC = 0xABCD
|
|
|
|
# ================= 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...")
|
|
|
|
with open(OUT_FILE, "w", encoding="utf-8") as f:
|
|
while True:
|
|
buffer += ser.read(1024)
|
|
|
|
while len(buffer) >= FRAME_SIZE:
|
|
# ricerca MAGIC (0xABCD little-endian)
|
|
if buffer[0] != 0xCD or buffer[1] != 0xAB:
|
|
buffer.pop(0)
|
|
continue
|
|
|
|
frame = buffer[:FRAME_SIZE]
|
|
buffer = buffer[FRAME_SIZE:]
|
|
|
|
magic, ts_us, *imu, crc = struct.unpack(FRAME_FMT, frame)
|
|
|
|
if crc16_xor(frame[:-2]) != crc:
|
|
continue
|
|
|
|
# tempo base
|
|
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)
|
|
|
|
imu1 = imu[:6]
|
|
imu2 = imu[6:]
|
|
|
|
line = (
|
|
f"{ts.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]}"
|
|
f"|1:{','.join(map(str, imu1))}"
|
|
f"|2:{','.join(map(str, imu2))}\n"
|
|
)
|
|
|
|
f.write(line)
|
|
f.flush() # rimuovi se vuoi più performance
|
|
|
|
print(line.strip())
|