import serial
import time

def checksum(data):
    return sum(data[:-1]) & 0xFF

def parse_packet(data):
    if len(data) != 11:
        return None
    if data[0] != 0x55:
        return None
    if checksum(data) != data[-1]:
        print("校验失败")
        return None

    packet_type = data[1]
    # 将后8字节分成4个16位有符号整数（小端序）
    vals = [int.from_bytes(data[i:i+2], 'little', signed=True) for i in range(2, 10, 2)]

    if packet_type == 0x51:  # 加速度，单位0.001g
        ax, ay, az, temp = vals
        ax /= 1000
        ay /= 1000
        az /= 1000
        temp = temp / 100  # 温度单位摄氏度
        # return f"加速度 (g): x={ax:.3f}, y={ay:.3f}, z={az:.3f}, 温度={temp:.2f}℃"
    elif packet_type == 0x52:  # 角速度，单位0.01°/s
        wx, wy, wz, temp = vals
        wx /= 100
        wy /= 100
        wz /= 100
        temp = temp / 100
        # return f"角速度 (°/s): x={wx:.2f}, y={wy:.2f}, z={wz:.2f}, 温度={temp:.2f}℃"
    elif packet_type == 0x53:  # 姿态角，单位0.01°
        roll, pitch, yaw, temp = vals
        roll /= 100
        pitch /= 100
        yaw /= 100
        temp = temp / 100
        return f"姿态角 (°): roll={roll:.2f}, pitch={pitch:.2f}, yaw={yaw:.2f}, 温度={temp:.2f}℃"
    elif packet_type == 0x54:  # 磁力计，单位uT
        mx, my, mz, temp = vals
        temp = temp / 100
        # return f"磁力计 (uT): x={mx}, y={my}, z={mz}, 温度={temp:.2f}℃"
    elif packet_type == 0x56:  # 气压，单位Pa
        p1, p2, p3, temp = vals
        pressure = ((p1 & 0xFFFF) | ((p2 & 0xFFFF) << 16)) / 100  # 简单合成，大部分IMU气压是3字节，这里简单示范
        temp = temp / 100
        # return f"气压 (Pa): pressure={pressure:.2f}, 温度={temp:.2f}℃"
    else:
        return f"未知包类型: {packet_type}"

def read_imu(port='COM6', baudrate=9600):
    ser = serial.Serial(port, baudrate, timeout=1)
    buffer = bytearray()

    try:
        while True:
            bytes_waiting = ser.in_waiting
            if bytes_waiting:
                data = ser.read(bytes_waiting)
                buffer.extend(data)

                # 解析buffer中所有完整包
                while len(buffer) >= 11:
                    if buffer[0] != 0x55:
                        buffer.pop(0)
                        continue
                    packet = buffer[:11]
                    result = parse_packet(packet)
                    if result:
                        print(result)
                    buffer = buffer[11:]
            time.sleep(0.01)
    except KeyboardInterrupt:
        print("程序终止")
    finally:
        ser.close()

if __name__ == "__main__":
    read_imu(port='COM8', baudrate=9600)