Commit 9a0d51cf authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[driver/python] Successfull decoding and checksum check on USBL message

parent 03adaace
import serial
import asyncio
import time
import threading
from ctypes import c_uint8, c_uint16
def hexascii_to_value(c):
if c >= ord('A'):
return c - ord('A') + 10
else:
return c - ord('0')
def crc16_update(checksum, v):
poly = 0xA001
checksum = ((checksum>>1) ^ poly) if ((v & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>1 & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>2 & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>3 & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>4 & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>5 & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>6 & 0x01) ^ (checksum & 0x01)) else checksum>>1
checksum = ((checksum>>1) ^ poly) if ((v>>7 & 0x01) ^ (checksum & 0x01)) else checksum>>1
return checksum
class SeatracSerial:
......@@ -21,11 +39,46 @@ class SeatracSerial:
self.running = True
while self.running:
part = self.serial.read(1024)
buf = b''
while len(part) > 0:
print(part)
idx = part.find(ord('\n'))
if idx < 0:
buf += part
else:
buf += part[:idx+1]
if len(buf) > 3 and buf[0] == ord('$') and buf[-2] == ord('\r'):
self.decode_received(buf[1:-2])
buf = b''
part = part[idx+1:]
part = self.serial.read(1024)
await asyncio.sleep(0.05)
def decode_received(self, buf):
checksum = 0x00
it = iter(buf)
data = b''
for i in range(int((len(buf) - 4) / 2)):
try:
value = hexascii_to_value(next(it)) << 4
value += hexascii_to_value(next(it))
data += bytes([value])
checksum = crc16_update(checksum, value)
except StopIteration:
raise RuntimeError("Fatal internal python error (should not happen)")
receivedChecksum = hexascii_to_value(next(it)) << 4
receivedChecksum += hexascii_to_value(next(it))
receivedChecksum += hexascii_to_value(next(it)) << 12
receivedChecksum += hexascii_to_value(next(it)) << 8
if receivedChecksum != checksum:
print("Error decoding message : checksum does not check")
return
self.on_receive(data)
def on_receive(self, data):
print(data)
def start(self, background=False):
self.serial.flush()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment