47 lines
1.1 KiB
Python
47 lines
1.1 KiB
Python
from msg_container import MsgContainer
|
|
from msg_sink import MsgSink
|
|
from msg_source import MsgSource
|
|
from struct import pack
|
|
|
|
class UbxPacketizer(MsgSink, MsgSource):
|
|
def __init__(self, sync_bytes: bytes):
|
|
MsgSink.__init__(self)
|
|
MsgSource.__init__(self)
|
|
self.sync_bytes = sync_bytes
|
|
self.packet = b''
|
|
self.wait_sync = True
|
|
self.latency = None
|
|
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
|
|
|
|
|
|
|
|
def is_msg(self, msg: MsgContainer):
|
|
return True
|
|
|
|
|
|
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))
|
|
|
|
frame2 = _sync + b'3' + _sync + b'10'
|
|
pkt.on_recv(MsgContainer('test', 31101970.0208, frame2))
|