else: flag_16 += 1 """Calcolo della probabilità a posteriori""" if flag_calc_belief: last_status = status_buf.get_last_status() dict_evidence = last_status.get_dict_by_keys( list_evidence_node) start = timer() pred, max_pred_class = ids.predict(dict_evidence) end = timer() print("classificazione numero: {}".format(cont_bayes), file=log_file) print("exec time: {}".format(end - start), file=log_file) print("status rete--------------------------", file=log_file) print(last_status, file=log_file) print("predizione---------------------------", file=log_file) print(pred[attack_index], file=log_file) if max_pred_class[attack_index] == '(0.5-inf)': print("--------------attacco riconosciuto-------------") print("--------------attacco riconosciuto-------------", file=log_file) bus.send(ids_msg) flag_calc_belief = False cont_bayes += 1 finally: bus.shutdown() log_file.close()
def handle_enable_motors(req): # set up connection to hardware can.rc['interface'] = "kvaser" can.rc['channel'] = '0' can.rc['bitrate'] = 500000 bus = Bus() # Clear fault commands for i in range(1, 8): msg = can.Message( arbitration_id=0x600 + i, data=[int("40", 16), int("41", 16), int("60", 16), 0, 0, 0, 0, 0], is_extended_id=False) bus.send(msg) # Start remote node via NMT # different commands can be used to set operation mode (pre-op, start, etc). For all of them the # Cob Id is 0 in NMT mode. Data has two bytes. First byte is a desired command, one of # the five following commands can be used # 80 is pre-operational # 81 is reset node # 82 is reset communication # 01 is start # 02 is stop # second byte is node id, can be 0 (all nodes) or a number between 1 to 256. for i in range(1, 8): msg = can.Message(arbitration_id=0x0, data=[0x01, int("0%d\n" % i, 16)], is_extended_id=False) bus.send(msg) # Enable All Motors for i in range(1, 8): msg = can.Message(arbitration_id=0x220 + i, data=[0x00, 0x00], is_extended_id=False) bus.send(msg) for i in range(1, 8): msg = can.Message(arbitration_id=0x220 + i, data=[0x06, 0x00], is_extended_id=False) bus.send(msg) for i in range(1, 8): msg = can.Message(arbitration_id=0x220 + i, data=[0x0F, 0x00], is_extended_id=False) bus.send(msg) # Set all motors to position mode # If you want to set the motors to velocity mode change # the comand from [hex2dec('0F') hex2dec('0') hex2dec('01')] to # [hex2dec('0F') hex2dec('0') hex2dec('03')]; for i in range(1, 8): msg = can.Message(arbitration_id=0x320 + i, data=[0x0F, 0x00, 0x01], is_extended_id=False) bus.send(msg) # Set rotational speed of motors for i in range(2, 8): msg = can.Message( arbitration_id=0x600 + i, data=[0x22, 0x81, 0x60, 0x0, 0x88, 0x13, 0x0, 0x0], # 5000 rpm is_extended_id=False) bus.send(msg) # motor 1 acceleration and deceleration to 100,000, velocity to 10,000 msg = can.Message( arbitration_id=0x601, data=[0x22, 0x81, 0x60, 0x0, 0x10, 0x27, 0x0, 0x0], # is_extended_id=False) bus.send(msg) msg = can.Message( arbitration_id=0x601, data=[0x22, 0x83, 0x60, 0x0, 0xA0, 0x86, 0x01, 0x0], # is_extended_id=False) bus.send(msg) msg = can.Message( arbitration_id=0x601, data=[0x22, 0x84, 0x60, 0x0, 0xA0, 0x86, 0x01, 0x0], # is_extended_id=False) bus.send(msg) bus.shutdown() print("Motors Enabled") return EnableMotorsResponse(req.a)
class Bus(BusABC): """ A CAN Bus that implements the J1939 Protocol. :param list j1939_filters: a list of dictionaries that specify filters that messages must match to be received by this Bus. Messages can match any of the filters. Options are: * :pgn: An integer PGN to show """ channel_info = "j1939 bus" def __init__(self, pdu_type=PDU, broadcast=True, *args, **kwargs): logger.debug("Creating a new j1939 bus") #self.rx_can_message_queue = Queue() self.queue = Queue() self.node_queue_list = [] # Start with nothing super(Bus, self).__init__(kwargs.get('channel'), kwargs.get('can_filters')) self._pdu_type = pdu_type self.timeout = 1 self._long_message_throttler = threading.Thread( target=self._throttler_function) self._long_message_throttler.daemon = True self._incomplete_received_pdus = {} self._incomplete_received_pdu_lengths = {} self._incomplete_transmitted_pdus = {} self._long_message_segment_queue = Queue(0) self._key_generation_fcn = None if 'keygen' in kwargs and kwargs['keygen'] is not None: self._key_generation_fcn = kwargs['keygen'] if 'ignoreCanSendError' in kwargs and kwargs[ 'ignoreCanSendError'] is not None: self._ignore_can_send_error = kwargs['ignoreCanSendError'] if broadcast: self.node_queue_list = [ (None, self) ] # Start with default logger Queue which will receive everything # Convert J1939 filters into Raw Can filters if 'j1939_filters' in kwargs and kwargs['j1939_filters'] is not None: filters = kwargs.pop('j1939_filters') logger.debug("Got filters: {}".format(filters)) can_filters = [] for filt in filters: can_id, can_mask = 0, 0 if 'pgn' in filt: can_id = filt['pgn'] << 8 # The pgn needs to be left shifted by 8 to ignore the CAN_ID's source address # Look at most significant 4 bits to determine destination specific if can_id & 0xF00000 == 0xF00000: logging.info("PDU2 (broadcast message)") can_mask = 0xFFFF00 else: logging.info("PDU1 (p2p)") can_mask = 0xFF0000 if 'source' in filt: # filter by source can_mask |= 0xFF can_id |= filt['source'] logger.info("added source", filt) logger.info("Adding CAN ID filter: {:0x}:{:0x}".format( can_id, can_mask)) can_filters.append({"can_id": can_id, "can_mask": can_mask}) kwargs['can_filters'] = can_filters if 'timeout' in kwargs and kwargs['timeout'] is not None: if isinstance(kwargs['timeout'], (int, float)): self.timeout = kwargs['timeout'] else: raise ValueError("Bad timeout type") logger.debug("Creating a new can bus") self.can_bus = RawCanBus(*args, **kwargs) canListener = j1939Listner(self.notification) self.can_notifier = canNotifier(self.can_bus, [canListener], timeout=self.timeout) self._long_message_throttler.start() def notification(self, inboundMessage): #self.rx_can_message_queue.put(inboundMessage) if self.can_notifier._running is False: logger.info('Aborting message %s bus is not running', inboundMessage) if isinstance(inboundMessage, Message): logger.info('\n\nnotification: Got a Message from CAN: %s' % inboundMessage) if inboundMessage.id_type: # Extended ID # Only J1939 messages (i.e. 29-bit IDs) should go further than this point. # Non-J1939 systems can co-exist with J1939 systems, but J1939 doesn't care # about the content of their messages. logger.debug('notification: Message is j1939 msg') # # Need to determine if it's a broadcase message or # limit to listening nodes only # arbitration_id = ArbitrationID() arbitration_id.can_id = inboundMessage.arbitration_id logger.debug('notification: ArbitrationID = %s' % (arbitration_id)) for (node, l_notifier) in self.node_queue_list: logger.debug("notification: node=%s" % (node)) logger.debug(" notifier=%s" % (l_notifier)) logger.debug(" arbitration_id.pgn=%s" % (arbitration_id.pgn)) logger.debug(" destination_address=%s" % (arbitration_id.destination_address)) # redirect the AC stuff to the node processors. the rest can go # to the main queue. if node and (arbitration_id.pgn in [ PGN_AC_ADDRESS_CLAIMED, PGN_AC_COMMANDED_ADDRESS, PGN_REQUEST_FOR_PGN ]): logger.info("notification: sending to notifier queue") # send the PDU to the node processor. l_notifier.queue.put(inboundMessage) # if node has the destination address, do something with the PDU elif node and (arbitration_id.destination_address in node.address_list): rx_pdu = self._process_incoming_message(inboundMessage) if rx_pdu: logger.info( "WP02: notification: sent to general queue: %s QQ=%s" % (rx_pdu, self.queue)) self.queue.put(rx_pdu) elif node and (arbitration_id.destination_address is None): logger.info( "notification: sending broadcast to general queue") rx_pdu = self._process_incoming_message(inboundMessage) logger.info( "WP01: notification: sent broadcast to general queue: %s QQ=%s" % (rx_pdu, self.queue)) self.queue.put(rx_pdu) elif node is None: # always send the message to the logging queue logger.info("notification: sending to general queue") rx_pdu = self._process_incoming_message(inboundMessage) logger.info( "WP03: notification: sent pdu [%s] to general queue" % rx_pdu) self.queue.put(rx_pdu) else: logger.info("WP04: notification: pdu dropped: %s\n\n" % inboundMessage) else: logger.info("Received non J1939 message (ignoring)") def connect(self, node): """ Attach a listening node (with a dest address) to the J1939 bus """ logger.debug("connect: type(node)=%s, node=%s" % (type(node), node)) if not isinstance(node, Node): raise ValueError( "bad parameter for node, must be a J1939 node object") notifier = Notifier(Queue(), node.on_message_received, timeout=None) self.node_queue_list.append((node, notifier)) def recv(self, timeout=None): #logger.debug("Waiting for new message") #logger.debug("Timeout is {}".format(timeout)) logger.debug('J1939 Bus recv(), waiting on QQ=%s with timeout %s' % (self.queue, timeout)) try: #m = self.rx_can_message_queue.get(timeout=timeout) rx_pdu = self.queue.get(timeout=timeout) logger.info('J1939 Bus recv() successful QQ=%s, pdu:%s' % (self.queue, rx_pdu)) return rx_pdu except Empty: logger.debug('J1939 Bus recv() timed out' % ()) return None # TODO: Decide what to do with CAN errors # if m.is_error_frame: # logger.warning("Appears we got an error frame!") # # rx_error = CANError(timestamp=m.timestamp) # if rx_error is not None: # logger.info('Sending error "%s" to registered listeners.' % rx_error) # for listener in self.listeners: # if hasattr(listener, 'on_error_received'): # listener.on_error_received(rx_error) def send(self, msg, timeout=None): logger.info("j1939.send: msg=%s" % msg) messages = [] if len(msg.data) > 8: logger.info("j1939.send: message is > than 8 bytes") # Making a copy of the PDU so that the original # is not altered by the data padding. pdu = copy.deepcopy(msg) pdu.data = bytearray(pdu.data) logger.info("j1939.send: Copied msg = %s" % pdu) pdu_length_lsb, pdu_length_msb = divmod(len(pdu.data), 256) while len(pdu.data) % 7 != 0: pdu.data += b'\xFF' logger.info("j1939.send: padded msg (mod 7) = %s" % pdu) logger.info("MIL8:---------------------") # # segment the longer message into 7 byte segments. We need to prefix each # data[0] with a sequence number for the transfer # for i, segment in enumerate(pdu.data_segments(segment_length=7)): arbitration_id = copy.deepcopy(pdu.arbitration_id) arbitration_id.pgn.value = PGN_TP_DATA_TRANSFER logger.info( "MIL8: j1939.send: i=%d, pdu.arbitration_id.pgn.is_destination_specific=%d, data=%s" % (i, pdu.arbitration_id.pgn.is_destination_specific, segment)) if pdu.arbitration_id.pgn.is_destination_specific and \ pdu.arbitration_id.destination_address != DESTINATION_ADDRESS_GLOBAL: arbitration_id.pgn.pdu_specific = pdu.arbitration_id.pgn.pdu_specific else: arbitration_id.pgn.pdu_specific = DESTINATION_ADDRESS_GLOBAL arbitration_id.destination_address = DESTINATION_ADDRESS_GLOBAL logger.info("MIL8: j1939.send: segment=%d, arb = %s" % (i, arbitration_id)) message = Message(arbitration_id=arbitration_id.can_id, extended_id=True, dlc=(len(segment) + 1), data=(bytearray([i + 1]) + segment)) messages.append(message) # # At this point we have the queued messages sequenced in 'messages' # logger.info( "MIL8: j1939.send: is_destination_specific=%d, destAddr=%s" % (pdu.arbitration_id.pgn.is_destination_specific, pdu.arbitration_id.destination_address)) logger.info("MIL8: j1939.send: messages=%s" % messages) if pdu.arbitration_id.pgn.is_destination_specific and \ pdu.arbitration_id.destination_address != DESTINATION_ADDRESS_GLOBAL: destination_address = pdu.arbitration_id.pgn.pdu_specific if pdu.arbitration_id.source_address in self._incomplete_transmitted_pdus: if destination_address in self._incomplete_transmitted_pdus[ pdu.arbitration_id.source_address]: logger.warning( "Duplicate transmission of PDU:\n{}".format(pdu)) else: self._incomplete_transmitted_pdus[ pdu.arbitration_id.source_address] = {} # append the messages to the 'incomplete' list self._incomplete_transmitted_pdus[ pdu.arbitration_id. source_address][destination_address] = messages else: destination_address = DESTINATION_ADDRESS_GLOBAL logger.debug("MIL8: rts arbitration id: src=%s, dest=%s" % (pdu.source, destination_address)) rts_arbitration_id = ArbitrationID( pgn=PGN_TP_CONNECTION_MANAGEMENT, source_address=pdu.source, destination_address=destination_address) rts_arbitration_id.pgn.pdu_specific = pdu.arbitration_id.pgn.pdu_specific temp_pgn = copy.deepcopy(pdu.arbitration_id.pgn) if temp_pgn.is_destination_specific: temp_pgn.value -= temp_pgn.pdu_specific pgn_msb = ((temp_pgn.value & 0xFF0000) >> 16) pgn_middle = ((temp_pgn.value & 0x00FF00) >> 8) pgn_lsb = (temp_pgn.value & 0x0000FF) if pdu.arbitration_id.pgn.is_destination_specific and \ pdu.arbitration_id.destination_address != DESTINATION_ADDRESS_GLOBAL: # send request to send logger.debug("MIL8: rts to specific dest: src=%s, dest=%s" % (pdu.source, destination_address)) rts_msg = Message(extended_id=True, arbitration_id=rts_arbitration_id.can_id, data=[ CM_MSG_TYPE_RTS, pdu_length_msb, pdu_length_lsb, len(messages), 0xFF, pgn_lsb, pgn_middle, pgn_msb ], dlc=8) try: logger.info("MIL08: j1939.send: sending TP.RTS to %s: %s" % (destination_address, rts_msg)) self.can_bus.send(rts_msg) except CanError: if self._ignore_can_send_error: pass raise else: rts_arbitration_id.pgn.pdu_specific = DESTINATION_ADDRESS_GLOBAL rts_arbitration_id.destination_address = DESTINATION_ADDRESS_GLOBAL logger.debug("MIL8: rts to Global dest: src=%s, dest=%s" % (pdu.source, destination_address)) bam_msg = Message( extended_id=True, arbitration_id=rts_arbitration_id.can_id | pdu.source, data=[ CM_MSG_TYPE_BAM, pdu_length_msb, pdu_length_lsb, len(messages), 0xFF, pgn_lsb, pgn_middle, pgn_msb ], dlc=8) bam_msg.destination_address = DESTINATION_ADDRESS_GLOBAL # bam_msg.arbitration_id.destination_address = DESTINATION_ADDRESS_GLOBAL # send BAM try: logger.info("j1939.send: sending TP.BAM to %s: %s" % (destination_address, bam_msg)) self.can_bus.send(bam_msg) time.sleep(0.05) except CanError: if self._ignore_can_send_error: pass raise for message in messages: # send data messages - no flow control, so no need to wait # for receiving devices to acknowledge logger.info("j1939.send: queue TP.BAM data to %s: %s" % (destination_address, message)) self._long_message_segment_queue.put_nowait(message) else: msg.display_radix = 'hex' logger.debug("j1939.send: calling can_bus_send: j1939-msg: %s" % (msg)) can_message = Message(arbitration_id=msg.arbitration_id.can_id, extended_id=True, dlc=len(msg.data), data=msg.data) logger.debug("j1939.send: calling can_bus_send: can-msg: %s" % can_message) try: self.can_bus.send(can_message) except CanError: if self._ignore_can_send_error: pass raise def shutdown(self): self.can_notifier._running = False self.can_bus.shutdown() #self.j1939_notifier.running.clear() super(Bus, self).shutdown() def _send_key_response(self, pdu): logger.info("PI04: _send_key_response src=%d, pdu=%s" % (pdu.source, pdu)) src = pdu.destination dest = pdu.source logger.info("PI05: new PDU, src=%d, dest=%d" % (src, dest)) pdu.destination = dest logger.info("PI05: new PDU.dest = %d" % (pdu.destination)) pdu.source = src logger.info("PI06: newPDU = %s" % (pdu)) logger.info("PI04: _send_key_response src/dest flipped pdu=%s" % (pdu)) assert (pdu.data[0] == 4) # only support long key for now data = pdu.data assert (len(data) == 8) seed = (data[5] << 24) + (data[4] << 16) + (data[3] << 8) + data[2] if self._key_generation_fcn is None: return None key = self._key_generation_fcn(seed) logger.info( "PI03: _send_key_response Seed: 0x%08x yields key: 0x%08x" % (seed, key)) data[5] = (key >> 24) & 0xff data[4] = (key >> 16) & 0xff data[3] = (key >> 8) & 0xff data[2] = (key) & 0xff data[1] = 1 pdu.data = data self.send(pdu) return None def _process_incoming_message(self, msg): logger.info( "PI01: Processing incoming message: instance=%s\n msg= %s" % (self, msg)) arbitration_id = ArbitrationID() arbitration_id.can_id = msg.arbitration_id if arbitration_id.pgn.is_destination_specific: arbitration_id.pgn.value -= arbitration_id.pgn.pdu_specific pdu = self._pdu_type(timestamp=msg.timestamp, data=msg.data, info_strings=[]) pdu.arbitration_id.can_id = msg.arbitration_id pdu.info_strings = [] pdu.radix = 16 logger.info("PI02: arbitration_id.pgn.value == 0x%04x" % arbitration_id.pgn.value) if arbitration_id.pgn.value == PGN_TP_CONNECTION_MANAGEMENT: logger.info("PGN_TP_CONNECTION_MANAGEMENT") retval = self._connection_management_handler(pdu) elif arbitration_id.pgn.value == PGN_TP_DATA_TRANSFER: logger.info("PGN_TP_DATA_TRANSFER") retval = self._data_transfer_handler(pdu) elif (arbitration_id.pgn.value == PGN_TP_SEED_REQUEST) and (self._key_generation_fcn is not None): logger.info("PGN_TP_SEED_REQUEST") retval = self._send_key_response(pdu) else: logger.info("PGN_PDU") retval = pdu logger.info("_process_incoming_message: returning %s" % (retval)) return retval def _connection_management_handler(self, msg): logger.debug("MP00: _connection_management_handler: %s, cmd=%s" % (msg, msg.data[0])) if len(msg.data) == 0: msg.info_strings.append( "Invalid connection management message - no data bytes") return msg cmd = msg.data[0] retval = None if cmd == CM_MSG_TYPE_RTS: retval = self._process_rts(msg) elif cmd == CM_MSG_TYPE_CTS: retval = self._process_cts(msg) elif cmd == CM_MSG_TYPE_EOM_ACK: retval = self._process_eom_ack(msg) elif cmd == CM_MSG_TYPE_BAM: retval = self._process_bam(msg) elif cmd == CM_MSG_TYPE_ABORT: retval = self._process_abort(msg) logger.debug("_connection_management_handler: returning %s" % (retval)) return retval def _data_transfer_handler(self, msg): logger.debug("_data_transfer_handler:") msg_source = msg.arbitration_id.source_address pdu_specific = msg.arbitration_id.pgn.pdu_specific if msg_source in self._incomplete_received_pdus: logger.debug("in self._incomplete_received_pdus:") if pdu_specific in self._incomplete_received_pdus[msg_source]: logger.debug("in self._incomplete_received_pdus[msg_source]:") self._incomplete_received_pdus[msg_source][ pdu_specific].data.extend(msg.data[1:]) total = self._incomplete_received_pdu_lengths[msg_source][ pdu_specific]["total"] if len(self._incomplete_received_pdus[msg_source] [pdu_specific].data) >= total: logger.debug( "allReceived: %s" % type(self._incomplete_received_pdus[msg_source])) logger.debug("allReceived: %s" % pprint.pformat( self._incomplete_received_pdus[msg_source])) logger.debug( "allReceived: %s from 0x%x" % (type(self._incomplete_received_pdus[msg_source] [pdu_specific]), pdu_specific)) logger.debug("allReceived: %s" % (self._incomplete_received_pdus[msg_source] [pdu_specific])) if pdu_specific == DESTINATION_ADDRESS_GLOBAL: logger.debug( "pdu_specific == DESTINATION_ADDRESS_GLOBAL") # Looks strange but makes sense - in the absence of explicit flow control, # the last CAN packet in a long message *is* the end of message acknowledgement return self._process_eom_ack(msg) # Find a Node object so we can search its list of known node addresses for this node # so we can find if we are responsible for sending the EOM ACK message # TODO: Was self.j1939_notifier.listeners send_ack = any( True for (_listener, l_notifier) in self.node_queue_list if isinstance(_listener, Node) and ( _listener.address == pdu_specific or pdu_specific in _listener.address_list)) #send_ack = any(True for l in self.can_notifier.listeners # if isinstance(l, Node) and (l.address == pdu_specific or # pdu_specific in l.address_list)) if send_ack: arbitration_id = ArbitrationID() arbitration_id.pgn.value = PGN_TP_CONNECTION_MANAGEMENT arbitration_id.pgn.pdu_specific = msg_source arbitration_id.source_address = pdu_specific arbitration_id.destination_address = 0x17 total_length = self._incomplete_received_pdu_lengths[ msg_source][pdu_specific]["total"] _num_packages = self._incomplete_received_pdu_lengths[ msg_source][pdu_specific]["num_packages"] pgn = self._incomplete_received_pdus[msg_source][ pdu_specific].arbitration_id.pgn pgn_msb = ((pgn.value & 0xFF0000) >> 16) _pgn_middle = ((pgn.value & 0x00FF00) >> 8) _pgn_lsb = 0 logger.debug( "in send_ack: arbitration_id=[%s], can_id=[%x], destAdder=0x%0x" % (arbitration_id, int(arbitration_id.can_id), arbitration_id.destination_address)) logger.debug("in send_ack: " % ()) div, mod = divmod(total_length, 256) can_message = Message( arbitration_id=arbitration_id.can_id, extended_id=True, dlc=8, data=[ CM_MSG_TYPE_EOM_ACK, mod, # total_length % 256, div, # total_length / 256, _num_packages, 0xFF, _pgn_lsb, _pgn_middle, pgn_msb ]) try: self.can_bus.send(can_message) except CanError: if self._ignore_can_send_error: pass raise logger.debug("_data_transfer_handler: returning %s" % (msg)) return self._process_eom_ack(msg) def _process_rts(self, msg): logger.debug("process_rts, source=0x%x" % (msg.arbitration_id.source_address)) if msg.arbitration_id.source_address not in self._incomplete_received_pdus: self._incomplete_received_pdus[ msg.arbitration_id.source_address] = {} self._incomplete_received_pdu_lengths[ msg.arbitration_id.source_address] = {} # Delete any previous messages that were not finished correctly if msg.arbitration_id.pgn.pdu_specific in self._incomplete_received_pdus[ msg.arbitration_id.source_address]: del self._incomplete_received_pdus[ msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific] del self._incomplete_received_pdu_lengths[ msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific] if msg.data[0] == CM_MSG_TYPE_BAM: logger.debug("CM_MSG_TYPE_BAM") self._incomplete_received_pdus[ msg.arbitration_id.source_address][0xFF] = self._pdu_type() self._incomplete_received_pdus[msg.arbitration_id.source_address][ 0xFF].arbitration_id.pgn.value = int( ("%.2X%.2X%.2X" % (msg.data[7], msg.data[6], msg.data[5])), 16) if self._incomplete_received_pdus[ msg.arbitration_id.source_address][ 0xFF].arbitration_id.pgn.is_destination_specific: self._incomplete_received_pdus[msg.arbitration_id.source_address][ 0xFF].arbitration_id.pgn.pdu_specific = msg.arbitration_id.pgn.pdu_specific self._incomplete_received_pdus[msg.arbitration_id.source_address][ 0xFF].arbitration_id.source_address = msg.arbitration_id.source_address self._incomplete_received_pdus[ msg.arbitration_id.source_address][0xFF].data = [] _message_size = int("%.2X%.2X" % (msg.data[2], msg.data[1]), 16) self._incomplete_received_pdu_lengths[ msg.arbitration_id.source_address][0xFF] = { "total": _message_size, "chunk": 255, "num_packages": msg.data[3], } else: logger.debug("not CM_MSG_TYPE_BAM, source=0x%x" % (msg.arbitration_id.source_address)) self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific] = self._pdu_type() self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn. pdu_specific].arbitration_id.pgn.value = int( ("%.2X%.2X%.2X" % (msg.data[7], msg.data[6], msg.data[5])), 16) if self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn. pdu_specific].arbitration_id.pgn.is_destination_specific: self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn. pdu_specific].arbitration_id.pgn.pdu_specific = msg.arbitration_id.pgn.pdu_specific self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn. pdu_specific].arbitration_id.source_address = msg.arbitration_id.source_address self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific].data = [] _message_size = int("%.2X%.2X" % (msg.data[2], msg.data[1]), 16) self._incomplete_received_pdu_lengths[ msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific] = { "total": _message_size, "chunk": 255, "num_packages": msg.data[3], } if msg.data[0] != CM_MSG_TYPE_BAM: logger.debug("not CM_MSG_TYPE_BAM--2") logger.debug("self.can_notifier.listeners = %s" % self.can_notifier.listeners) logger.debug("self.node_queue_list = %s" % self.node_queue_list) #for _listener in self.can_notifier.listeners: for (_listener, l_notifier) in self.node_queue_list: logger.debug("MIL2: _listner/l_notifier = %s/%s" % (_listener, l_notifier)) if isinstance(_listener, Node): logger.debug("6, dest=0x%x" % (msg.arbitration_id.source_address)) # find a Node object so we can search its list of known node addresses # for this node - if we find it we are responsible for sending the CTS message logger.debug("MIL3: Node: %s" % (Node)) logger.debug("MIL3: _listener.address: %s" % (_listener.address)) logger.debug( "MIL3: msg.arbitration_id.pgn.pdu_specific: %s" % (msg.arbitration_id.pgn.pdu_specific)) logger.debug("MIL3: _listener.address_list: %s" % (_listener.address_list)) if _listener.address == msg.arbitration_id.pgn.pdu_specific or \ msg.arbitration_id.pgn.pdu_specific in _listener.address_list: _cts_arbitration_id = ArbitrationID( source_address=msg.arbitration_id.pgn.pdu_specific) _cts_arbitration_id.pgn.value = PGN_TP_CONNECTION_MANAGEMENT _cts_arbitration_id.pgn.pdu_specific = msg.arbitration_id.source_address _cts_arbitration_id.destination_address = msg.arbitration_id.source_address _data = [0x11, msg.data[4], 0x01, 0xFF, 0xFF] _data.extend(msg.data[5:]) logger.debug("send CTS: AID: %s" % _cts_arbitration_id) cts_msg = Message( extended_id=True, arbitration_id=_cts_arbitration_id.can_id, data=_data, dlc=8) # send clear to send logger.debug("send CTS: %s" % cts_msg) try: self.can_bus.send(cts_msg) except CanError: if self._ignore_can_send_error: pass raise return """ # # MIL: This is the wrong way around this, I should have a node assigned. # elif _listener is None and l_notifier is not None: logger.debug("7, dest=0x%x" % (msg.arbitration_id.source_address)) # find a Node object so we can search its list of known node addresses # for this node - if we find it we are responsible for sending the CTS message if msg.arbitration_id.pgn.pdu_specific : _cts_arbitration_id = ArbitrationID(source_address=msg.arbitration_id.pgn.pdu_specific) _cts_arbitration_id.pgn.value = PGN_TP_CONNECTION_MANAGEMENT _cts_arbitration_id.pgn.pdu_specific = msg.arbitration_id.source_address _cts_arbitration_id.destination_address = msg.arbitration_id.source_address _data = [0x11, msg.data[4], 0x01, 0xFF, 0xFF] _data.extend(msg.data[5:]) logger.debug("send CTS: AID: %s" % _cts_arbitration_id) cts_msg = Message(extended_id=True, arbitration_id=_cts_arbitration_id.can_id, data=_data, dlc=8) # send clear to send logger.debug("send CTS: %s" % cts_msg) self.can_bus.send(cts_msg) return """ def _process_cts(self, msg): logger.debug("_process_cts") logger.debug("MIL8: cts message is: %s" % msg) logger.debug("MIL8: len(pdu-send-buffer) = %d" % len(self._incomplete_transmitted_pdus[0][23])) if msg.arbitration_id.pgn.pdu_specific in self._incomplete_transmitted_pdus: if msg.arbitration_id.source_address in self._incomplete_transmitted_pdus[ msg.arbitration_id.pgn.pdu_specific]: # Next packet number in CTS message (Packet numbers start at 1 not 0) start_index = msg.data[2] - 1 # Using total number of packets in CTS message end_index = start_index + msg.data[1] for _msg in self._incomplete_transmitted_pdus[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id. source_address][start_index:end_index]: logger.debug("MIL8: msg=%s" % (_msg)) # TODO: Needs to be pacing if we get this working... try: # Shouldent send a J1939 PDU as a CAN Message unless we are careful canMessage = Message( arbitration_id=_msg.arbitration_id, data=_msg.data) self.can_bus.send(canMessage) except CanError: if self._ignore_can_send_error: pass raise logger.debug("MIL8: _process_cts complete") def _process_eom_ack(self, msg): logger.debug("_process_eom_ack") if (msg.arbitration_id.pgn.value - msg.arbitration_id.pgn.pdu_specific) == PGN_TP_DATA_TRANSFER: logger.debug("_process_eom_ack: PGN_TP_DATA_TRANSFER") self._incomplete_received_pdus[msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific].timestamp = msg.timestamp retval = copy.deepcopy(self._incomplete_received_pdus[ msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific]) retval.data = retval.data[:self._incomplete_received_pdu_lengths[ msg.arbitration_id.source_address][msg.arbitration_id.pgn. pdu_specific]["total"]] del self._incomplete_received_pdus[ msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific] del self._incomplete_received_pdu_lengths[ msg.arbitration_id.source_address][ msg.arbitration_id.pgn.pdu_specific] else: logger.debug("_process_eom_ack: not PGN_TP_DATA_TRANSFER") if msg.arbitration_id.pgn.pdu_specific in self._incomplete_received_pdus: if msg.arbitration_id.source_address in self._incomplete_received_pdus[ msg.arbitration_id.pgn.pdu_specific]: self._incomplete_received_pdus[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id. source_address].timestamp = msg.timestamp retval = copy.deepcopy(self._incomplete_received_pdus[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id.source_address]) retval.data = retval.data[:self. _incomplete_received_pdu_lengths[ msg.arbitration_id.pgn. pdu_specific][ msg.arbitration_id. source_address]["total"]] del self._incomplete_received_pdus[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id.source_address] del self._incomplete_received_pdu_lengths[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id.source_address] else: retval = None else: retval = None if msg.arbitration_id.pgn.pdu_specific in self._incomplete_transmitted_pdus: if msg.arbitration_id.source_address in self._incomplete_transmitted_pdus[ msg.arbitration_id.pgn.pdu_specific]: del self._incomplete_transmitted_pdus[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id.source_address] logger.debug("_process_eom_ack: returning %s" % (retval)) return retval def _process_bam(self, msg): self._process_rts(msg) def _process_abort(self, msg): if msg.arbitration_id.pgn.pdu_specific in self._incomplete_received_pdus: if msg.source in self._incomplete_received_pdus[ msg.arbitration_id.pgn.pdu_specific]: del self._incomplete_received_pdus[ msg.arbitration_id.pgn.pdu_specific][ msg.arbitration_id.source_address] def _throttler_function(self): while self.can_notifier._running: _msg = None try: _msg = self._long_message_segment_queue.get(timeout=0.1) except Empty: pass if _msg is not None: try: self.can_bus.send(_msg) time.sleep(0.05) except CanError: if self._ignore_can_send_error: pass raise @property def transmissions_in_progress(self): retval = 0 for _tx_address in self._incomplete_transmitted_pdus: retval += len(self._incomplete_transmitted_pdus[_tx_address]) for _rx_address in self._incomplete_received_pdus: retval += len(self._incomplete_received_pdus[_rx_address]) return retval
def handle_enable_motors_homing(req): # set up connection to hardware can.rc['interface'] = "kvaser" can.rc['channel'] = '0' can.rc['bitrate'] = 500000 bus = Bus() # Clear fault commands for i in range(1, 8): msg = can.Message(arbitration_id=0x600 + i, data=[int("40", 16), int("41", 16), int("60", 16), 0, 0, 0, 0, 0], is_extended_id=False) bus.send(msg) # Start remote node via NMT # different commands can be used to set operation mode (pre-op, start, etc). For all of them the # Cob Id is 0 in NMT mode. Data has two bytes. First byte is a desired command, one of # the five following commands can be used # 80 is pre-operational # 81 is reset node # 82 is reset communication # 01 is start # 02 is stop # second byte is node id, can be 0 (all nodes) or a number between 1 to 256. for i in range(1, 8): msg = can.Message(arbitration_id=0x0, data=[0x01, int("0%d\n" % i, 16)], is_extended_id=False) bus.send(msg) # Enable All Motors for i in range(1, 8): msg = can.Message(arbitration_id=0x220 + i, data=[0x00, 0x00], is_extended_id=False) bus.send(msg) for i in range(1, 8): msg = can.Message(arbitration_id=0x220 + i, data=[0x06, 0x00], is_extended_id=False) bus.send(msg) for i in range(1, 8): msg = can.Message(arbitration_id=0x220 + i, data=[0x0F, 0x00], is_extended_id=False) bus.send(msg) # Set all motors to pos mode for i in range(1, 8): msg = can.Message(arbitration_id=0x320 + i, data=[0x0F, 0x00, 0x01], is_extended_id=False) bus.send(msg) bus.shutdown() print("Motors ready for homing.") return EnableMotorsResponse(req.a)