#!/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)
|
