#!/usr/bin/env python3
|
# -*- coding: utf-8 -*-
|
"""
|
测试GPS/IMU数据解析 - 使用实际数据验证
|
"""
|
|
import struct
|
|
# 从你提供的数据中提取的完整GPS帧
|
gps_frame_hex = """
|
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
|
"""
|
|
# 完整IMU帧
|
imu_frame_hex = """
|
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 test_gps_parsing():
|
"""测试GPS数据解析"""
|
print("="*60)
|
print("测试GPS数据包解析")
|
print("="*60)
|
|
# 转换为字节
|
hex_clean = gps_frame_hex.replace(' ', '').replace('\n', '')
|
frame = bytes.fromhex(hex_clean)
|
|
print(f"完整帧长度: {len(frame)} 字节")
|
print(f"帧头: {frame[0]:02X} {frame[1]:02X}")
|
print(f"类型: {frame[2]:02X}")
|
|
data_len = struct.unpack('<H', frame[3:5])[0]
|
print(f"数据长度: {data_len} 字节")
|
|
payload = frame[5:5+data_len]
|
print(f"负载长度: {len(payload)} 字节")
|
|
checksum = struct.unpack('<H', frame[5+data_len:5+data_len+2])[0]
|
calc_checksum = sum(frame[0:5+data_len]) & 0xFFFF
|
print(f"校验和: 0x{checksum:04X} (计算: 0x{calc_checksum:04X}) {'✓' if checksum==calc_checksum else '✗'}")
|
|
# 尝试不同的解析格式
|
print("\n尝试解析 (格式: <dd5fIBB2x):")
|
try:
|
gps = struct.unpack('<dd5fIBB2x', payload)
|
print(f" [0] 纬度: {gps[0]:.8f}°")
|
print(f" [1] 经度: {gps[1]:.8f}°")
|
print(f" [2] 航向角: {gps[2]:.2f}°")
|
print(f" [3] 东速度: {gps[3]:.3f} m/s")
|
print(f" [4] 北速度: {gps[4]:.3f} m/s")
|
print(f" [5] 天速度: {gps[5]:.3f} m/s")
|
print(f" [6] 高程: {gps[6]:.2f} m")
|
print(f" [7] UTC时间: {gps[7]}")
|
print(f" [8] 定位质量: {gps[8]}")
|
print(f" [9] 卫星数: {gps[9]}")
|
print("✓ 解析成功!")
|
except Exception as e:
|
print(f"✗ 解析失败: {e}")
|
|
# 十六进制显示负载
|
print(f"\n负载十六进制 ({len(payload)}字节):")
|
hex_str = payload.hex(' ').upper()
|
for i in range(0, len(hex_str), 48):
|
print(f" {hex_str[i:i+48]}")
|
|
|
def test_imu_parsing():
|
"""测试IMU数据解析"""
|
print("\n" + "="*60)
|
print("测试IMU数据包解析")
|
print("="*60)
|
|
# 转换为字节
|
hex_clean = imu_frame_hex.replace(' ', '').replace('\n', '')
|
frame = bytes.fromhex(hex_clean)
|
|
print(f"完整帧长度: {len(frame)} 字节")
|
print(f"帧头: {frame[0]:02X} {frame[1]:02X}")
|
print(f"类型: {frame[2]:02X}")
|
|
data_len = struct.unpack('<H', frame[3:5])[0]
|
print(f"数据长度: {data_len} 字节")
|
|
payload = frame[5:5+data_len]
|
print(f"负载长度: {len(payload)} 字节")
|
|
checksum = struct.unpack('<H', frame[5+data_len:5+data_len+2])[0]
|
calc_checksum = sum(frame[0:5+data_len]) & 0xFFFF
|
print(f"校验和: 0x{checksum:04X} (计算: 0x{calc_checksum:04X}) {'✓' if checksum==calc_checksum else '✗'}")
|
|
# 解析IMU数据
|
print("\n尝试解析 (格式: <7fI):")
|
try:
|
imu = struct.unpack('<7fI', payload)
|
print(f" [0] 加速度X: {imu[0]:.3f} g")
|
print(f" [1] 加速度Y: {imu[1]:.3f} g")
|
print(f" [2] 加速度Z: {imu[2]:.3f} g")
|
print(f" [3] 角速度X: {imu[3]:.2f} °/s")
|
print(f" [4] 角速度Y: {imu[4]:.2f} °/s")
|
print(f" [5] 角速度Z: {imu[5]:.2f} °/s")
|
print(f" [6] 温度: {imu[6]:.1f} ℃")
|
print(f" [7] UTC时间: {imu[7]} ({imu[7]/1000:.3f}s)")
|
print("✓ 解析成功!")
|
except Exception as e:
|
print(f"✗ 解析失败: {e}")
|
|
# 十六进制显示负载
|
print(f"\n负载十六进制 ({len(payload)}字节):")
|
hex_str = payload.hex(' ').upper()
|
for i in range(0, len(hex_str), 48):
|
print(f" {hex_str[i:i+48]}")
|
|
|
if __name__ == "__main__":
|
test_gps_parsing()
|
test_imu_parsing()
|
|
print("\n" + "="*60)
|
print("结论:")
|
print("="*60)
|
print("✓ GPS数据包: 44字节，格式 <dd5fIBB2x")
|
print("✓ IMU数据包: 32字节，格式 <7fI")
|
print("✓ 校验和: 16位累加和")
|
print("✓ 帧格式: AA 55 TYPE LEN(2) PAYLOAD CHECKSUM(2) 0D 0A")
|
