Index: DialityCoreCanProtocol.py =================================================================== diff -u -reeded10f89bd0722474d326a6423c755a11cc988 -r6c057347d02b98f98260ccefca3ffada9c7d628d --- DialityCoreCanProtocol.py (.../DialityCoreCanProtocol.py) (revision eeded10f89bd0722474d326a6423c755a11cc988) +++ DialityCoreCanProtocol.py (.../DialityCoreCanProtocol.py) (revision 6c057347d02b98f98260ccefca3ffada9c7d628d) @@ -21,7 +21,167 @@ from time import sleep -class DialinChannels: +class DenaliMessage: + BYTE_ORDER = 'little' + + START_BYTE = 0xA5 + + START_INDEX = 0 + MSG_ID_INDEX = 1 + + PAYLOAD_LENGTH_INDEX = 3 + PAYLOAD_START_INDEX = 4 + + PAYLOAD_LENGTH_FIRST_PACKET = 3 + HEADER_LENGTH = 4 + + PACKET_LENGTH = 8 + MAX_MSG_ID_NUMBER = 65535 + + @staticmethod + def buildBasicMessage(channel_id=0, message=None): + """ + build a message using channel_id and dialin message + :param channel_id: integer with channel id + :param message: array of int with message + :return: dictionary with channel_id and message keys + """ + if message is None: + message = [] + return {'channel_id': channel_id, 'message': message} + + @staticmethod + def buildMessage(channel_id=0, message_id=0, payload=None): + """ + buildPacket builds a Diality Packet + + :param channel_id: is an integer with channel ID + :param message_id: is an integer indicating request ID + :param payload: list with payload. It does not include length + + :return dictionary with channel_id and 8-byte padded message + """ + + if payload is None: + payload = [] + message = [DenaliCanMessenger.START_BYTE] + + if 0 <= message_id <= DenaliMessage.MAX_MSG_ID_NUMBER: + # Make sure an unsigned int was passed + message_id_in_bytes = message_id.to_bytes(2, byteorder=DenaliMessage.BYTE_ORDER) + + message += [message_id_in_bytes[0]] + message += [message_id_in_bytes[1]] + + else: + + return [] + + # Check payload length + payload_length = len(payload) + + # if payload is larger than 255 return nothing + if payload_length <= 255: + # payload has to be a list + message += [payload_length] + else: + return [] + + message += payload + + message = DenaliMessage.__padMessageWithZeros(message) + + return DenaliMessage.buildBasicMessage(channel_id=channel_id, message=message) + + @staticmethod + def __padMessageWithZeros(message): + """ + returns a packet padded with zeros that guarantees that the packet is a multiple of 8 bytes. + :param message: packet that may or may not be multiple of 8 bytes + + :return: packet that is 8-byte multiple + + """ + message_length = len(message) + + # message must be multiple of 8 + if message_length % DenaliMessage.PACKET_LENGTH != 0: + # We need to pad the message with trailing zeros + add_these_many_zeros = math.ceil(message_length / DenaliMessage.PACKET_LENGTH) * \ + DenaliMessage.PACKET_LENGTH - message_length + + message += [0] * add_these_many_zeros + + return message + + @staticmethod + def getChannelID(message): + """ + returns request ID from message + + :param message: dictionary with channel_id and message keys + :return: integer with channel id + + """ + + return message['channel_id'] + + @staticmethod + def getMessageID(message): + """ + returns request ID from packet + + :param message: complete Diality Packet + + :return: integer with request ID + + """ + msg_id_array = message['message'][DenaliMessage.MSG_ID_INDEX: + DenaliMessage.PAYLOAD_LENGTH_INDEX] + + return int.from_bytes(msg_id_array, byteorder=DenaliMessage.BYTE_ORDER) + + @staticmethod + def getPayloadLength(message): + """ + returns payload length from message + :param message: dictionary with channel_id and message keys + :return: a unsigned payload length + """ + + return message['message'][DenaliMessage.PAYLOAD_LENGTH_INDEX] + + @staticmethod + def getPayload(message): + """ + returns payload array from message + :param message: dictionary with channel_id and message keys + :return: a payload array if exist + """ + + payload_length = DenaliMessage.getPayloadLength(message) + + if payload_length == 0: + return None + else: + return message['message'][DenaliMessage.PAYLOAD_START_INDEX:] + + @staticmethod + def getTotalPackets(message, is_array=True): + """ + returns the number of packets needed to transmit Denali Message + :param message: dictionary with channel_id and message keys or raw message + :param is_array: True if message is an array and not a dictionary + :return: number of packets + """ + the_message = message if is_array else message['message'] + + return math.ceil((the_message[DenaliMessage.PAYLOAD_LENGTH_INDEX] + + DenaliMessage.HEADER_LENGTH) / + DenaliMessage.PACKET_LENGTH) + + +class DenaliChannels: # This are all the channels available hd_alarm_broadcast_ch_id = 0x001 @@ -33,6 +193,7 @@ hd_sync_broadcast_ch_id = 0x040 dg_sync_broadcast_ch_id = 0x080 ui_to_hd_ch_id = 0x100 + ui_sync_broadcast_ch_id = 0x200 dialin_to_hd_ch_id = 0x400 hd_to_dialin_ch_id = 0x401 dialin_to_dg_ch_id = 0x402 @@ -41,7 +202,7 @@ ui_to_dialin_ch_id = 0x405 -class LongDialityMessageBuilder: +class LongDenaliMessageBuilder: def __init__(self, can_message): """ @@ -62,8 +223,8 @@ """ self.__message = can_message - self.__number_of_can_message_needed = math.ceil((can_message[3] + 4) / 8) - self.__number_of_can_message_up_to_now = 1 + self.__number_of_can_packets_needed = DenaliMessage.getTotalPackets(can_message) + self.__number_of_can_packets_up_to_now = 1 def push(self, can_message, first_packet=False): """ @@ -77,34 +238,37 @@ """ if first_packet: self.__message = can_message - self.__number_of_can_message_needed = math.ceil((can_message[3] + 4) / 8) - self.__number_of_can_message_up_to_now = 1 + self.__number_of_can_packets_needed = DenaliMessage.getTotalPackets(can_message) + self.__number_of_can_packets_up_to_now = 1 + else: self.__message += can_message - self.__number_of_can_message_up_to_now += 1 + self.__number_of_can_packets_up_to_now += 1 - if self.__number_of_can_message_up_to_now == self.__number_of_can_message_needed: + if self.__number_of_can_packets_up_to_now == self.__number_of_can_packets_needed: return_message = self.__message self.__message = None return return_message + else: return None -class DialinCanMessenger: - START_BYTE = 0xA5 +class DenaliCanMessenger: + START_BYTE = DenaliMessage.START_BYTE def __init__(self, can_interface='can0'): """ - DialityCanMessenger constructor + DenaliCanMessenger constructor :param can_interface - string containing the can interface, e.g., 'can0" :returns DialityCanMessenger object """ - BusType = 'socketcan' - self.__canConnection = can.interface.Bus(channel=can_interface, bustype=BusType) + self.__bus = can.interfaces.socketcan.SocketcanBus(channel=can_interface) + self.__listener_buffer = can.BufferedReader() + self.__notifier = can.Notifier(self.__bus, [self.__listener_buffer]) self.__sendPacketRequestID = -1 self.__sendEvent = threading.Event() @@ -120,8 +284,9 @@ self.__sync_response_dictionary = {} - if self.__canConnection is not None: - self.__serialListenerThread = threading.Thread(target=self.__listener) + if self.__bus is not None: + self.__serialListenerThread = threading.Thread(target=self.__listener, daemon=True) + else: self.__serialListenerThread = None print("Can connection is not valid") @@ -132,7 +297,7 @@ """ - if self.__canConnection is None: + if self.__bus is None: print("Cannot start can listener.") return else: @@ -146,7 +311,7 @@ """ self.__run = False - print("Can listener has stopped.") + print("\nCan listener has stopped.") def __listener(self): """ @@ -156,70 +321,61 @@ while self.__run: - message = self.__canConnection.recv(0.0) + message = self.__listener_buffer.get_message(0.0) - if message is not None and message.dlc == 8: - + if message is not None and message.dlc == DenaliMessage.PACKET_LENGTH: # We have received a legit can message of 8 bytes - can_data = [b for b in message.data] channel_id = message.arbitration_id message_length = can_data[3] # if we are building a long message, then proceed to push it to the channel dictionary if channel_id in self.__longMsgChannelIDSet: - self.__dialinMessage = self.__longMessageBuilders[channel_id].push(can_data) - elif can_data[0] == self.START_BYTE and message_length <= 4: # This is a short packet + elif can_data[0] == self.START_BYTE and \ + message_length <= DenaliMessage.PAYLOAD_LENGTH_FIRST_PACKET: # This is a short packet # This is the first time that we are building a message - self.__dialinMessage = can_data # deliver the packet - elif can_data[0] == self.START_BYTE and message_length > 4: # This is the start of a long packet - + elif can_data[0] == self.START_BYTE and \ + message_length > DenaliMessage.PAYLOAD_LENGTH_FIRST_PACKET: # Long packet start # We are starting to build a long message, include it in the lonMsgChannelIDSet self.__longMsgChannelIDSet.add(channel_id) if channel_id not in self.__longMessageBuilders.keys(): # if we don't have a builder. Create it! - - self.__longMessageBuilders[channel_id] = LongDialityMessageBuilder(can_data) + self.__longMessageBuilders[channel_id] = LongDenaliMessageBuilder(can_data) self.__dialinMessage = None else: # if we do have a builder. This is the first time - self.__dialinMessage = self.__longMessageBuilders[channel_id].push(can_data, first_packet=True) # At this point we have a complete (long or short) Diality Packet if self.__dialinMessage is not None: + completeDialinMessage = DenaliMessage.buildBasicMessage(channel_id=channel_id, + message=self.__dialinMessage) + dialin_msg_id = DenaliMessage.getMessageID(completeDialinMessage) + dialin_ch_id = DenaliMessage.getChannelID(completeDialinMessage) - completeDialinMessage = DialinMessage.buildBasicMessage(channel_id=channel_id, - message=self.__dialinMessage) - dialin_msg_id = DialinMessage.getMessageID(completeDialinMessage) - dialin_ch_id = DialinMessage.getChannelID(completeDialinMessage) - if dialin_ch_id in self.__longMsgChannelIDSet: # We need to remove channel ID from the long message set self.__longMsgChannelIDSet.remove(dialin_ch_id) # We first check if this is a response to a send request that is pending - if dialin_msg_id == self.__sendPacketRequestID: self.__dialinCommandResponseMessage = completeDialinMessage self.__sendEvent.set() self.__sendPacketRequestID = -1 # If it is not, this is a publication message and we need to call it's register function - elif dialin_ch_id in self.__sync_response_dictionary.keys() and \ dialin_msg_id in self.__sync_response_dictionary[channel_id].keys(): self.__sync_response_dictionary[dialin_ch_id][dialin_msg_id](completeDialinMessage) # Done with this message, let's get the next one - self.__dialinMessage = None else: @@ -239,12 +395,10 @@ # if the channel_id exist, we just update the dictionary for the channel_id if channel_id in self.__sync_response_dictionary.keys(): - self.__sync_response_dictionary[channel_id].update({message_id: function}) # otherwise, we need to create the dictionary for the channel_id else: - self.__sync_response_dictionary[channel_id] = {message_id: function} def send(self, built_message, time_out=1): @@ -254,33 +408,32 @@ :param built_message: message built using DialinMessage class :param time_out: time it will wait for a response in seconds - :return: Diality Packet, it it times out it returns None + :returns: Diality Packet, it it times out it returns None """ - channel_id = DialinMessage.getChannelID(built_message) + channel_id = DenaliMessage.getChannelID(built_message) padded_can_message_array = built_message['message'] - padded_can_message_length = padded_can_message_array[3] + self.__sendPacketRequestID = DenaliMessage.getMessageID(built_message) - self.__sendPacketRequestID = DialinMessage.getMessageID(built_message) - # A message can be longer than 8 bytes, so we need to split it # into 8 bytes packets. - number_of_packets = math.ceil((padded_can_message_length + 5) / 8) + number_of_packets = DenaliMessage.getTotalPackets(padded_can_message_array) # We are sending one message at a time on CAN for n in range(number_of_packets): - packet = padded_can_message_array[n * 8:n * 8 + 8] + packet = padded_can_message_array[n * DenaliMessage.PACKET_LENGTH: + (n + 1) * DenaliMessage.PACKET_LENGTH] # Sending one packet at a time - pckt = can.Message(arbitration_id=channel_id, - data=packet, - is_extended_id=False) + packet = can.Message(arbitration_id=channel_id, + data=packet, + is_extended_id=False) - self.__canConnection.send(pckt) + self.__bus.send(packet) # Sending self.__dialinCommandResponseMessage = None @@ -298,146 +451,15 @@ return self.__dialinCommandResponseMessage -class DialinMessage: - BYTE_ORDER = 'little' - - @staticmethod - def buildBasicMessage(channel_id=0, message=None): - """ - build a message using channel_id and dialin message - :param channel_id: integer with channel id - :param message: array of int with message - :return: dictionary with channel_id and message keys - """ - if message is None: - message = [] - return {'channel_id': channel_id, 'message': message} - - @staticmethod - def buildMessage(channel_id=0, message_id=0, payload=None): - """ - buildPacket builds a Diality Packet - - :param channel_id: is an integer with channel ID - :param message_id: is an integer indicating request ID - :param payload: list with payload. It does not include length - - :return dictionary with channel_id and 8-byte padded message - """ - - if payload is None: - payload = [] - message = [DialinCanMessenger.START_BYTE] - - if 0 <= message_id <= (2 ** 16 - 1): - # Make sure an int was passed - message_id_in_bytes = message_id.to_bytes(2, byteorder=DialinMessage.BYTE_ORDER) - - message += [message_id_in_bytes[0]] - message += [message_id_in_bytes[1]] - - else: - - return [] - - # Check payload length - payload_length = len(payload) - - # if payload is larger than 255 return nothing - if payload_length <= 255: - # payload has to be a list - message += [payload_length] - else: - return [] - - message += payload - - message = DialinMessage.__padMessageWithZeros(message) - - return DialinMessage.buildBasicMessage(channel_id=channel_id, message=message) - - @staticmethod - def __padMessageWithZeros(message): - """ - returns a packet padded with zeros that guarantees that the packet is a multiple of 8 bytes. - :param message: packet that may or may not be multiple of 8 bytes - - :return: packet that is 8-byte multiple - - """ - message_length = len(message) - - # message must be multiple of 8 - if message_length % 8 != 0: - # We need to pad the message with trailing zeros - add_these_many_zeros = math.ceil(message_length / 8) * 8 - message_length - - message += [0] * add_these_many_zeros - - return message - - @staticmethod - def getChannelID(message): - """ - returns request ID from message - - :param message: dictionary with channel_id and message keys - :return: integer with channel id - - """ - - return message['channel_id'] - - @staticmethod - def getMessageID(message): - """ - returns request ID from packet - - :param message: complete Diality Packet - - :return: integer with request ID - - """ - msg_id_array = message['message'][1:3] - - return int.from_bytes(msg_id_array, byteorder=DialinMessage.BYTE_ORDER) - - @staticmethod - def getPayloadLength(message): - """ - returns payload length from message - :param message: dictionary with channel_id and message keys - :return: a unsigned payload length - """ - - return message['message'][4] - - @staticmethod - def getPayload(message): - """ - returns payload array from message - :param message: dictionary with channel_id and message keys - :return: a payload array if exist - """ - - payload_length = DialinMessage.getPayloadLength(message) - - if payload_length == 0: - return None - else: - return message['message'][5:] - - -def test_print_received_packet(messsage, sync=False): - channel_id = messsage[0] - messsage[0] = 0xA5 - +def test_print_received_packet(message, sync=False): + channel_id = message[0] + message[0] = DenaliMessage.START_BYTE introduction = "Received: " if sync: introduction = "Sync " + introduction - print(introduction, messsage, " in channel: ", channel_id) + print(introduction, message, " in channel: ", channel_id) def test_print_to_screen(message): @@ -451,25 +473,25 @@ test_print_received_packet(message, sync=True) -def test_print_sending_dg_board(channel_id, message): - print("Sending to board: ", message, " in channel: ", channel_id) +def test_print_sending_dg_board(message): + print("Sending to board: ", message['message'], " in channel: ", message['channel_id']) -def test_print_sending_dg_sim(channel_id, messsage): - print("Sending to DG simulator: ", messsage, " in channel", channel_id, end=" ---> ") +def test_print_sending_dg_sim(message): + print("Sending to DG simulator: ", message['message'], " in channel", message['channel_id'], end=" ---> ") if __name__ == "__main__": - test_messenger = DialinCanMessenger() - test_channel_id = DialinChannels.ui_to_hd_ch_id + test_messenger = DenaliCanMessenger() + test_channel_id = DenaliChannels.ui_to_hd_ch_id - test_received_channel_id = DialinMessage.hd_to_ui_ch_id + test_received_channel_id = DenaliChannels.hd_to_ui_ch_id test_received_message_id = 0x100 test_messenger.registerReceivingPublicationFunction(test_received_channel_id, test_received_message_id, test_function_for_sync) - test_dg_simulator_received_channel_id = DialinMessage.dialin_to_dg_ch_id + test_dg_simulator_received_channel_id = DenaliChannels.dialin_to_dg_ch_id test_dg_simulator_sync_msg_id = 0x05 test_dg_simulator_msg_id = 0x03 @@ -479,26 +501,29 @@ test_messenger.start() - # test_packet = DialityPacket.buildPacket(0x01, [1]) - test_packet = DialinMessage.buildMessage(channel_id=1000, message_id=0x01, payload=[1]) - test_dg_packet = DialinMessage.buildMessage(test_dg_simulator_msg_id, []) + test_msg = DenaliMessage.buildMessage(channel_id=1000, + message_id=0x01, + payload=[1]) + test_dg_msg = DenaliMessage.buildMessage(channel_id=test_dg_simulator_received_channel_id, + message_id=test_dg_simulator_msg_id, + payload=[]) sleep(3.0) - test_print_sending_dg_board(test_channel_id, test_packet) - test_response = test_messenger.send(test_channel_id, test_packet) + test_print_sending_dg_board(test_msg) + test_response = test_messenger.send(test_msg) test_print_to_screen(test_response) sleep(3.0) - test_print_sending_dg_board(test_channel_id, test_packet) - test_response = test_messenger.send(test_channel_id, test_packet) + test_print_sending_dg_board(test_msg) + test_response = test_messenger.send(test_msg) test_print_to_screen(test_response) sleep(3.0) - test_print_sending_dg_sim(test_dg_simulator_received_channel_id, test_dg_packet) - test_response = test_messenger.send(test_dg_simulator_received_channel_id, test_dg_packet) + test_print_sending_dg_sim(test_dg_msg) + test_response = test_messenger.send(test_dg_msg) test_print_to_screen(test_response) sleep(3.0) - test_print_sending_dg_sim(test_dg_simulator_received_channel_id, test_dg_packet) - test_response = test_messenger.send(test_dg_simulator_received_channel_id, test_dg_packet) + test_print_sending_dg_sim(test_dg_msg) + test_response = test_messenger.send(test_dg_msg) test_print_to_screen(test_response)