67 lines
1.8 KiB
Python
67 lines
1.8 KiB
Python
import struct
|
|
from msg_container import MsgContainer
|
|
from msg_sink import MsgSink
|
|
from msg_source import MsgSource
|
|
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 = UBX_SYNC_WORD):
|
|
MsgSink.__init__(self)
|
|
MsgSource.__init__(self)
|
|
self.sync_bytes = sync_bytes
|
|
self.packet = b''
|
|
self.wait_sync = True
|
|
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
|
|
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):
|
|
return True
|
|
|
|
|
|
if __name__ == "__main__":
|
|
class MySink(MsgSink):
|
|
def __init__(self):
|
|
MsgSink.__init__(self)
|
|
|
|
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))
|