refactored

receiver/ubx_packetizer.py

@@ -1,34 +1,44 @@
+import struct
 from msg_container import MsgContainer
 from msg_sink import MsgSink
 from msg_source import MsgSource
-from struct import pack
+from ubx.ubx import UBX_SYNC_WORD, frame_parse_hdr, frame_create
+from receiver.ubx_receiver import UbxReceiver
 
 class UbxPacketizer(MsgSink, MsgSource):
-	def __init__(self, sync_bytes: bytes):
+	def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
 		MsgSink.__init__(self)
 		MsgSource.__init__(self)
 		self.sync_bytes = sync_bytes
 		self.packet = b''
 		self.wait_sync = True
-		self.latency = None
+		self.sync_count = 0
+
+	def reset(self):
+		self.packet = b''
+		self.wait_sync = True
+		self.sync_count = 0
+
 	def on_recv(self, msg: MsgContainer):
 		name = msg.name
-		data = msg.data
-		w = len(self.sync_bytes)
-		self.latency = 0
-		for n in range (0, len(data)):
-			d = data[n:n+w]
-			if d == self.sync_bytes:
-				self.latency = w-1
-				print("UBX-sync")
-				timestamp = msg.timestamp
-				self.wait_sync = False
-			elif not self.wait_sync:
-				if self.latency == 0:
-					self.packet += d
-				else:
-					self.latency -= 1
-
+		timestamp = msg.timestamp
+		for n in range (0, len(msg.data)):
+			d = msg.data[n]
+			if d == self.sync_bytes[self.sync_count]:
+				self.sync_count += 1
+				if self.sync_count == len(self.sync_bytes):
+					print("UBX-sync")
+					self.sync_count = 0
+					self.packet = b''
+					self.wait_sync = False
+			else:
+				if not self.wait_sync:
+					self.packet += struct.pack('B', d)
+					hdr = frame_parse_hdr(self.packet)
+					if hdr is not None:
+						if (6 + hdr.length) == len(self.packet):
+							self.call_sinks(MsgContainer(name, timestamp, self.packet))
+							self.packet = b''
 
 
 	def is_msg(self, msg: MsgContainer):
@@ -36,11 +46,21 @@ class UbxPacketizer(MsgSink, MsgSource):
 
 
 if __name__ == "__main__":
-	_sync = b'JA'
-	_data = b'31'
-	pkt = UbxPacketizer(sync_bytes=_sync)
-	frame1 = _sync + b'31' + _sync + b'31'
-	pkt.on_recv(MsgContainer('test', 31101970.0208, frame1))
+	class MySink(MsgSink):
+		def __init__(self):
+			MsgSink.__init__(self)
 
-	frame2 = _sync + b'3' + _sync + b'10'
-	pkt.on_recv(MsgContainer('test', 31101970.0208, frame2))
+		def on_recv(self, msg: MsgContainer):
+			print(msg)
+
+		def is_msg(self, msg: MsgContainer):
+			return True
+
+	ubx_rx = UbxReceiver('test', 1, 2)
+
+	pkt = UbxPacketizer()
+	pkt.register_recv(ubx_rx)
+
+	pkt.reset()
+	frame = frame_create(_class=1, _id=2, _data=b'Hallo') + frame_create(_class=1, _id=2, _data=b',') + frame_create(_class=1, _id=2, _data=b'UBX') + frame_create(_class=1, _id=2, _data=b'World')
+	pkt.on_recv(MsgContainer('test', 31101970.0208, frame))