fixed parsing

ubx_parser.py

@@ -0,0 +1,92 @@
+import struct
+
+UBX_SYNC_WORD = b'\xb5\x62'
+
+def checksum(data: bytes) -> bytes:
+	ck_a = 0
+	ck_b = 0
+	for i in range(len(data)):
+		ck_a = (ck_a + data[i]) & 0xFF
+		ck_b = (ck_b + ck_a) & 0xFF
+
+	return struct.pack("BB", ck_a, ck_b)
+
+class Hdr:
+	def __init__(self, _class: int, _id: int, _length: int):
+		self.mclass = _class
+		self.mid = _id
+		self.length = _length
+
+
+def frame_parse_hdr(frame: bytes) -> Hdr|None:
+	hdr = Hdr(0,0,0)
+
+	if len(frame) < 4:
+		return None
+
+	hdr.mclass = struct.unpack('B', frame[0:1])[0]
+	hdr.mid = struct.unpack('B', frame[1:2])[0]
+	hdr.length = struct.unpack('<H', frame[2:4])[0]
+
+	return hdr
+
+def frame_parse(frame: bytes):
+	_data = None
+	_hdr = Hdr(0,0,0)
+
+	while True:
+		_hdr = frame_parse_hdr(frame[0:4])
+		if _hdr is None:
+			break
+
+		if (4+_hdr.length+2) != len(frame):
+			break
+
+		_check_soll = frame[-2:]
+		_check_ist = checksum(frame[0:-2])
+		if _check_soll != _check_ist:
+			break
+
+		_data = frame[4:-2]
+		break
+
+	return _data, _hdr
+
+class UbxPacketizer:
+	def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
+		self.sync_bytes = sync_bytes
+		self.packet = b''
+		self.wait_sync = True
+		self.sync_count = 0
+		self.d_save = b''
+
+	def reset(self):
+		self.packet = b''
+		self.wait_sync = True
+		self.sync_count = 0
+
+	def on_recv(self, msg: bytes):
+		frames = []
+		for d in msg:
+			if d == self.sync_bytes[self.sync_count]:
+				self.d_save += struct.pack('B', d)
+				self.sync_count += 1
+				if self.sync_count == len(self.sync_bytes):
+					self.sync_count = 0
+					self.d_save = b''
+					self.packet = b''
+					self.wait_sync = False
+			else:
+				if self.sync_count > 0:
+					self.packet += self.d_save
+					self.d_save = b''
+					self.sync_count = 0
+				if not self.wait_sync:
+					self.packet += struct.pack('B', d)
+					data, hdr = frame_parse(self.packet)
+					if data is not None:
+						if hdr.length == len(data):
+							frames.append({"cls": hdr.mclass, "id": hdr.mid, "payload": data})
+							self.packet = b''
+							self.wait_sync = True
+		return frames