- UBX: fixed data loss when sync byte in payload
- refactored
This commit is contained in:
@@ -13,6 +13,8 @@ class UbxPacketizer(MsgSink, MsgSource):
|
||||
self.packet = b''
|
||||
self.wait_sync = True
|
||||
self.sync_count = 0
|
||||
self.timestamp = 0
|
||||
self.d_save = b''
|
||||
|
||||
def reset(self):
|
||||
self.packet = b''
|
||||
@@ -20,24 +22,27 @@ class UbxPacketizer(MsgSink, MsgSource):
|
||||
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.d_save = struct.pack('B', d)
|
||||
self.sync_count += 1
|
||||
if self.sync_count == len(self.sync_bytes):
|
||||
print("UBX-sync")
|
||||
self.d_save = b''
|
||||
self.timestamp = msg.timestamp
|
||||
self.sync_count = 0
|
||||
self.packet = b''
|
||||
self.wait_sync = False
|
||||
else:
|
||||
self.packet += self.d_save
|
||||
self.d_save = b''
|
||||
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_sinks(MsgContainer(name, timestamp, self.packet))
|
||||
self.call_sinks(MsgContainer(msg.name, self.timestamp, self.packet))
|
||||
self.packet = b''
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user