refactored

ubx/ubx.py

@@ -1,5 +1,8 @@
 import struct
 
+from msg_container import MsgContainer
+from msg_sink import MsgSink
+
 
 def checksum(data: bytes) -> bytes:
 	ck_a = 0
@@ -11,34 +14,65 @@ def checksum(data: bytes) -> bytes:
 	return struct.pack("BB", ck_a, ck_b)
 
 def frame_create(_class: int, _id: int, _data: bytes) -> bytes:
-	hdr = b'\xb5\x62'
-	frame = struct.pack('B', _class)
-	frame += struct.pack('B', _id)
-	frame += struct.pack('<H', len(_data))
-	frame += _data
-	check = checksum(frame)
+	_sync = b'\xb5\x62'
+	_hdr = struct.pack('B', _class)
+	_hdr += struct.pack('B', _id)
+	_hdr += struct.pack('<H', len(_data))
+	_check = checksum(_hdr + _data)
 
-	return hdr + frame + check
+	return _sync + _hdr + _data + _check
 
 def frame_parse(frame: bytes):
-	hdr = frame[0:2]
-	if hdr != b'\xb5\x62':
-		return None
+	_class = 0
+	_id = 0
+	_data = None
 
-	_class = struct.unpack('B', frame[2:3])[0]
-	_id = struct.unpack('B', frame[3:4])[0]
-	_length = struct.unpack('<H', frame[4:6])[0]
-	if (6+_length+2) != len(frame):
-		return None
+	while True:
+		hdr = frame[0:2]
+		if hdr != b'\xb5\x62':
+			break
 
-	_data = frame[6:-2]
-	_check_soll = frame[-2:]
-	_check_ist = checksum(frame[2:-2])
-	if _check_soll != _check_ist:
-		return None
+		_class = struct.unpack('B', frame[2:3])[0]
+		_id = struct.unpack('B', frame[3:4])[0]
+		_length = struct.unpack('<H', frame[4:6])[0]
+		if (6+_length+2) != len(frame):
+			break
+
+		_check_soll = frame[-2:]
+		_check_ist = checksum(frame[2:-2])
+		if _check_soll != _check_ist:
+			break
+
+		_data = frame[6:-2]
+		break
 
 	return _data, _class, _id
 
+class UbxReceiver(MsgSink):
+	def __init__(self, name: str, mid: int, mclass: int):
+		MsgSink.__init__(self)
+		self.name = name
+		self.mid = mid
+		self.mclass = mclass
+
+	def on_recv(self, msg: MsgContainer):
+		data, mclass, mid = frame_parse(msg.data)
+		print(f"{mclass}:{mid}:{data}")
+
+
+	def is_msg(self, msg: MsgContainer):
+		data, mclass, mid = frame_parse(msg.data)
+		if data is None:
+			return False
+
+		if mclass != self.mclass:
+			return False
+
+		if mid != self.mid:
+			return False
+
+		return True
+
 if __name__ == "__main__":
 	result = frame_create(1, 2, b'Hallo')
 	print(result)