import threading from can import BusABC, Message from udsoncan.client import Client from udsoncan.exceptions import TimeoutException import udsoncan from udsoncan.connections import BaseConnection from udsoncan import services, Response, MemoryLocation import isotp from isotp import CanMessage import queue from USBCAN import * from Shifter import ShifterClass from mainwindows import Ui_MainWindow from PySide2 import QtWidgets, QtCore, QtGui import struct import time import datetime from multiprocessing import Array, Pool, Process, Queue, Value MAX_RCV_NUM = 20 logger = logging.getLogger() logger.setLevel(logging.DEBUG) formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') consoleHandler = logging.StreamHandler() consoleHandler.setLevel(logging.DEBUG) fileHandler = logging.FileHandler( './log/ShiftTool.log', mode='a', encoding='UTF-8') fileHandler.setLevel(logging.NOTSET) consoleHandler.setFormatter(formatter) fileHandler.setFormatter(formatter) logger.addHandler(consoleHandler) logger.addHandler(fileHandler) class UISignals(QtCore.QObject): sig_Disp_str = QtCore.Signal(str) sig_MsgReceived = QtCore.Signal(int) class HardwreDevice(): def __init__(self): self.device_type = 4 self.channel = 0 # can_index self.device = 0 # device_index self.baudrate = 0 class PeriodSendThread(object): def __init__(self, period_func, args=[], kwargs={}): self._thread = threading.Thread(target=self._run) self._function = period_func self._args = args self._kwargs = kwargs self._period = 0 self._event = threading.Event() self._period_event = threading.Event() self._terminated = False def start(self): self._thread.start() def stop(self): self._terminated = True self._event.set() self._thread.join() def send_start(self, period): self._period = period self._event.set() def send_stop(self): self._period_event.set() def _run(self): while True: self._event.wait() self._event.clear() if self._terminated: break self._function(*self._args, **self._kwargs) while not self._period_event.wait(self._period): self._function(*self._args, **self._kwargs) self._period_event.clear() class ShifterTool(): class IsoTpConnection(BaseConnection): mtu = 4095 def __init__(self, isotp_layer, name=None): BaseConnection.__init__(self, name) self.UDStoIsoTPQueue = queue.Queue() # USD --> isotp self.IsoTPtoUDSQueue = queue.Queue() # ISOTP --> UDS self.CANtoIsoTPQueue = queue.Queue() # CAN --> isotp # self.IsoTPtoCANQueue = queue.Queue() # ISOTP--> CAN self._read_thread = None self.exit_requested = False self.opened = False self.isotp_layer = isotp_layer assert isinstance( self.isotp_layer, isotp.TransportLayer), 'isotp_layer must be a valid isotp.TransportLayer ' def open(self): self.exit_requested = False self._read_thread = threading.Thread( None, target=self.rxthread_task) self._read_thread.start() self.opened = True logger.info('Connection opened') return self def __enter__(self): return self def __exit__(self, type, value, traceback): self.close() def is_open(self): return self.opened def close(self): self.empty_rxqueue() self.empty_txqueue() self.exit_requested = True self._read_thread.join() self.isotp_layer.reset() self.opened = False logger.info('Connection closed') def specific_send(self, payload): if self.mtu is not None: if len(payload) > self.mtu: logger.warning( "Truncating payload to be set to a length of %d" % (self.mtu)) payload = payload[0:self.mtu] # isotp.protocol.TransportLayer uses byte array. udsoncan is strict on bytes format self.UDStoIsoTPQueue.put(bytearray(payload)) def specific_wait_frame(self, timeout=2): if not self.opened: raise RuntimeError("Connection is not open") timedout = False frame = None try: frame = self.IsoTPtoUDSQueue.get(block=True, timeout=timeout) # frame = self.CANtoIsoTPQueue.get(block=True, timeout=timeout) except queue.Empty: timedout = True if timedout: raise TimeoutException( "Did not receive frame IsoTP Transport layer in time (timeout=%s sec)" % timeout) if self.mtu is not None: if frame is not None and len(frame) > self.mtu: logger.warning( "Truncating received payload to a length of %d" % (self.mtu)) frame = frame[0:self.mtu] # isotp.protocol.TransportLayer uses bytearray. udsoncan is strict on bytes format return bytes(frame) def empty_rxqueue(self): while not self.IsoTPtoUDSQueue.empty(): self.IsoTPtoUDSQueue.get() def empty_txqueue(self): while not self.UDStoIsoTPQueue.empty(): self.UDStoIsoTPQueue.get() def rxthread_task(self): while not self.exit_requested: try: # self.logger.debug("UDStoIsoTPQueue queue size is now %d" % ( # self.UDStoIsoTPQueue.qsize())) while not self.UDStoIsoTPQueue.empty(): self.isotp_layer.send(self.UDStoIsoTPQueue.get()) self.isotp_layer.process() while self.isotp_layer.available(): self.IsoTPtoUDSQueue.put(self.isotp_layer.recv()) # self.logger.debug("IsoTPtoUDSQueue queue size is now %d" % ( # self.IsoTPtoUDSQueue.qsize())) # time.sleep(self.isotp_layer.sleep_time()) time.sleep(0.0001) except Exception as e: self.exit_requested = True logger.error(str(e)) print("Error occurred while read CAN(FD) data!") def __init__(self, window: QtWidgets.QMainWindow): super(ShifterTool, self).__init__() # self.title("CCDiag") # self.resizable(False, False) # self.geometry(str(WIDGHT_WIDTH) + "x" + # str(WIDGHT_HEIGHT) + '+200+100') # self.protocol("WM_DELETE_WINDOW", self.Form_OnClosing) self.msgQueue = Queue() # can layer receive queue self.sendQueue = Queue() # can layer send queue self.signal = UISignals() self.shifter = ShifterClass() self.devicedescription = HardwreDevice() self.windows = QtWidgets.QMainWindow() self.UI = Ui_MainWindow() self.UI.setupUi(window) self._dev_info = None self.can_thread = threading.Thread(target=self.can_thread) self.DeviceInit() self.WidgetsInit() self.ChnInfoUpdate(self._isOpen) def DeviceInit(self): self._usbcan = None self._isOpen = Value('i', 0) # 进程之间交换can连接状态 self._isChnOpen = False self.needdisconnect = Value('i', 0) # 当前连接需要断开 # current device info self._is_canfd = False self._res_support = False # read can/canfd message thread # self._read_thread = None self._terminated = False self._lock = threading.RLock() self.isotp_params = { # Will request the sender to wait 32ms between consecutive frame. 0-127ms or 100-900ns with values from 0xF1-0xF9 'stmin': 32, # Request the sender to send 8 consecutives frames before sending a new flow control message 'blocksize': 8, # Number of wait frame allowed before triggering an error 'wftmax': 0, # Link layer (CAN layer) works with 8 byte payload (CAN 2.0) 'tx_data_length': 8, # Will pad all transmitted CAN messages with byte 0x00. None means no padding 'tx_padding': 0, # Triggers a timeout if a flow control is awaited for more than 1000 milliseconds 'rx_flowcontrol_timeout': 1000, # Triggers a timeout if a consecutive frame is awaited for more than 1000 milliseconds 'rx_consecutive_frame_timeout': 1000, # When sending, respect the stmin requirement of the receiver. If set to True, go as fast as possible. 'squash_stmin_requirement': False } self._isotpaddr_PHYS = isotp.Address( isotp.AddressingMode.Normal_11bits, txid=self.shifter.canid.phy_rxId, rxid=self.shifter.canid.phy_txId) self._isotpaddr_FUNC = isotp.Address( isotp.AddressingMode.Normal_11bits, txid=self.shifter.canid.fun_rxId, rxid=self.shifter.canid.phy_txId) # self._isotpaddr_EPS = isotp.Address( # isotp.AddressingMode.Normal_11bits, txid=EPS_RX_ID_PHYS, rxid=EPS_TX_ID) # self._isotpaddr_EPS4wd = isotp.Address( # isotp.AddressingMode.Normal_11bits, txid=EPS4wd_RX_ID_PHYS, rxid=EPS4wd_TX_ID) self.isotp_layer = isotp.TransportLayer( rxfn=self.isotp_rcv, txfn=self.isotp_send, address=self._isotpaddr_PHYS, params=self.isotp_params) self.conn = ShifterTool.IsoTpConnection(isotp_layer=self.isotp_layer) self.udsclient = Client(self.conn, request_timeout=2) self.udsclient.config['security_algo'] = self.SecAlgo self.udsclient.config['security_algo_params'] = [ 0x4FE87269, 0x6BC361D8, 0x9B127D51, 0x5BA41903] self.udsclient.config['data_identifiers'] = self.shifter.did_config self.udsclient.config['server_address_format'] = 32 self.udsclient.config['server_memorysize_format'] = 32 def SecAlgo(self, level, seed, params): """ Builds the security key to unlock a security level. temp_key = bytearray(seed) self.output_key = bytearray(seed) xorkey = bytearray(params['xorkey']) for i in range(len(temp_key)): temp_key[i] = temp_key[i] ^ xorkey[i] self.output_key[0] = (temp_key[3] & 0x0F) | (temp_key[2] & 0xF0) self.output_key[1] = ((temp_key[2] & 0x1F) << 3) | ( (temp_key[1] & 0xF8) >> 3) self.output_key[2] = ((temp_key[1] & 0xFC) >> 2) | (temp_key[0] & 0xC0) self.output_key[3] = ((temp_key[0] & 0x0F) << 4) | (temp_key[3] & 0x0F) """ temp_key = (seed[0] << 24) | ( seed[1] << 16) | (seed[2] << 8) | (seed[3]) if level == 0x01: output_key_temp = ((((temp_key >> 4) ^ temp_key) << 3) ^ temp_key) & 0xFFFFFFFF elif level == 0x11: _temp_y = ((temp_key << 24) & 0xFF000000) + ((temp_key << 8) & 0xFF0000) + ((temp_key >> 8) & 0xFF00) + ((temp_key >> 24) & 0xFF) _temp_z = 0 _temp_sum = 0 for i in range(64): _temp_y += ((((_temp_z << 4) ^ (_temp_z >> 5)) + _temp_z) ^ (_temp_sum + params[_temp_sum & 0x3])) & 0xFFFFFFFF _temp_y = _temp_y & 0xFFFFFFFF _temp_sum += 0x8F750A1D _temp_sum = _temp_sum & 0xFFFFFFFF _temp_z += ((((_temp_y << 4) ^ (_temp_y >> 5)) + _temp_y) ^ (_temp_sum + params[(_temp_sum >> 11) & 0x3])) & 0xFFFFFFFF _temp_z = _temp_z & 0xFFFFFFFF output_key_temp = (((_temp_z << 24) & 0xFF000000) | ((_temp_z << 8) & 0xFF0000) | ( (_temp_z >> 8) & 0xFF00) | ((_temp_z >> 24) & 0xFF)) else: output_key_temp = temp_key output_key = struct.pack('BBBB', (output_key_temp >> 24) & 0xFF, ( output_key_temp >> 16) & 0xFF, (output_key_temp >> 8) & 0xFF, output_key_temp & 0xFF) return output_key def getDateTimeBytes(self): """ get year/month/day and convert into bytes """ _year_high = int(str(datetime.datetime.now().year), 16) >> 8 _year_low = int(str(datetime.datetime.now().year), 16) & 0xFF _month = int(str(datetime.datetime.now().month), 16) _day = int(str(datetime.datetime.now().day), 16) _hour = int(str(datetime.datetime.now().hour), 16) _minute = int(str(datetime.datetime.now().minute), 16) _second = int(str(datetime.datetime.now().second), 16) return (_year_high, _year_low, _month, _day, _hour, _minute, _second) def isotp_rcv(self): '''receive msg from can layer use queue read from can layer listen''' can_msgs = None if not self.conn.CANtoIsoTPQueue.empty(): can_msgs = self.conn.CANtoIsoTPQueue.get() # can_num = self.conn.CANtoIsoTPQueue.size() # # can_num = self._usbcan.GetReceiveNum(self.devicedescription.channel) # if can_num and not self._terminated: # read_cnt = MAX_RCV_NUM if can_num >= MAX_RCV_NUM else can_num # for i in range(read_cnt): # can_msgs[i] = self.conn.CANtoIsoTPQueue.get() # else: # can_msgs = None return can_msgs def isotp_send(self, isotp_msg): '''send isotp message to can layer use queue send''' msg = Message() msg.arbitration_id = isotp_msg.arbitration_id msg.dlc = isotp_msg.dlc msg.data = isotp_msg.data msg.is_extended_id = False # for i in range(isotp_msg.dlc): # msg.data[i] = isotp_msg.data[i] self.sendQueue.put(msg) def WidgetsInit(self): # self.UI.setupUi(self) self.UI.comboBox_2.addItem('USBCAN-I') self.UI.comboBox_2.addItem('USBCAN-II') # 波特率 self.UI.comboBox_3.addItems([('%dK' % (i/1000)) for i in TIMING_DICT.keys()]) # CHANNEL self.UI.comboBox_5.addItems(['CH1/3', 'CH2/4']) self.UI.pushButton.clicked.connect(self.open_close) self.UI.comboBox_3.activated.connect(self.update_HardwareDevice) self.UI.comboBox_5.activated.connect(self.update_HardwareDevice) self.signal.sig_Disp_str.connect(self.disp_string) self.signal.sig_MsgReceived.connect(self._updateRootList) self.initUDSUI() def initUDSUI(self): self.UI.comboBox.addItems(['0x01默认模式', '0x02编程模式', '0x03编程模式']) self.UI.comboBox_8.addItems(['0x01硬件复位', '0x03软件复位']) self.UI.comboBox_9.addItems(['0x00使能收发', '0x01能收禁发', '0x03禁止收发']) self.UI.radioButton.toggled.connect(self.TestPresentEvent) def _formatMsgData(self, index, item, received): '''msg data to list Arguments: index {int} -- msg index item {} -- recevive received {-bool} -- always true Returns: [list] -- data list: [index, received, TimeStamp, id, RemoteFlag, ExternFlag, DataLen, data] ''' data = [] if received: data.append('{0:0>4}'.format(index)) data.append(item.timestamp) data.append('接收') else: data.append('') data.append(item.timestamp) data.append('发送') data.append(str.upper(str(hex(item.arbitration_id))).replace('X', 'x')) data.append(item.dlc) if int(item.is_extended_id) == 1: data.append('扩展帧') else: data.append('标准帧') data.append(' '.join(['0x' + '{:0<2x}'.format(a).upper() for a in list(item.data)])) return data def can_thread_stop(self): self.can_thread.join() def can_thread_start(self): self.can_thread.start() def can_thread(self): while self._isOpen.value == 1: time.sleep(0.01) if self.needdisconnect.value == 1: ret = self._usbcan.CloseDevice() if ret == 1: self.needdisconnect.value = 0 self._isOpen.value = 0 else: msg, num = self._usbcan.Receive(len=10) if not num == 0: for i in range(num): if msg[i].arbitration_id == self.shifter.canid.phy_txId: self.conn.CANtoIsoTPQueue.put( msg[i]) # send msg to isotp self.msgQueue.put(msg[i]) # send signal to update screen logger.info('Rx: ID=0x{:0<3x} '.format(msg[i].arbitration_id) + ' '.join(['0x' + '{:0<2x}'.format(a).upper() for a in list(msg[i].data)])) self.signal.sig_MsgReceived.emit(num) msgToSendCnt = self.sendQueue.qsize() if msgToSendCnt > 0: msg = Message() for i in range(msgToSendCnt): msg = self.sendQueue.get() logger.info('Tx: ID=0x{:0<3x} '.format( msg.arbitration_id)+'DLC={} '.format(msg.dlc)+'externd flag ={} '.format(msg.is_extended_id)+'remote frame:{} '.format(msg.is_remote_frame)) self._usbcan.send(msg) def open_close(self): if self._isOpen.value == 0: if self._usbcan is None: self.update_HardwareDevice() can_filters = [ {'can_id': 0x420, 'can_mask': 0xFFFFFFFF}] bitrate = list(TIMING_DICT.keys())[ self.devicedescription.baudrate] self._usbcan = USBCAN(device_type=4, device_index=0, can_index=0, bitrate=bitrate, can_filters=can_filters) if not self._usbcan.InitAndStart(): logger.info("Open usbcan device fail.") self._isOpen.value = 1 self._deviceOpenUpdate() self.can_thread_start() self.conn.open() # start iso tp thread else: self._usbcan.InitAndStart() self._isOpen.value = 1 self._deviceOpenUpdate() else: self.needdisconnect.value = 1 self._deviceOpenUpdate() def update_HardwareDevice(self): self.devicedescription.device_type = self.UI.comboBox_2.currentIndex()+3 self.devicedescription.channel = self.UI.comboBox_5.currentIndex() self.devicedescription.baudrate = self.UI.comboBox_3.currentIndex() def _deviceOpenUpdate(self): if self._isOpen.value == 1: self.UI.pushButton.setText("Close CAN") elif self.needdisconnect.value == 1: self.UI.pushButton.setText("Open CAN") # set ui info def _StartlistenMsgProcess(self): self.msgProcess = Process( name='pyUSBCANListener', target=self._usbcan.ListeningMsg, args=(self._isOpen, self.needdisconnect, self.msgQueue, self.sendQueue)) self.msgProcess.daemon = True self.msgProcess.start() # 1.5s后检测连接状态，该值可能需要标定 # self.root.after(1500, func=self._checkConnectStatus) while self._isOpen.value == 1: self._updateRootList() time.sleep(0.1) def disp_string(self, out): self.UI.statusbar.showMessage(str(out)) def _updateRootList(self): _dataSize = self.msgQueue.qsize() receiveNum = 0 formateddata = list() if _dataSize > 5: _dataSize = 5 for i in range(_dataSize): receiveNum += 1 msg = self.msgQueue.get() formateddata.append(self._formatMsgData( receiveNum, msg, True)) # return a data list self._insertDataSmooth(data=formateddata, datasize=_dataSize) def _insertDataSmooth(self, data, datasize): # row = 6-datasize # for row in range(datasize): for row in range(datasize): insertdata = data[row] if insertdata[3] == '0x420': row = 0 elif insertdata[3] == "0x77a": row = 1 for column in range(7): item = QtWidgets.QTableWidgetItem() item.setTextAlignment(0x84) # 文本显示位置 item.setText(str(insertdata[column])) self.UI.tableWidget.setItem(row, column, item) def ChnInfoUpdate(self, openflag): pass def TestPresentEvent(self): self.udsclient.tester_present()