47 lines
1.0 KiB
Python
47 lines
1.0 KiB
Python
import struct
|
|
|
|
|
|
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:
|
|
hdr = b'\xb5\x62'
|
|
frame = struct.pack('B', _class)
|
|
frame += struct.pack('B', _id)
|
|
frame += struct.pack('<H', len(_data))
|
|
frame += _data
|
|
check = checksum(frame)
|
|
|
|
return hdr + frame + check
|
|
|
|
def frame_parse(frame: bytes):
|
|
hdr = frame[0:2]
|
|
if hdr != b'\xb5\x62':
|
|
return None
|
|
|
|
_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):
|
|
return None
|
|
|
|
_data = frame[6:-2]
|
|
_check_soll = frame[-2:]
|
|
_check_ist = checksum(frame[2:-2])
|
|
if _check_soll != _check_ist:
|
|
return None
|
|
|
|
return _data, _class, _id
|
|
|
|
if __name__ == "__main__":
|
|
result = frame_create(1, 2, b'Hallo')
|
|
print(result)
|
|
payload, msg_class, msg_id = frame_parse(result)
|
|
print(payload)
|