started adding UBX protocol
This commit is contained in:
+26
@@ -0,0 +1,26 @@
|
|||||||
|
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(msg_class: int, msg_id: int, payload: bytes) -> bytes:
|
||||||
|
hdr = b'\xb5\x62'
|
||||||
|
frame = struct.pack('B', msg_class)
|
||||||
|
frame += struct.pack('B', msg_id)
|
||||||
|
frame += struct.pack('<H', len(payload))
|
||||||
|
frame += payload
|
||||||
|
check = checksum(frame)
|
||||||
|
|
||||||
|
return hdr + payload + check
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
result = frame_create(1, 2, b'Hallo')
|
||||||
|
print(result)
|
||||||
Reference in New Issue
Block a user