56 lines
1.5 KiB
Python
56 lines
1.5 KiB
Python
import struct
|
|
from msg.container import MsgContainer
|
|
from msg.listener import MsgListener
|
|
from msg.talker import MsgTalker
|
|
from ubx.ubx import UBX_SYNC_WORD, frame_parse
|
|
|
|
UBX_PKT_DEBUG = False
|
|
def ubx_pkt_debug(s: str):
|
|
if UBX_PKT_DEBUG:
|
|
print(s)
|
|
|
|
class UbxPacketizer(MsgListener, MsgTalker):
|
|
def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
|
|
MsgListener.__init__(self)
|
|
MsgTalker.__init__(self)
|
|
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 is_msg(self, msg: MsgContainer):
|
|
return True
|
|
|
|
def on_recv(self, msg: MsgContainer):
|
|
for d in msg.data:
|
|
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):
|
|
ubx_pkt_debug(f"{msg.source.name}:UBX-sync")
|
|
if len(self.packet) > 0:
|
|
ubx_pkt_debug(f"{msg.source.name}:Remaining packet: {self.packet}")
|
|
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):
|
|
self.call_listener(MsgContainer(self.packet, msg.source))
|
|
self.packet = b''
|
|
self.wait_sync = True
|