82 lines
2.2 KiB
Python
82 lines
2.2 KiB
Python
import serial
|
|
import struct
|
|
import csv
|
|
from datetime import datetime, timedelta
|
|
|
|
# ================= CONFIG =================
|
|
PORT = "COM3" # <-- cambia
|
|
BAUD = 921600
|
|
OUT_CSV = "imu_log.csv"
|
|
|
|
FRAME_FMT = "<H I 6h 6h H" # little-endian
|
|
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...")
|
|
|
|
try:
|
|
with open(OUT_CSV, "w", newline="") as f:
|
|
writer = csv.writer(f)
|
|
writer.writerow([
|
|
"timestamp",
|
|
"imu1_ax","imu1_ay","imu1_az","imu1_gx","imu1_gy","imu1_gz",
|
|
"imu2_ax","imu2_ay","imu2_az","imu2_gx","imu2_gy","imu2_gz"
|
|
])
|
|
|
|
while True:
|
|
buffer += ser.read(1024)
|
|
|
|
while len(buffer) >= FRAME_SIZE:
|
|
# cerca MAGIC
|
|
if buffer[0] != 0xCD or buffer[1] != 0xAB:
|
|
buffer.pop(0)
|
|
continue
|
|
|
|
frame = buffer[:FRAME_SIZE]
|
|
buffer = buffer[FRAME_SIZE:]
|
|
|
|
# unpack
|
|
magic, ts_us, *imu, crc = struct.unpack(FRAME_FMT, frame)
|
|
|
|
# CRC check
|
|
if crc16_xor(frame[:-2]) != crc:
|
|
continue
|
|
|
|
# inizializza tempo base
|
|
if start_wall_time is None:
|
|
start_wall_time = datetime.now()
|
|
start_ts_us = ts_us
|
|
|
|
# timestamp assoluto coerente con ESP
|
|
delta_us = ts_us - start_ts_us
|
|
ts = start_wall_time + timedelta(microseconds=delta_us)
|
|
|
|
writer.writerow([
|
|
ts.strftime("%Y-%m-%d %H:%M:%S.%f")[:-3],
|
|
*imu
|
|
])
|
|
|
|
# logging si può escludere
|
|
print(
|
|
ts.strftime("%H:%M:%S.%f")[:-3],
|
|
"| 1:", tuple(imu[:6]),
|
|
"| 2:", tuple(imu[6:])
|
|
)
|
|
except KeyboardInterrupt:
|
|
ser.close()
|
|
print("Stopped") |