Exemple #1
0
def main():
    can_interface = 'can1'
    bus = Bus(can_interface)

    try:
        while True:
            Message = bus.recv(0.0)
            if Message:
                check_rx(Message.arbitration_id, Message.data[0])

    except KeyboardInterrupt:
        bus.shutdown()
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, *args, **kwargs):
        logger.debug("Creating a new j1939 bus")

        self.rx_can_message_queue = Queue()

        super(Bus, self).__init__()
        self._pdu_type = pdu_type
        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)

        # 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

        logger.debug("Creating a new can bus")
        self.can_bus = RawCanBus(*args, **kwargs)
        self.can_notifier = Notifier(self.can_bus, [self.rx_can_message_queue.put])
        self.j1939_notifier = Notifier(self, [])

        self._long_message_throttler.start()

    def recv(self, timeout=None):
        logger.debug("Waiting for new message")
        logger.debug("Timeout is {}".format(timeout))
        try:
            m = self.rx_can_message_queue.get(timeout=timeout)
        except Empty:
            return

        rx_pdu = None

        if isinstance(m, Message):
            logger.debug('Got a Message: %s' % m)
            if m.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.info('Message is j1939 msg')
                rx_pdu = self._process_incoming_message(m)
            else:
                logger.info("Received non J1939 message (ignoring)")

            # 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)

        # Return to BusABC where it will get fed to any listeners
        return rx_pdu

    def send(self, msg):
        messages = []
        if len(msg.data) > 8:
            # 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)

            pdu_length_lsb, pdu_length_msb = divmod(len(pdu.data), 256)

            while len(pdu.data) % 7 != 0:
                pdu.data += b'\xFF'

            for i, segment in enumerate(pdu.data_segments(segment_length=7)):
                arbitration_id = copy.deepcopy(pdu.arbitration_id)
                arbitration_id.pgn.value = constants.PGN_TP_DATA_TRANSFER
                if pdu.arbitration_id.pgn.is_destination_specific and \
                                pdu.arbitration_id.destination_address != constants.DESTINATION_ADDRESS_GLOBAL:
                    arbitration_id.pgn.pdu_specific = pdu.arbitration_id.pgn.pdu_specific
                else:
                    arbitration_id.pgn.pdu_specific = constants.DESTINATION_ADDRESS_GLOBAL

                message = Message(arbitration_id=arbitration_id.can_id,
                                  extended_id=True,
                                  dlc=(len(segment) + 1),
                                  data=(bytearray([i + 1]) + segment))
                messages.append(message)

            if pdu.arbitration_id.pgn.is_destination_specific and \
                            pdu.arbitration_id.destination_address != constants.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] = {}
                self._incomplete_transmitted_pdus[pdu.arbitration_id.source_address][destination_address] = messages
            else:
                destination_address = constants.DESTINATION_ADDRESS_GLOBAL

            rts_arbitration_id = ArbitrationID(source_address=pdu.source)
            rts_arbitration_id.pgn.value = constants.PGN_TP_CONNECTION_MANAGEMENT
            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 != constants.DESTINATION_ADDRESS_GLOBAL:
                # send request to send
                rts_msg = Message(extended_id=True,
                                  arbitration_id=rts_arbitration_id.can_id,
                                  data=[constants.CM_MSG_TYPE_RTS,
                                        pdu_length_msb,
                                        pdu_length_lsb,
                                        len(messages),
                                        0xFF,
                                        pgn_lsb,
                                        pgn_middle,
                                        pgn_msb],
                                  dlc=8)
                self.can_bus.send(rts_msg)
            else:
                rts_arbitration_id.pgn.pdu_specific = constants.DESTINATION_ADDRESS_GLOBAL
                bam_msg = Message(extended_id=True,
                                  arbitration_id=rts_arbitration_id.can_id,
                                  data=[constants.CM_MSG_TYPE_BAM,
                                        pdu_length_msb,
                                        pdu_length_lsb, len(messages),
                                        0xFF,
                                        pgn_lsb,
                                        pgn_middle,
                                        pgn_msb],
                                  dlc=8)
                # send BAM
                self.can_bus.send(bam_msg)

                for message in messages:
                    # send data messages - no flow control, so no need to wait
                    # for receiving devices to acknowledge
                    self._long_message_segment_queue.put_nowait(message)
        else:
            can_message = Message(arbitration_id=msg.arbitration_id.can_id,
                                  extended_id=True,
                                  dlc=len(msg.data),
                                  data=msg.data)

            self.can_bus.send(can_message)

    def shutdown(self):
        self.can_notifier.running.clear()
        self.can_bus.shutdown()
        self.j1939_notifier.running.clear()
        super(Bus, self).shutdown()

    def _process_incoming_message(self, msg):
        logger.debug("Processing incoming message")
        logging.debug(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 = []

        if arbitration_id.pgn.value == constants.PGN_TP_CONNECTION_MANAGEMENT:
            retval = self._connection_management_handler(pdu)
        elif arbitration_id.pgn.value == constants.PGN_TP_DATA_TRANSFER:
            retval = self._data_transfer_handler(pdu)
        else:
            retval = pdu

        return retval

    def _connection_management_handler(self, msg):
        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 == constants.CM_MSG_TYPE_RTS:
            retval = self._process_rts(msg)
        elif cmd == constants.CM_MSG_TYPE_CTS:
            retval = self._process_cts(msg)
        elif cmd == constants.CM_MSG_TYPE_EOM_ACK:
            retval = self._process_eom_ack(msg)
        elif cmd == constants.CM_MSG_TYPE_BAM:
            retval = self._process_bam(msg)
        elif cmd == constants.CM_MSG_TYPE_ABORT:
            retval = self._process_abort(msg)

        return retval

    def _data_transfer_handler(self, msg):
        msg_source = msg.arbitration_id.source_address
        pdu_specific = msg.arbitration_id.pgn.pdu_specific

        if msg_source in self._incomplete_received_pdus:

            if pdu_specific 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:
                    if pdu_specific == constants.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
                    send_ack = any(True for l in self.j1939_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 = constants.PGN_TP_CONNECTION_MANAGEMENT
                        arbitration_id.pgn.pdu_specific = msg_source
                        arbitration_id.source_address = pdu_specific
                        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

                        div, mod = divmod(total_length, 256)
                        can_message = Message(arbitration_id=arbitration_id.can_id,
                                              extended_id=True,
                                              dlc=8,
                                              data=[constants.CM_MSG_TYPE_EOM_ACK,
                                                    mod, #total_length % 256,
                                                    div, #total_length / 256,
                                                    _num_packages,
                                                    0xFF,
                                                    _pgn_lsb,
                                                    _pgn_middle,
                                                    pgn_msb])
                        self.can_bus.send(can_message)

                    return self._process_eom_ack(msg)

    def _process_rts(self, msg):
        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] == constants.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:
            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] != constants.CM_MSG_TYPE_BAM:
            for _listener in self.j1939_notifier.listeners:
                if isinstance(_listener, Node):
                    # 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 _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 = constants.PGN_TP_CONNECTION_MANAGEMENT
                        _cts_arbitration_id.pgn.pdu_specific = msg.arbitration_id.source_address
                        _data = [0x11, msg.data[4], 0x01, 0xFF, 0xFF]
                        _data.extend(msg.data[5:])
                        cts_msg = Message(extended_id=True, arbitration_id=_cts_arbitration_id.can_id, data=_data,
                                          dlc=8)

                        # send clear to send
                        self.can_bus.send(cts_msg)
                        return

    def _process_cts(self, msg):
        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]:
                    self.can_bus.send(_msg)

    def _process_eom_ack(self, msg):
        if (msg.arbitration_id.pgn.value - msg.arbitration_id.pgn.pdu_specific) == constants.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:
            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]

        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.is_set():
            _msg = None
            try:
                _msg = self._long_message_segment_queue.get(timeout=0.1)
            except Empty:
                pass
            if _msg is not None:
                self.can_bus.send(_msg)

    @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
Exemple #3
0
class driver:

	"""
	Driver construcotr
	@param interface  Name of the CAN interface to listen on
	@param verbose  On true, print extra verbose debugging info
	@param listeners  Additional listeners for CAN notifier
	"""
	def __init__(self, interface, verbose = False, listeners = None):
		self.bus = Bus(interface, bustype = 'socketcan')
		self.verbose = verbose
		self.listeners = listeners
		self.bms = bms.bms()
		self.ccs = ccs.ccs()
		self.notifier = can.Notifier(self.bus, [self.on_message_received])
		
	"""
	Starts a loop to keep notifier alive.  Do not use 
	if you already have an event loop.
	@exception on any thrown event, shuts down bus and stops notifier
	"""
	def listen(self):
		try:
			while True:
				time.sleep(1)
		except KeyboardInterrupt:
			print("Exception Handled!")
			self.bus.shutdown()
			self.notifier.stop()
	
	
	"""
	TODO DOC
	"""
	def notify_listeners(self, hypercan_message):
		for listener in self.listeners:
			listener(hypercan_message, self)

	"""
	Function is called whenever the notifier receives a message.
	@param message  Python-CAN message object
	@return hypermessage  Hyperloop level message dictionary
	"""
	def on_message_received(self, message):
		
		# Verbose output
		if self.verbose:
			print("Message Receieved")
			canFrameVars = vars(message)
			for var in canFrameVars:
				print(var+": "+str(canFrameVars[var]))
		
		# Properly handle messages
		if message.arbitration_id == 0x18FF50E5:
			self.notify_listeners(self.ccs.handle_message(message))
		elif message.arbitration_id >= 0x620 and message.arbitration_id <= 0x628:
			self.notify_listeners(self.bms.handle_message(message))
		elif self.verbose:
			# Eventually I need to add an exception class here....and add the motor controller
			# It is also noteworthy that this block may be deleted completely, there are many
			# arbitration IDs that we can safely ignore transmitted over the bus
			print('Unknown arbitration ID '+hex(message.arbitration_id))
			#raise Exception('Unknown arbitration ID '+str(message.arbitration_id))
			

	"""
	Attempts to send message over CAN bus
	@param Message  CAN message object to TX
	@throws on CAN error
	"""
	def send_message(self, Message):
		try:
			self.bus.send(Message);
		except:
			print("Some error was handled")
Exemple #4
0
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, *args, **kwargs):
        logger.debug("Creating a new j1939 bus")

        self.rx_can_message_queue = Queue()

        super(Bus, self).__init__()
        self._pdu_type = pdu_type
        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)

        # 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

        logger.debug("Creating a new can bus")
        self.can_bus = RawCanBus(*args, **kwargs)
        self.can_notifier = Notifier(self.can_bus,
                                     [self.rx_can_message_queue.put])
        self.j1939_notifier = Notifier(self, [])

        self._long_message_throttler.start()

    def recv(self, timeout=None):
        logger.debug("Waiting for new message")
        logger.debug("Timeout is {}".format(timeout))
        try:
            m = self.rx_can_message_queue.get(timeout=timeout)
        except Empty:
            return

        rx_pdu = None

        if isinstance(m, Message):
            logger.debug('Got a Message: %s' % m)
            if m.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.info('Message is j1939 msg')
                rx_pdu = self._process_incoming_message(m)
            else:
                logger.info("Received non J1939 message (ignoring)")

            # 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)

        # Return to BusABC where it will get fed to any listeners
        return rx_pdu

    def send(self, msg):
        messages = []
        if len(msg.data) > 8:
            # 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)

            pdu_length_lsb, pdu_length_msb = divmod(len(pdu.data), 256)

            while len(pdu.data) % 7 != 0:
                pdu.data += b'\xFF'

            for i, segment in enumerate(pdu.data_segments(segment_length=7)):
                arbitration_id = copy.deepcopy(pdu.arbitration_id)
                arbitration_id.pgn.value = constants.PGN_TP_DATA_TRANSFER
                if pdu.arbitration_id.pgn.is_destination_specific and \
                   pdu.arbitration_id.destination_address != constants.DESTINATION_ADDRESS_GLOBAL:
                    arbitration_id.pgn.pdu_specific = pdu.arbitration_id.pgn.pdu_specific
                else:
                    arbitration_id.pgn.pdu_specific = constants.DESTINATION_ADDRESS_GLOBAL

                message = Message(arbitration_id=arbitration_id.can_id,
                                  extended_id=True,
                                  dlc=(len(segment) + 1),
                                  data=(bytearray([i + 1]) + segment))
                messages.append(message)

            if pdu.arbitration_id.pgn.is_destination_specific and \
               pdu.arbitration_id.destination_address != constants.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] = {}
                self._incomplete_transmitted_pdus[
                    pdu.arbitration_id.
                    source_address][destination_address] = messages
            else:
                destination_address = constants.DESTINATION_ADDRESS_GLOBAL

            rts_arbitration_id = ArbitrationID(source_address=pdu.source)
            rts_arbitration_id.pgn.value = constants.PGN_TP_CONNECTION_MANAGEMENT
            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 != constants.DESTINATION_ADDRESS_GLOBAL:
                # send request to send
                rts_msg = Message(extended_id=True,
                                  arbitration_id=rts_arbitration_id.can_id,
                                  data=[
                                      constants.CM_MSG_TYPE_RTS,
                                      pdu_length_msb, pdu_length_lsb,
                                      len(messages), 0xFF, pgn_lsb, pgn_middle,
                                      pgn_msb
                                  ],
                                  dlc=8)
                self.can_bus.send(rts_msg)
            else:
                rts_arbitration_id.pgn.pdu_specific = constants.DESTINATION_ADDRESS_GLOBAL
                bam_msg = Message(extended_id=True,
                                  arbitration_id=rts_arbitration_id.can_id,
                                  data=[
                                      constants.CM_MSG_TYPE_BAM,
                                      pdu_length_msb, pdu_length_lsb,
                                      len(messages), 0xFF, pgn_lsb, pgn_middle,
                                      pgn_msb
                                  ],
                                  dlc=8)
                # send BAM
                self.can_bus.send(bam_msg)

                for message in messages:
                    # send data messages - no flow control, so no need to wait
                    # for receiving devices to acknowledge
                    self._long_message_segment_queue.put_nowait(message)
        else:
            can_message = Message(arbitration_id=msg.arbitration_id.can_id,
                                  extended_id=True,
                                  dlc=len(msg.data),
                                  data=msg.data)

            self.can_bus.send(can_message)

    def shutdown(self):
        self.can_notifier.running.clear()
        self.can_bus.shutdown()
        self.j1939_notifier.running.clear()
        super(Bus, self).shutdown()

    def _process_incoming_message(self, msg):
        logger.debug("Processing incoming message")
        logging.debug(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 = []

        if arbitration_id.pgn.value == constants.PGN_TP_CONNECTION_MANAGEMENT:
            retval = self._connection_management_handler(pdu)
        elif arbitration_id.pgn.value == constants.PGN_TP_DATA_TRANSFER:
            retval = self._data_transfer_handler(pdu)
        else:
            retval = pdu
        logging.debug(retval)
        return retval

    def _connection_management_handler(self, msg):
        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 == constants.CM_MSG_TYPE_RTS:
            retval = self._process_rts(msg)
        elif cmd == constants.CM_MSG_TYPE_CTS:
            retval = self._process_cts(msg)
        elif cmd == constants.CM_MSG_TYPE_EOM_ACK:
            retval = self._process_eom_ack(msg)
        elif cmd == constants.CM_MSG_TYPE_BAM:
            retval = self._process_bam(msg)
        elif cmd == constants.CM_MSG_TYPE_ABORT:
            retval = self._process_abort(msg)

        return retval

    def _data_transfer_handler(self, msg):
        msg_source = msg.arbitration_id.source_address
        pdu_specific = msg.arbitration_id.pgn.pdu_specific

        if msg_source in self._incomplete_received_pdus:

            if pdu_specific 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:
                    if pdu_specific == constants.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
                    send_ack = any(True for l in self.j1939_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 = constants.PGN_TP_CONNECTION_MANAGEMENT
                        arbitration_id.pgn.pdu_specific = msg_source
                        arbitration_id.source_address = pdu_specific
                        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

                        div, mod = divmod(total_length, 256)
                        can_message = Message(
                            arbitration_id=arbitration_id.can_id,
                            extended_id=True,
                            dlc=8,
                            data=[
                                constants.CM_MSG_TYPE_EOM_ACK,
                                mod,  # total_length % 256,
                                div,  # total_length / 256,
                                _num_packages,
                                0xFF,
                                _pgn_lsb,
                                _pgn_middle,
                                pgn_msb
                            ])
                        self.can_bus.send(can_message)

                    return self._process_eom_ack(msg)

    def _process_rts(self, msg):
        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] == constants.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:
            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] != constants.CM_MSG_TYPE_BAM:
            for _listener in self.j1939_notifier.listeners:
                if isinstance(_listener, Node):
                    # 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 _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 = constants.PGN_TP_CONNECTION_MANAGEMENT
                        _cts_arbitration_id.pgn.pdu_specific = msg.arbitration_id.source_address
                        _data = [0x11, msg.data[4], 0x01, 0xFF, 0xFF]
                        _data.extend(msg.data[5:])
                        cts_msg = Message(
                            extended_id=True,
                            arbitration_id=_cts_arbitration_id.can_id,
                            data=_data,
                            dlc=8)

                        # send clear to send
                        self.can_bus.send(cts_msg)
                        return

    def _process_cts(self, msg):
        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]:
                    self.can_bus.send(_msg)

    def _process_eom_ack(self, msg):
        if (msg.arbitration_id.pgn.value - msg.arbitration_id.pgn.pdu_specific
            ) == constants.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:
            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]

        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.is_set():
            _msg = None
            try:
                _msg = self._long_message_segment_queue.get(timeout=0.1)
            except Empty:
                pass
            if _msg is not None:
                self.can_bus.send(_msg)

    @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
    # sutiable figures using a calibration procedure
    x_accel_offset = -5489
    y_accel_offset = -1441
    z_accel_offset = 1305
    x_gyro_offset = -2
    y_gyro_offset = -72
    z_gyro_offset = -5
    enable_debug_output = False
    enable_logging = True
    log_file = './logs/mpulog.csv'

    mpu = MPU6050(i2c_bus, device_address, x_accel_offset,
                  y_accel_offset, z_accel_offset, x_gyro_offset,
                  y_gyro_offset, z_gyro_offset, enable_debug_output)
    mpuC = MPU6050IRQHandler(mpu, enable_logging, log_file)

    GPIO.setup("P9_11", GPIO.IN)
    GPIO.add_event_detect("P9_11", GPIO.RISING, callback=mpuC.action)

try:
    while True:
        time.sleep(1)
except KeyboardInterrupt:
    if GPS_logging:
        gps_thread.running = False
        gps_thread.join()
    if CAN_logging:
        bus.shutdown()
    if MPU_logging:
        GPIO.cleanup()
class IBSInterface(can.Listener):
    """ This class defines the methods for a minimal ISOBUS CF.
    This means address claiming procedures (part 5) and diagnostics (part 12).
    Other interfaces inherit from this class to implement specific parts, such as
    IBSVTInterface (part 6).
    """
    def __init__(self, interface, channel):
        can.rc['interface'] = interface
        can.rc['channel'] = channel
        log.info('Opening CAN connection on {0}'.format(can.rc['channel']))
        self.bus = Bus(bitrate=250000)
        self.periodic_tasks = list()
        self._rxHandlers = list()

        #Have to use a separate bus object, otherwise WaitForIsobusmessage does not work
        #self.notifier =  can.Notifier(Bus(), [self])

    def __del__(self):
        self.bus.shutdown()

    def on_message_received(self, mesg):
        #TODO: Check 'listening' flag, if listening, store in queue
        #      Change 'WaitForIBSMessage' accordingly, so we don't have to call recv on bus
        #      Then we need a 'Start listening' and 'StopListening' + flushqueue method

        ibsid = IBSID.FromCANID(mesg.arbitration_id)
        log.debug('Rx Mesg PGN {pgn:04X} SA {sa:02X} DA {da:02X}'.format(
            pgn=ibsid.pgn, sa=ibsid.sa, da=ibsid.da))

        #TODO: Smarter filters, SA, DA, PGN, muxbyte?
        for handler in self._rxHandlers:
            if pgn in handler.pgnlist:
                handler.RxMessage(ibsid, mesg.data)

    def AddRxHandler(self, handler):
        self._rxHandlers.append(handler)

    def AddPeriodicMessage(self, ibsid, contents, period):
        # For socketcan_native, bit 32 (MSb) needs to be set for extended ID
        # Is fixed in latest python-can though!
        log.debug(
            'Adding periodic message ID : 0x{mesgid:08X} period {T}'.format(
                mesgid=ibsid.GetCANID(), T=period))
        msg = can.Message(arbitration_id=(ibsid.GetCANID() | (1 << 31)),
                          data=contents,
                          extended_id=True)
        self.periodic_tasks.append(
            can.send_periodic(can.rc['channel'], msg, period))

    def StopPeriodicMessage(self, ibsid):
        # For socketcan_native, bit 32 (MSb) needs to be set for extended ID
        # Is fixed in latest python-can though!
        for periodMsg in self.periodic_tasks:
            if periodMsg.can_id == (ibsid.GetCANID() | (1 << 31)):
                log.debug(
                    'Stopping periodic message ID : 0x{mesgid:08X}'.format(
                        mesgid=ibsid.GetCANID()))
                self.periodic_tasks.remove(periodMsg)
                periodMsg.stop()
                break

    def ModifyPeriodicMessage(self, ibsid, newContent):
        # For socketcan_native, bit 32 (MSb) needs to be set for extended ID
        # Is fixed in latest python-can though!
        msg = can.Message(arbitration_id=(ibsid.GetCANID() | (1 << 31)),
                          data=newContent,
                          extended_id=True)
        for periodMsg in self.periodic_task:
            if periodMsg.can_id == canid:
                periodMsg.modify_data(self, msg)
                break

    def SendRequestAddressClaim(self, sa):
        log.debug('Sending Request Address Claim')
        self.SendRequest(sa, da=SA_GLOBAL, reqPGN=PGN_ADDRCLAIM)

    def SendAddressClaim(self, ibsName, sa):
        log.debug('Sending Address claim for name {:016X}'.format(ibsName))
        candata = NumericValue(ibsName).AsLEBytes(8)
        self._SendIBSMessage(PGN_ADDRCLAIM, SA_GLOBAL, sa, candata)

    def SendRequest(self, sa, da, reqPGN):
        self._SendIBSMessage(PGN_REQUEST, sa, da,
                             NumericValue(reqPGN).AsLEBytes(3))

    ## PROTECTED FUNCTIONS
    def _SendCANMessage(self, canid, candata):
        if len(candata) <= 8:
            msg = can.Message(arbitration_id=canid,
                              data=candata,
                              extended_id=True)
            try:
                self.bus.send(msg)
            except can.CanError:
                log.warning('Error sending message')

    def _WaitForIBSMessage(self, pgn, fromsa, tosa, muxByte, maxtime=3.0):
        # TODO: Also accept incoming TP session
        # TODO: Can we miss messages because we start listening too late?

        received = False
        data = [RESERVED] * 8  # Dummy data for when nothing is received
        starttime = time.time()
        matchID = IBSID(da=tosa, sa=fromsa, pgn=pgn, prio=6)
        while not (received):
            mesg = self.bus.recv(0.5)
            if mesg is not None:
                receivedID = IBSID.FromCANID(mesg.arbitration_id)
                if (receivedID.pgn == matchID.pgn
                        and receivedID.da == matchID.da
                        and receivedID.sa == matchID.sa
                        and mesg.data[0] == muxByte):
                    received = True
                    data = mesg.data
                    break
            if (time.time() - starttime) > maxtime:
                log.debug('Timeout waiting for CAN ID {canid:08X}'.format(
                    canid=matchID.GetCANID()))
                break

        return received, data

    def _SendIBSMessage(self, pgn, da, sa, data, prio=6):
        if len(data) <= 8:
            canid = IBSID(da, sa, pgn, prio).GetCANID()
            self._SendCANMessage(canid, data)
        elif len(data) <= 1785:
            self._SendTPMessage(pgn, da, sa, data)
        elif len(data) <= 117440505:
            self._SendETPMessage(pgn, da, sa, data)
        else:
            log.warning('ERROR : CAN message too large to send')

    def _SendTPMessage(self, pgn, da, sa, data):
        log.debug('(TP) Request starting TP for {n} bytes'.format(n=len(data)))
        tpcm_id = IBSID(da, sa, pgn=PGN_TP_CM, prio=6)
        tpdt_id = IBSID(da, sa, pgn=PGN_TP_DT, prio=7)

        # Send RTS
        rts_control = 0x10
        nr_of_packets = int(math.ceil(len(data) / 7.0))
        rts_data = ([rts_control] + NumericValue(len(data)).AsLEBytes(2) +
                    [nr_of_packets, RESERVED] + NumericValue(pgn).AsLEBytes(3))

        log.debug(
            '(TP) Sending RTS for PGN {0} : {1} bytes in {2} packets'.format(
                pgn, len(data), nr_of_packets))
        self._SendCANMessage(tpcm_id.GetCANID(), rts_data)

        # Check the CTS
        #FIXME: Only send min(nrOfPackets,maxPackets), what to do if less?
        [received, ctsdata] = self._WaitForIBSMessage(0xEC00, da, sa, 0x11)
        if received:
            log.debug('(TP) Received CTS for max {0} packets, next packet {1}'.
                      format(ctsdata[1], ctsdata[2]))

        else:
            return False

        # Pack with 0xFF
        if len(data) % 7 > 0:
            data = data + list([RESERVED] * (7 - (len(data) % 7)))

        # Send bytes
        for seqN in range(nr_of_packets):
            log.debug('(TP) Send package {n}'.format(n=seqN + 1))
            startByte = seqN * 7
            self._SendCANMessage(tpdt_id.GetCANID(),
                                 [seqN + 1] + data[startByte:startByte + 7])
            # sleep 1 msec, otherwise hardware buffer gets full!
            time.sleep(0.001)

    def _SendETPMessage(self, pgn, da, sa, data):
        log.debug(
            '(ETP) Request starting ETP for {n} bytes'.format(n=len(data)))
        etpcm_id = IBSID(da, sa, PGN_ETP_CM, prio=6)
        etpdt_id = IBSID(da, sa, PGN_ETP_DT, prio=7)

        mesg_size = len(data)

        # Send RTS
        rts_control = 0x14
        totalPackets = int(math.ceil(len(data) / 7.0))

        log.debug("(ETP) Sending {0} bytes in {1} packets".format(
            len(data), totalPackets))

        rts_data = ([rts_control] + NumericValue(mesg_size).AsLEBytes(4) +
                    NumericValue(pgn).AsLEBytes(3))
        self._SendCANMessage(etpcm_id.GetCANID(), rts_data)

        # Pack data with 0xFF to multiple of 7
        if len(data) % 7 > 0:
            data = data + list([RESERVED] * (7 - (len(data) % 7)))

        # Setup for the data transfer
        nextPacket = 1
        maxSentPackets = 0
        done = False

        while not (done):
            # TODO: What is the time out for this one?
            [received, ctsdata] = self._WaitForIBSMessage(0xC800, da, sa, 0x15)
            if received:
                nextPacket = NumericValue.FromLEBytes(ctsdata[2:5]).Value()
                maxSentPackets = ctsdata[1]
                log.debug(
                    "(ETP) Received CTS for max {0} packets, next packet {1}".
                    format(maxSentPackets, nextPacket))
            else:
                log.warning('(ETP) Wait for CTS timed out')
                break

            packetOffset = nextPacket - 1

            nPackets = min(maxSentPackets, totalPackets - packetOffset)

            log.debug(
                '(ETP) Sending {0} packets with packet offset {1}'.format(
                    nPackets, packetOffset))
            log.debug('(ETP) bytes[{0} - {1}]'.format(
                packetOffset * 7, packetOffset * 7 + nPackets * 7 - 1))

            dpoData = ([0x16] + [nPackets] +
                       NumericValue(packetOffset).AsLEBytes(3) +
                       NumericValue(pgn).AsLEBytes(3))
            self._SendCANMessage(etpcm_id.GetCANID(), dpoData)

            for n in range(nPackets):
                startByte = (n * 7) + (packetOffset * 7)
                # Send send data[n * 7 + dataoffset: n* 7 +7 + dataoffset]
                self._SendCANMessage(etpdt_id.GetCANID(),
                                     [n + 1] + data[startByte:startByte + 7])

                # If it is the last packet, quit the loop
                if (n + nextPacket) >= totalPackets:
                    done = True
                    break

                time.sleep(0.001)