Index: DialityCoreSerialProtocol.py =================================================================== diff -u -r45dc69508ab8752571ff888887a59c6d7d12ff05 -rabd93c1d800178a5ee618b9bba83532b853e3c96 --- DialityCoreSerialProtocol.py (.../DialityCoreSerialProtocol.py) (revision 45dc69508ab8752571ff888887a59c6d7d12ff05) +++ DialityCoreSerialProtocol.py (.../DialityCoreSerialProtocol.py) (revision abd93c1d800178a5ee618b9bba83532b853e3c96) @@ -17,20 +17,25 @@ import serial import threading +import math from enum import Enum from time import sleep -START_BYTE = b'\xA5' class DSM_State(Enum): - SEARCHING_FOR_START_BYTE = 0 - SEARCHING_FOR_PACKET_LENGTH = 1 - SEARCHING_FOR_CRC = 2 + """ + \brief defines 3 states of search + """ + SEARCHING_FOR_START_BYTE = 0 # ///< search for start byte + SEARCHING_FOR_PACKET_LENGTH = 1 # ///< search for the end of packet + SEARCHING_FOR_CRC = 2 # ///< search and check for CRC class DialitySerialMessenger: + START_BYTE = b'\xA5' + def __init__(self, serial_port='/dev/ttyUSB0'): self.__serialConnection = serial.Serial(serial_port, baudrate=115200) @@ -39,8 +44,9 @@ self.__searchState = DSM_State.SEARCHING_FOR_START_BYTE self.__dialityPacket = bytearray() + self.__dialityResponsePacket = bytearray() self.__dialityPacketLength = 0 - self.__sendRID = 0 + self.__sendRID = -1 self.__sendEvent = threading.Event() self.responseFunction = None @@ -59,9 +65,10 @@ print("Serial listener has started.") self.__serialListenerThread.start() - def __getRequestID(self, message): + @staticmethod + def __getRequestID(message): - return int.from_bytes(message[1:2], byteorder='little', signed=False) + return int.from_bytes(message[1:3], byteorder='big', signed=False) def __listener(self): @@ -75,7 +82,7 @@ self.__dialitySerialByteBuffer += chunk_of_bytes - print("\n\nArriving Buffer: " + str(chunk_of_bytes)) + #print("\n\nArriving Buffer: " + str(chunk_of_bytes)) continueProcessing = True @@ -86,10 +93,10 @@ if self.__searchState == DSM_State.SEARCHING_FOR_START_BYTE: - print("\n\nProcessing buffer: " + str(self.__dialitySerialByteBuffer)) - print("Searching for start byte") + # print("\n\nProcessing buffer: " + str(self.__dialitySerialByteBuffer)) + # print("Searching for start byte") - index = self.__dialitySerialByteBuffer.find(START_BYTE) + index = self.__dialitySerialByteBuffer.find(self.START_BYTE) if index >= 0: self.__dialitySerialByteBuffer = self.__dialitySerialByteBuffer[index:] @@ -104,7 +111,7 @@ if self.__searchState == DSM_State.SEARCHING_FOR_PACKET_LENGTH and \ len(self.__dialitySerialByteBuffer) >= 4: - print("Searching for packet length") + # print("Searching for packet length") self.__dialityPacketLength = self.__dialitySerialByteBuffer[3] + 5 self.__searchState = DSM_State.SEARCHING_FOR_CRC @@ -117,25 +124,29 @@ if self.__searchState == DSM_State.SEARCHING_FOR_CRC and \ len(self.__dialitySerialByteBuffer) >= self.__dialityPacketLength: - print("Searching for crc") + #print("Searching for crc") crc = self.__dialitySerialByteBuffer[self.__dialityPacketLength - 1] # Get the Denali packet self.__dialityPacket = self.__dialitySerialByteBuffer[0:self.__dialityPacketLength] - print("Packet CRC: " + str(crc) + ", Calculated CRC:" + str(self.__denaliPacketCRC())) + #print("Packet CRC: " + str(crc) + ", Calculated CRC:" + str(self.__denaliPacketCRC())) - if crc == 0: #self.__denaliPacketCRC(): + if True: # crc == self.__denaliPacketCRC(): denaliResponseRequestID = self.__getRequestID(self.__dialityPacket) + #print("send: "+str(self.__sendRID)+" rec: "+str(denaliResponseRequestID)) if denaliResponseRequestID == self.__sendRID: + self.__dialityResponsePacket = self.__dialityPacket self.__sendEvent.set() # Set the "sendEvent", so the "send" command can continue + self.__sendRID = -1 else: + self.__dialityResponsePacket = bytearray() self.__dialityPacket.clear() # We didn't get what we were expecting # We are going to let the command timeout - print("Final Denali Message: " + str(self.__dialityPacket)) + # print("Final Denali Message: " + str(self.__dialityPacket)) else: @@ -164,7 +175,7 @@ def registerResponseFunction(self, function): self.responseFunction = function - def write(self, message): + def send(self, message, time_out=1): # Check message ID @@ -174,13 +185,75 @@ self.__serialConnection.write(message) - return self.__sendEvent + self.__sendEvent.wait(time_out) - def getMessage(self): + self.__sendRID = -1 - return self.__dialityPacket + return self.__dialityResponsePacket +class DialityPacket: + + @staticmethod + def getCRC(message): + """ \brief getCRC returns a message with its CRC """ + + message_cargo_length = message[3] + return sum(message[0:4 + message_cargo_length]) % 256 + + @staticmethod + def buildPacket(request_id=0, cargo=bytearray()): + """ \brief buildPacket builds a Diality Packet + + \param + + \request_id is an integer indicating request ID + \cargo is a byte array with cargo. It does not include length + """ + + message = b'\xA5' + + if 0 <= request_id <= (2**16 - 1): + # Make sure an int was passed + message += request_id.to_bytes(2, byteorder='big') + else: + return bytearray() + + # Check cargo length + cargo_length = len(cargo) + + # if cargo is larger than 255 return nothing + if cargo_length <= 255: + # cargo has to be a byte array + message += cargo_length.to_bytes(1, byteorder='big') + else: + return bytearray() + + message += cargo + + crc = DialityPacket.getCRC(message) + + message += crc.to_bytes(1, byteorder='big') # crc location + + message_length = len(message) + + # message must be multiple of 8 + if message_length % 8 != 0: + # We need to patch the message with zeros + + add_these_many_zeros = math.ceil(message_length / 8) * 8 - message_length + + message += bytearray(add_these_many_zeros) + + return message + + if __name__ == "__main__": - the_messenger = DialitySerialMessenger(serial_port="/dev/ttyUSB0") - the_messenger.start() + # the_messenger = DialitySerialMessenger(serial_port="/dev/ttyUSB0") + # the_messenger.start() + + test = DialityPacket.buildPacket(request_id=8000, cargo=b'123') + print(test) + + test1 = DialityPacket.buildPacket(request_id=8000, cargo=b'12345') + print(test1)