73 lines
1.6 KiB
Python
73 lines
1.6 KiB
Python
import serial
|
|
import struct
|
|
import datetime
|
|
import csv
|
|
|
|
PORT = 'COM3'
|
|
BAUD = 921600
|
|
|
|
MAGIC = 0xABCD
|
|
FRAME_FMT = '<HI6h6hH'
|
|
FRAME_SIZE = struct.calcsize(FRAME_FMT)
|
|
|
|
def crc16(data: bytes) -> int:
|
|
c = 0
|
|
for b in data:
|
|
c ^= b
|
|
return c
|
|
|
|
ser = serial.Serial(PORT, BAUD, timeout=1)
|
|
buffer = bytearray()
|
|
|
|
start_ts = None
|
|
start_time = None
|
|
|
|
csv_file = open('imu_log.csv', 'w', newline='')
|
|
writer = csv.writer(csv_file)
|
|
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'
|
|
])
|
|
|
|
print("Listening...")
|
|
|
|
try:
|
|
while True:
|
|
buffer += ser.read(512)
|
|
|
|
while len(buffer) >= FRAME_SIZE:
|
|
if buffer[0:2] != MAGIC.to_bytes(2, 'little'):
|
|
buffer.pop(0)
|
|
continue
|
|
|
|
frame = buffer[:FRAME_SIZE]
|
|
buffer = buffer[FRAME_SIZE:]
|
|
|
|
unpacked = struct.unpack(FRAME_FMT, frame)
|
|
crc_rx = unpacked[-1]
|
|
crc_calc = crc16(frame[:-2])
|
|
|
|
if crc_rx != crc_calc:
|
|
continue # frame corrotto
|
|
|
|
ts_us = unpacked[1]
|
|
|
|
if start_ts is None:
|
|
start_ts = ts_us
|
|
start_time = datetime.datetime.now()
|
|
|
|
abs_time = start_time + datetime.timedelta(
|
|
microseconds=(ts_us - start_ts)
|
|
)
|
|
|
|
writer.writerow([
|
|
abs_time.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3],
|
|
*unpacked[2:14]
|
|
])
|
|
|
|
except KeyboardInterrupt:
|
|
csv_file.close()
|
|
ser.close()
|
|
print("Stopped")
|