class BPLProtocol:
""" Helper class used to encode and decode BPL packets."""
CRC8_FUNC = crcmod.mkCrcFun(0x14D, initCrc=0xFF, xorOut=0xFF)
@staticmethod
def packet_splitter(buff: bytes) -> Tuple[List[bytes], Optional[bytes]]:
"""
Split packets coming in along bpl protocol, Packets are split via 0x00.
"""
incomplete_packet = None
packets = re.split(b'\x00', buff)
if buff[-1] != b'0x00':
incomplete_packet = packets.pop()
return packets, incomplete_packet
@staticmethod
def parse_packet(
packet_in: Union[bytes, bytearray]) -> Tuple[int, int, bytes]:
"""
Parse the packet returning a tuple of [int, int, bytes].
If unable to parse the packet, then return 0,0,b''.
"""
packet_in = bytearray(packet_in)
if packet_in and len(packet_in) > 3:
try:
decoded_packet: bytes = cobs.decode(packet_in)
except cobs.DecodeError as e:
logger.warning(f"parse_packet(): Cobs Decoding Error, {e}")
return 0, 0, b''
if decoded_packet[-2] != len(decoded_packet):
logger.warning(
f"parse_packet(): Incorrect length: length is {len(decoded_packet)} "
f"in {[hex(x) for x in list(decoded_packet)]}")
return 0, 0, b''
else:
if BPLProtocol.CRC8_FUNC(
decoded_packet[:-1]) == decoded_packet[-1]:
rx_data = decoded_packet[:-4]
device_id = decoded_packet[-3]
packet_id = decoded_packet[-4]
rx_data = rx_data
return device_id, packet_id, rx_data
else:
logger.warning(
f"parse_packet(): CRC error in {[hex(x) for x in list(decoded_packet)]} "
)
return 0, 0, b''
return 0, 0, b''
@staticmethod
def encode_packet(device_id: int, packet_id: int, data: Union[bytes,
bytearray]):
""" Encode the packet using the bpl protocol."""
tx_packet = bytes(data)
tx_packet += bytes([packet_id, device_id, len(tx_packet) + 4])
tx_packet += bytes([BPLProtocol.CRC8_FUNC(tx_packet)])
packet: bytes = cobs.encode(tx_packet) + b'\x00'
return packet
@staticmethod
def decode_floats(data: Union[bytes, bytearray]) -> List[float]:
""" Decode a received byte list, into a float list as specified by the bpl protocol"""
list_data = list(struct.unpack(str(int(len(data) / 4)) + "f", data))
return list_data
HEARTBEAT_FREQUENCY = 0x92
"1 byte - set the frequency of a packet to be sent from a device."
HEARTBEAT_SET = 0x91
"10 bytes - Specify the Packet IDs to be sent via heartbeat."
POSITION_LIMITS = 0x10
"2 floats - Maximum and Minimum positions of the device"
VELOCITY_LIMITS = 0x11
"2 floats - Maximum and Minimum velocities of the device"
CURRENT_LIMITS = 0x12
"2 floats - Maximum and Minimum currents of the device"
ATI_FT_READING = 0xD8
"6 floats - Read force in N and Torque in Nm from the Force torque sensor. (FX, FY, FZ, TX, TY, TZ). Send this packet to the FT Sensor to Tare it"
BOOTLOADER = 0xFF
CRC8_FUNC = crcmod.mkCrcFun(0x14D, initCrc=0xFF, xorOut=0xFF)
class BPLProtocol:
"""Class used to encode and decode BPL packets."""
@staticmethod
def packet_splitter(buff):
"""
Split packets coming in along bpl protocol, Packets are split at b'0x00'.
:param buff: input buffer of bytes
:return: List of bytes separated by 0x00, and a remaining bytes of an incomplete packet.
"""
incomplete_packet = None
packets = re.split(b'\x00', buff)
if buff[-1] != b'0x00':
class BPLProtocol:
"""Class used to encode and decode BPL packets."""
CRC8_FUNC = crcmod.mkCrcFun(0x14D, initCrc=0xFF, xorOut=0xFF)
@staticmethod
def packet_splitter(buff: bytes) -> Tuple[List[bytes], Optional[bytes]]:
"""
Split packets coming in along bpl protocol, Packets are split at b'0x00'.
:param buff: input buffer of bytes
:return: List of bytes separated by 0x00, and a remaining bytes of an incomplete packet.
"""
incomplete_packet = None
packets = re.split(b'\x00', buff)
if buff[-1] != b'0x00':
incomplete_packet = packets.pop()
return packets, incomplete_packet
@staticmethod
def parse_packet(packet_in: Union[bytes, bytearray]) -> Tuple[int, int, bytes]:
"""
Parse the packet returning a tuple of [int, int, bytes].
If unable to parse the packet, then return 0,0,b''.
:param packet_in: bytes of a full packet
:return: device_id, packet_id, data in bytes.
"""
packet_in = bytearray(packet_in)
if packet_in and len(packet_in) > 3:
try:
decoded_packet: bytes = cobs.decode(packet_in)
except cobs.DecodeError as e:
logger.warning(f"parse_packet(): Cobs Decoding Error, {e}")
return 0, 0, b''
if decoded_packet[-2] != len(decoded_packet):
logger.warning(f"parse_packet(): Incorrect length: length is {len(decoded_packet)} "
f"in {[hex(x) for x in list(decoded_packet)]}")
return 0, 0, b''
else:
if BPLProtocol.CRC8_FUNC(decoded_packet[:-1]) == decoded_packet[-1]:
rx_data = decoded_packet[:-4]
device_id = decoded_packet[-3]
packet_id = decoded_packet[-4]
rx_data = rx_data
return device_id, packet_id, rx_data
else:
logger.warning(f"parse_packet(): CRC error in {[hex(x) for x in list(decoded_packet)]} ")
return 0, 0, b''
return 0, 0, b''
@staticmethod
def encode_packet(device_id: int, packet_id: int, data: Union[bytes, bytearray]):
"""
Encode the packet using the bpl protocol.
:param device_id: Device ID
:param packet_id: Packet ID
:param data: Data in bytes
:return: bytes of the encoded packet.
"""
tx_packet = bytes(data)
tx_packet += bytes([packet_id, device_id, len(tx_packet)+4])
tx_packet += bytes([BPLProtocol.CRC8_FUNC(tx_packet)])
packet: bytes = cobs.encode(tx_packet) + b'\x00'
return packet
@staticmethod
def decode_floats(data: Union[bytes, bytearray]) -> List[float]:
"""
Decode a received byte list, into a float list as specified by the bpl protocol
Bytes are decoded into 32 bit floats.
:param data: bytes, but be divisible by 4.
:return: decoded list of floats
"""
list_data = list(struct.unpack(str(int(len(data)/4)) + "f", data))
return list_data
@staticmethod
def encode_floats(float_list: List[float]) -> bytes:
""" Encode a list of floats into bytes
Floats are encoded into 32 bits (4 bytes)
:param float_list: list of floats
:return: encoded bytes
"""
data = struct.pack('%sf' % len(float_list), *float_list)
return data