92 lines
2.0 KiB
Python
92 lines
2.0 KiB
Python
import struct
|
|
|
|
UBX_SYNC_WORD = b'\xb5\x62'
|
|
|
|
def checksum(data: bytes) -> bytes:
|
|
ck_a = 0
|
|
ck_b = 0
|
|
for i in range(len(data)):
|
|
ck_a = (ck_a + data[i]) & 0xFF
|
|
ck_b = (ck_b + ck_a) & 0xFF
|
|
|
|
return struct.pack("BB", ck_a, ck_b)
|
|
|
|
class Hdr:
|
|
def __init__(self, _class: int, _id: int, _length: int):
|
|
self.mclass = _class
|
|
self.mid = _id
|
|
self.length = _length
|
|
|
|
|
|
def frame_parse_hdr(frame: bytes) -> Hdr|None:
|
|
hdr = Hdr(0,0,0)
|
|
|
|
if len(frame) < 4:
|
|
return None
|
|
|
|
hdr.mclass = struct.unpack('B', frame[0:1])[0]
|
|
hdr.mid = struct.unpack('B', frame[1:2])[0]
|
|
hdr.length = struct.unpack('<H', frame[2:4])[0]
|
|
|
|
return hdr
|
|
|
|
def frame_parse(frame: bytes):
|
|
_data = None
|
|
_hdr = Hdr(0,0,0)
|
|
|
|
while True:
|
|
_hdr = frame_parse_hdr(frame[0:4])
|
|
if _hdr is None:
|
|
break
|
|
|
|
if (4+_hdr.length+2) != len(frame):
|
|
break
|
|
|
|
_check_soll = frame[-2:]
|
|
_check_ist = checksum(frame[0:-2])
|
|
if _check_soll != _check_ist:
|
|
break
|
|
|
|
_data = frame[4:-2]
|
|
break
|
|
|
|
return _data, _hdr
|
|
|
|
class UbxPacketizer:
|
|
def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
|
|
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 on_recv(self, msg: bytes):
|
|
frames = []
|
|
for d in msg:
|
|
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):
|
|
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):
|
|
frames.append({"cls": hdr.mclass, "id": hdr.mid, "payload": data})
|
|
self.packet = b''
|
|
self.wait_sync = True
|
|
return frames |