228 lines
11 KiB
Python
228 lines
11 KiB
Python
from typing import Optional, Tuple
|
||
import logging
|
||
from frame import FrameFormat
|
||
|
||
class FrameFinder:
|
||
"""帧查找器 - 在数据缓冲区中查找符合帧格式的数据"""
|
||
|
||
def __init__(self, num_channels: int = 17):
|
||
"""
|
||
初始化帧查找器
|
||
Args:
|
||
num_channels: 通道数量，默认为8
|
||
"""
|
||
self.frame_format = FrameFormat(num_channels)
|
||
self.frame_length = self.frame_format.FIXED_SIZE + self.frame_format.DATA_SIZE
|
||
self.frame_header = self.frame_format.FRAME_HEADER
|
||
self.logger = logging.getLogger(__name__)
|
||
# 设置日志级别为INFO，只有在需要调试时才改为DEBUG
|
||
self.logger.setLevel(logging.INFO)
|
||
|
||
def find_frame_in_buffer(self, buffer: bytes, start_pos: int = 0) -> Tuple[Optional[bytes], int]:
|
||
"""
|
||
在给定的缓冲区数据中查找下一个有效帧
|
||
Args:
|
||
buffer: 要搜索的缓冲区数据
|
||
start_pos: 开始搜索的位置
|
||
Returns:
|
||
Tuple[Optional[bytes], int]:
|
||
- 找到的帧数据（如果没找到则为None）
|
||
- 下一次搜索的起始位置（如果找到帧，则是帧后的位置；如果没找到，则是需要保留的数据起始位置）
|
||
"""
|
||
buffer_size = len(buffer)
|
||
current_pos = start_pos
|
||
|
||
while current_pos < buffer_size:
|
||
# 检查当前位置是否有足够的数据
|
||
if buffer_size - current_pos < len(self.frame_header):
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"剩余数据不足帧头长度: {buffer_size - current_pos} < {len(self.frame_header)}")
|
||
return None, current_pos
|
||
|
||
# 在当前位置查找帧头
|
||
header_pos = self._find_header(buffer, current_pos)
|
||
if header_pos is None:
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"未找到帧头，将移除所有数据 ({buffer_size - current_pos} 字节)")
|
||
if buffer_size - current_pos > 0:
|
||
self.logger.debug(f"移除的数据: {buffer[current_pos:].hex()}")
|
||
return None, buffer_size
|
||
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"找到帧头位置: {header_pos}")
|
||
if header_pos > current_pos:
|
||
removed_data = buffer[current_pos:header_pos]
|
||
self.logger.debug(f"将移除帧头前的无效数据: {removed_data.hex() if len(removed_data) > 0 else '无'} ({len(removed_data)} 字节)")
|
||
|
||
# 检查是否有完整帧的数据
|
||
if buffer_size - header_pos < self.frame_length:
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"帧头后数据不足: {buffer_size - header_pos} < {self.frame_length}")
|
||
self.logger.debug(f"保留从位置 {header_pos} 开始的数据: {buffer[header_pos:].hex()}")
|
||
return None, header_pos
|
||
|
||
# 提取并验证帧
|
||
frame_data = buffer[header_pos:header_pos + self.frame_length]
|
||
if self._verify_frame(frame_data):
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"找到有效帧: {frame_data.hex()}")
|
||
next_pos = header_pos + self.frame_length
|
||
remaining_data = buffer[next_pos:] if next_pos < buffer_size else b''
|
||
self.logger.debug(f"处理完帧后剩余数据: {remaining_data.hex() if remaining_data else '无'} ({len(remaining_data)} 字节)")
|
||
return frame_data, header_pos + self.frame_length
|
||
|
||
# 帧验证失败，从下一个位置继续查找
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"帧验证失败，从下一个位置继续查找")
|
||
|
||
# 防止在同一位置反复查找
|
||
current_pos = header_pos + 1 # 从帧头后的下一个字节开始查找
|
||
if current_pos >= buffer_size:
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"搜索位置已到达缓冲区末尾，将移除所有数据")
|
||
return None, buffer_size
|
||
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
removed_data = buffer[header_pos:current_pos]
|
||
self.logger.debug(f"移除无效帧头数据: {removed_data.hex()} ({len(removed_data)} 字节)")
|
||
|
||
# 如果遍历完整个缓冲区都没有找到有效帧
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"遍历完整个缓冲区未找到有效帧")
|
||
return None, buffer_size
|
||
|
||
def _find_header(self, buffer: bytes, start_pos: int) -> Optional[int]:
|
||
"""
|
||
在缓冲区中查找帧头
|
||
Args:
|
||
buffer: 要搜索的缓冲区数据
|
||
start_pos: 开始搜索的位置
|
||
Returns:
|
||
Optional[int]: 帧头位置，如果未找到则返回None
|
||
"""
|
||
header_len = len(self.frame_header)
|
||
buffer_size = len(buffer)
|
||
|
||
# 确保有足够的数据来查找帧头
|
||
if buffer_size - start_pos < header_len:
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"剩余数据不足帧头长度: {buffer_size - start_pos} < {header_len}")
|
||
return None
|
||
|
||
# 从起始位置开始查找帧头
|
||
for i in range(start_pos, buffer_size - header_len + 1):
|
||
if buffer[i:i+header_len] == self.frame_header:
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"在位置 {i} 找到帧头: {buffer[i:i+header_len].hex()}")
|
||
# 显示帧头前后的数据
|
||
before = buffer[max(0, i-4):i]
|
||
after = buffer[i+header_len:i+header_len+4]
|
||
self.logger.debug(f"帧头前4字节: {before.hex() if before else '无'}")
|
||
self.logger.debug(f"帧头后4字节: {after.hex() if after else '无'}")
|
||
return i
|
||
|
||
if self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"从位置 {start_pos} 开始未找到帧头")
|
||
self.logger.debug(f"剩余数据: {buffer[start_pos:].hex()[:100]}...")
|
||
self.logger.debug(f"剩余数据长度: {buffer_size - start_pos}")
|
||
return None
|
||
|
||
def _verify_frame(self, frame_data: bytes) -> bool:
|
||
"""
|
||
验证帧的有效性
|
||
Args:
|
||
frame_data: 要验证的帧数据
|
||
Returns:
|
||
bool: 帧是否有效
|
||
"""
|
||
# 使用FrameFormat验证帧
|
||
frame = FrameFormat.parse_frame(frame_data, self.frame_format.CHANNEL_COUNT)
|
||
if frame is None and self.logger.isEnabledFor(logging.DEBUG):
|
||
self.logger.debug(f"帧验证失败: {frame_data.hex()}")
|
||
return frame is not None
|
||
|
||
if __name__ == "__main__":
|
||
# 使用示例
|
||
import logging
|
||
|
||
# 配置日志
|
||
logging.basicConfig(
|
||
level=logging.DEBUG,
|
||
format='%(asctime)s - %(levelname)s - %(message)s'
|
||
)
|
||
|
||
# 创建帧查找器实例
|
||
finder = FrameFinder(num_channels=24)
|
||
|
||
# with open("test_data/test_frames.bin", "rb") as f:
|
||
# test_data = f.read()
|
||
# test_data_hex = "AA AA AA AA 02 00 00 00 49 00 00 00 B1 87 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 2B AA AA AA AA 02 00 00 00 49 00 00 00 B2 87 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 2C AA AA AA AA 02 00 00 00 49 00 00 00 B3 87 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 2D AA AA AA AA 02 00 00 00 49 00 00 00 B4 87 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 2E"
|
||
test_data_hex = '''AA AA AA AA 02 00 00 00 65 00
|
||
00 00 00 00 00 00 E1 FE FF FF
|
||
20 01 00 00 E1 FB FF FF 00 00
|
||
00 00 A0 FE FF FF A1 FD FF FF
|
||
C0 02 00 00 C1 FF FF FF 41 FF
|
||
FF FF 61 FF FF FF 60 FF FF FF
|
||
60 FF FF FF 00 00 00 00 00 00
|
||
00 00 00 00 00 00 00 00 00 00
|
||
00 00 00 00 00 00 00 00 00 00
|
||
00 00 00 00 00 00 00 00 00 00
|
||
00 00 00 00 00 00 00 00 00 00
|
||
00 00 F5'''
|
||
test_data = bytes.fromhex(test_data_hex)
|
||
print("初始测试数据:")
|
||
print("总长度:", len(test_data))
|
||
print("数据:", test_data.hex())
|
||
print("\n开始查找帧:")
|
||
|
||
# 模拟实际使用场景，从不同位置开始查找
|
||
remaining_data = test_data
|
||
frame_count = 0
|
||
|
||
while len(remaining_data) > 0:
|
||
frame, next_pos = finder.find_frame_in_buffer(remaining_data)
|
||
if frame:
|
||
frame_count += 1
|
||
print(f"\n找到第 {frame_count} 个帧:")
|
||
print(f"帧数据: {frame.hex()}")
|
||
print(f"下一个搜索位置: {next_pos}")
|
||
print(frame)
|
||
|
||
# 解析帧内容
|
||
parsed_frame = FrameFormat.parse_frame(frame, finder.frame_format.CHANNEL_COUNT)
|
||
if parsed_frame:
|
||
print(f"命令码: 0x{parsed_frame.command:08X}")
|
||
print(f"长度: {parsed_frame.length}")
|
||
print(f"帧IDX: {parsed_frame.frame_idx}")
|
||
print("通道数据:")
|
||
for i, value in enumerate(parsed_frame.channels):
|
||
print(f"通道 {i+1}: 0x{value:08X}")
|
||
print(f"校验和: 0x{parsed_frame.checksum:02X}")
|
||
|
||
# 更新剩余数据
|
||
remaining_data = remaining_data[next_pos:]
|
||
print(f"\n剩余数据: ({len(remaining_data)} 字节)")
|
||
if remaining_data:
|
||
# print(f"数据: {remaining_data.hex()}")
|
||
pass
|
||
else:
|
||
# 如果没有找到帧，next_pos 表示需要保留的数据起始位置
|
||
if next_pos == len(remaining_data):
|
||
print("\n没有更多帧了")
|
||
break
|
||
elif next_pos == 0:
|
||
# 如果next_pos为0，说明剩余数据不足一个帧头，直接结束
|
||
print("\n剩余数据不足一个帧头，结束查找")
|
||
break
|
||
else:
|
||
# 移除无效数据
|
||
removed_data = remaining_data[:next_pos]
|
||
if removed_data:
|
||
print(f"\n移除 {len(removed_data)} 字节无效数据: {removed_data.hex()}")
|
||
remaining_data = remaining_data[next_pos:]
|
||
print(f"剩余数据: ({len(remaining_data)} 字节)")
|
||
if remaining_data:
|
||
# print(f"数据: {remaining_data.hex()}")
|
||
pass
|
||
|
||
print(f"\n总共找到 {frame_count} 个帧") |