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