81 lines
1.6 KiB
Python
81 lines
1.6 KiB
Python
import struct
|
|
|
|
from msg_container import MsgContainer
|
|
from msg_sink import MsgSink
|
|
|
|
|
|
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)
|
|
|
|
def frame_create(_class: int, _id: int, _data: bytes) -> bytes:
|
|
_sync = b'\xb5\x62'
|
|
_hdr = struct.pack('B', _class)
|
|
_hdr += struct.pack('B', _id)
|
|
_hdr += struct.pack('<H', len(_data))
|
|
_check = checksum(_hdr + _data)
|
|
|
|
return _sync + _hdr + _data + _check
|
|
|
|
def frame_parse(frame: bytes):
|
|
_class = 0
|
|
_id = 0
|
|
_data = None
|
|
|
|
while True:
|
|
hdr = frame[0:2]
|
|
if hdr != b'\xb5\x62':
|
|
break
|
|
|
|
_class = struct.unpack('B', frame[2:3])[0]
|
|
_id = struct.unpack('B', frame[3:4])[0]
|
|
_length = struct.unpack('<H', frame[4:6])[0]
|
|
if (6+_length+2) != len(frame):
|
|
break
|
|
|
|
_check_soll = frame[-2:]
|
|
_check_ist = checksum(frame[2:-2])
|
|
if _check_soll != _check_ist:
|
|
break
|
|
|
|
_data = frame[6:-2]
|
|
break
|
|
|
|
return _data, _class, _id
|
|
|
|
class UbxReceiver(MsgSink):
|
|
def __init__(self, name: str, mid: int, mclass: int):
|
|
MsgSink.__init__(self)
|
|
self.name = name
|
|
self.mid = mid
|
|
self.mclass = mclass
|
|
|
|
def on_recv(self, msg: MsgContainer):
|
|
data, mclass, mid = frame_parse(msg.data)
|
|
print(f"{mclass}:{mid}:{data}")
|
|
|
|
|
|
def is_msg(self, msg: MsgContainer):
|
|
data, mclass, mid = frame_parse(msg.data)
|
|
if data is None:
|
|
return False
|
|
|
|
if mclass != self.mclass:
|
|
return False
|
|
|
|
if mid != self.mid:
|
|
return False
|
|
|
|
return True
|
|
|
|
if __name__ == "__main__":
|
|
result = frame_create(1, 2, b'Hallo')
|
|
print(result)
|
|
payload, msg_class, msg_id = frame_parse(result)
|
|
print(payload)
|