#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
分析STM32输出的原始十六进制数据
"""

import struct

# 原始数据
raw_hex = """
41 E7 00 2F 06 AF 0E 0D 0A AA 55 01 2C 00 68 70 21 B0 6E EA 43 40 E9 11 40 BD DB 11 5D 40 
7F C3 65 43 F4 FD 54 BC 58 39 34 BC A6 9B 44 BB 77 3E A5 42 EC 4C 9E 00 01 1B 00 00 6A 14 
0D 0A AA 55 02 20 00 58 39 34 BC BA 49 8C BE 4E 62 80 BF 7F 6A BC 3D D1 22 5B BE 00 00 00 
80 00 00 F0 41 F1 00 2F 06 A3 0D 0D 0A
"""

def parse_frames(hex_string):
    """解析帧数据"""
    # 移除空格和换行，转换为字节
    hex_clean = hex_string.replace(' ', '').replace('\n', '')
    data = bytes.fromhex(hex_clean)
    
    print(f"总数据长度: {len(data)} 字节\n")
    
    i = 0
    frame_count = 0
    
    while i < len(data) - 2:
        # 查找帧头 AA 55
        if data[i] == 0xAA and data[i+1] == 0x55:
            frame_count += 1
            print(f"{'='*60}")
            print(f"帧 #{frame_count} - 位置: {i} (0x{i:04X})")
            print(f"{'='*60}")
            
            if i + 5 > len(data):
                print("数据不足，无法解析帧头")
                break
            
            # 解析帧头
            frame_type = data[i+2]
            data_len = struct.unpack('<H', data[i+3:i+5])[0]
            
            print(f"帧头: AA 55")
            print(f"类型: 0x{frame_type:02X}", end='')
            if frame_type == 0x01:
                print(" (GPS数据)")
            elif frame_type == 0x02:
                print(" (IMU数据)")
            elif frame_type == 0x10:
                print(" (控制命令)")
            else:
                print(" (未知类型)")
            
            print(f"数据长度: {data_len} 字节")
            
            # 计算完整帧长度
            total_frame_len = 5 + data_len + 4  # 帧头(5) + 数据 + 校验和(2) + 帧尾(2)
            
            if i + total_frame_len > len(data):
                print(f"错误: 帧不完整，需要{total_frame_len}字节，剩余{len(data)-i}字节")
                print(f"原始数据: {data[i:min(i+20, len(data))].hex(' ').upper()}")
                i += 1
                continue
            
            # 提取完整帧
            frame = data[i:i+total_frame_len]
            payload = frame[5:5+data_len]
            checksum = struct.unpack('<H', frame[5+data_len:5+data_len+2])[0]
            footer = frame[5+data_len+2:5+data_len+4]
            
            # 计算校验和
            calc_checksum = sum(frame[0:5+data_len]) & 0xFFFF
            
            print(f"数据负载: {data_len} 字节")
            print(f"校验和: 0x{checksum:04X} (计算值: 0x{calc_checksum:04X})", end='')
            if checksum == calc_checksum:
                print(" ✓")
            else:
                print(" ✗ 校验失败!")
            
            print(f"帧尾: {footer.hex(' ').upper()}", end='')
            if footer == b'\x0D\x0A':
                print(" ✓")
            else:
                print(" ✗ 帧尾错误!")
            
            # 解析GPS数据
            if frame_type == 0x01 and data_len == 56:
                try:
                    gps = struct.unpack('<ddfffffff I BB 2x', payload)
                    print(f"\nGPS数据解析:")
                    print(f"  纬度:     {gps[0]:.8f}°")
                    print(f"  经度:     {gps[1]:.8f}°")
                    print(f"  航向角:   {gps[2]:.2f}°")
                    print(f"  东速度:   {gps[3]:.3f} m/s")
                    print(f"  北速度:   {gps[4]:.3f} m/s")
                    print(f"  天速度:   {gps[5]:.3f} m/s")
                    print(f"  高程:     {gps[6]:.2f} m")
                    print(f"  UTC时间:  {gps[7]}")
                    print(f"  定位质量: {gps[8]}")
                    print(f"  卫星数:   {gps[9]}")
                except Exception as e:
                    print(f"GPS解析错误: {e}")
            
            # 解析IMU数据
            elif frame_type == 0x02 and data_len == 32:
                try:
                    imu = struct.unpack('<7f I', payload)
                    print(f"\nIMU数据解析:")
                    print(f"  加速度X:  {imu[0]:.3f} g")
                    print(f"  加速度Y:  {imu[1]:.3f} g")
                    print(f"  加速度Z:  {imu[2]:.3f} g")
                    print(f"  角速度X:  {imu[3]:.2f} °/s")
                    print(f"  角速度Y:  {imu[4]:.2f} °/s")
                    print(f"  角速度Z:  {imu[5]:.2f} °/s")
                    print(f"  温度:     {imu[6]:.1f} ℃")
                    print(f"  UTC时间:  {imu[7]}")
                except Exception as e:
                    print(f"IMU解析错误: {e}")
            
            # 显示原始十六进制
            print(f"\n完整帧 ({total_frame_len}字节):")
            hex_str = frame.hex(' ').upper()
            for j in range(0, len(hex_str), 48):
                print(f"  {hex_str[j:j+48]}")
            
            print()
            
            # 跳到下一帧
            i += total_frame_len
        else:
            # 不是帧头，继续搜索
            i += 1
    
    print(f"共找到 {frame_count} 个完整帧")


if __name__ == "__main__":
    parse_frames(raw_hex)