import serial
import struct
import math

# --- impostazioni ---
G = 9.80665
ACC_LSB = 2048.0
GYRO_LSB = 16.4

def acc_raw_to_ms2(raw):
    return (raw / ACC_LSB) * G

def gyro_raw_to_rad(raw):
    return (raw / GYRO_LSB) * (math.pi / 180.0)

# apri seriale
ser = serial.Serial('COM8', 2000000)

frame_format = '<HIhhhhhhHH'  # magic, ts, imu1(ax..gz), imu2(ax..gz), crc
frame_size = struct.calcsize(frame_format)

while True:
    buf = ser.read(frame_size)
    if len(buf) != frame_size:
        continue

    unpacked = struct.unpack(frame_format, buf)
    magic, ts = unpacked[0], unpacked[1]
    imu1_raw = unpacked[2:8]
    imu2_raw = unpacked[8:14]

    # conversione accelerometro e giroscopio
    imu1 = [round(acc_raw_to_ms2(imu1_raw[0]), 2),
            round(acc_raw_to_ms2(imu1_raw[1]), 2),
            round(acc_raw_to_ms2(imu1_raw[2]), 2),
            round(gyro_raw_to_rad(imu1_raw[3]), 2),
            round(gyro_raw_to_rad(imu1_raw[4]), 2),
            round(gyro_raw_to_rad(imu1_raw[5]), 2)]

    imu2 = [round(acc_raw_to_ms2(imu2_raw[0]), 2),
            round(acc_raw_to_ms2(imu2_raw[1]), 2),
            round(acc_raw_to_ms2(imu2_raw[2]), 2),
            round(gyro_raw_to_rad(imu2_raw[3]), 2),
            round(gyro_raw_to_rad(imu2_raw[4]), 2),
            round(gyro_raw_to_rad(imu2_raw[5]), 2)]

    print(f"t={ts} imu1={imu1} imu2={imu2}")