def run(self):
vehicle_bus = Bus(channel=settings.CAN_VEHICLE_CHANNEL,
bustype=settings.CAN_BUS_TYPE)
last_gps_odom = None
while True:
message = vehicle_bus.recv()
if message.arbitration_id == 0x5D7:
new_odometer_value = self.decode_odometer_value(message.data)
logging.info('CanManager : odometer received = ' +
str(new_odometer_value))
self.odometer.value = new_odometer_value
if last_gps_odom is None or (new_odometer_value -
last_gps_odom) >= 1:
try:
self.session_manager.add_gps_point()
except AutoReportException:
pass
else:
last_gps_odom = new_odometer_value
logging.info('CanManager : gps_point requested')
elif message.arbitration_id == 0x69F:
logging.info('CanManager : vin received = ' +
str(self.decode_vin_value(message.data)))
try:
self.vin.put_nowait(self.decode_vin_value(message.data))
except queue.Full:
pass
def __init__(self, tx_id, rx_id, connection_mode=0, channel=None):
"""Sets up a CAN bus instance with a filter and a notifier.
:param tx_id:
Id for outgoing messages
:param rx_id:
Id for incoming messages
:param connection_mode:
Connection mode for the xcp-protocol. Only set if a custom mode is needed.
:param channel:
The channel for the can adapter. Only needed for Usb2can on Windows.
"""
self._reader = BufferedReader()
if platform.system() == "Windows":
from can.interfaces.usb2can import Usb2canBus
self._bus = Usb2canBus(channel=channel)
else:
from can.interface import Bus
self._bus = Bus()
self._bus.set_filters([{
"can_id": rx_id,
"can_mask": rx_id + 0x100,
"extended": False
}])
self._notifier = Notifier(self._bus, [self._reader])
self._tx_id = tx_id
self._rx_id = rx_id
self._conn_mode = connection_mode
def initCAN():
global bus
can.rc['interface'] = 'socketcan'
can.rc['bitrate'] = 500000
can.rc['channel'] = 'can0'
bus = Bus()
bus.flush_tx_buffer()
class VehicleCanSender(Process):
def __init__(self, odometer, test_type, is_log_player):
super(VehicleCanSender, self).__init__()
self.odometer = odometer
self.is_log_player = is_log_player
try:
if test_type == "local":
self.vehicle_bus = Bus(channel=LOCAL_VEHICLE_CHANNEL,
bustype=LOCAL_BUS_TYPE)
else:
self.vehicle_bus = Bus(channel=RPI_VEHICLE_CHANNEL,
can_filters=RPI_FILTERS,
bustype=RPI_BUS_TYPE)
except CanError:
logging.warning(
"VehicleCanSender: couldn't create the vehicle bus !")
def run(self):
if self.is_log_player:
self.play_log_file()
else:
while True:
data = self.encode_odometer_value()
message = Message(arbitration_id=0x5D7,
data=data,
extended_id=False)
try:
self.vehicle_bus.send(message)
time.sleep(1)
except AttributeError:
logging.warning(
"VehicleCanSender: couldn't send the message !")
break
def encode_odometer_value(self):
return (self.odometer.value << 12).to_bytes(7, byteorder='big')
def play_log_file(self):
with open(VEHICLE_LOGS_FILE, "r") as file:
lines = file.readlines()
for line in lines:
words = line.split()
arbitration_id = int(words[1], 16)
data = [int(number, 16) for number in words[3:]]
message = Message(arbitration_id=arbitration_id,
data=data,
extended_id=False)
try:
self.vehicle_bus.send(message)
time.sleep(1)
except AttributeError:
logging.warning(
"VehicleCanSender: couldn't send the message !")
break
def __init__(self, mode, test_type, is_log_player):
super(MABXCanSender, self).__init__()
self.mode = mode
self.is_log_player = is_log_player
try:
if test_type == "local":
self.mabx_bus = Bus(channel=LOCAL_MABX_CHANNEL,
bustype=LOCAL_BUS_TYPE)
else:
self.mabx_bus = Bus(channel=RPI_MABX_CHANNEL,
can_filters=RPI_FILTERS,
bustype=RPI_BUS_TYPE)
except CanError:
logging.warning("MABXCanSender: couldn't create the mabx bus !")
def initCAN(self):
try:
# send out command to set up can0 from the shell to interface CAN controller hardware
call('sudo ip link set can0 up type can bitrate 500000', shell=True)
# initialize Bus object
self.bus = Bus(channel='can0', bustype='socketcan_native')
# initialize message buffer to store incoming CAN messages
self.bufferedReader = can.BufferedReader()
# notifier will notify when new message arrives on the bus and places it in the buffer
self.notifier = can.Notifier(self.bus, [self.bufferedReader])
except:
print(traceback.print_exc())
def setup_can(self, async_loop=None):
for i in range(len(Cfg.Can.CAN_CHS)):
# can.interface.Bus(bustype='socketcan', channel='can0', bitrate=500000, data_bitrate=2000000, fd=True)
bus = Bus(bustype=Cfg.Can.CAN_BUS,
channel=Cfg.Can.CAN_CHS[i],
fd=True)
self.log.info(f'Can message recv on {bus.channel_info}')
# filter
bus.set_filters(Cfg.Can.CAN_FILTERS[i])
self._buses.append(bus)
self._notifier = Notifier(self._buses, [
self.parse_msg,
])
def __init__(self, odometer, test_type, is_log_player):
super(VehicleCanSender, self).__init__()
self.odometer = odometer
self.is_log_player = is_log_player
try:
if test_type == "local":
self.vehicle_bus = Bus(channel=LOCAL_VEHICLE_CHANNEL,
bustype=LOCAL_BUS_TYPE)
else:
self.vehicle_bus = Bus(channel=RPI_VEHICLE_CHANNEL,
can_filters=RPI_FILTERS,
bustype=RPI_BUS_TYPE)
except CanError:
logging.warning(
"VehicleCanSender: couldn't create the vehicle bus !")
def open(self):
logging.debug("Open CAN Interface..")
can0 = Bus(channel=self._device_name, bustype=self._bus_type, receive_own_messages=False)
loop = asyncio.get_event_loop()
reader = AsyncBufferedReader(loop)
listeners = [reader]
notifier = Notifier(can0, listeners, loop=loop)
logging.debug("CAN Interface opened")
return can0, reader, notifier
def read_canbus(filename=None):
if filename is None:
bus = Bus()
listener = can.BufferedReader()
while True:
msg = bus.recv(timeout=1)
listener.on_message_received(msg=msg)
tmp_msg = listener.get_message(timeout=0.1)
if tmp_msg:
nmea_msg = NmeaMessage(tmp_msg.timestamp,
tmp_msg.arbitration_id,
tmp_msg.dlc,
tmp_msg.data)
print(nmea_msg.convert_to_actisense())
else:
with open(filename) as test_file:
for line in test_file:
print(line)
def run(self):
mabx_bus = Bus(channel=settings.CAN_MABX_CHANNEL,
bustype=settings.CAN_BUS_TYPE)
mode = None
while True:
message = mabx_bus.recv()
if message.arbitration_id == 0xC1:
new_mode = self.decode_mode_value(message.data)
logging.info('CanManager : mode received = ' + str(new_mode))
if mode != new_mode and new_mode != settings.MODE.UNKNOWN:
if mode is None:
if self.odometer.value == 0:
continue
self.session_manager.start_session(mode=new_mode,
car=self.vin.get())
mode = new_mode
else:
self.session_manager.change_mode(new_mode)
mode = new_mode
def initCAN():
global bus
global listener
global notifier
can.rc['interface'] = 'socketcan'
can.rc['bitrate'] = 500000
can.rc['channel'] = 'can0'
try:
bus = Bus()
listener = can.BufferedReader()
notifier = can.Notifier(bus, [listener])
except Exception as e:
print ('ERR: ' + str(e))
def configComm(self):
if not self.commStatus:
if self.sensor.get() != 'BEG':
messagebox.showinfo('Error', 'Device not supported.')
else:
try:
tp_addr = isotp.Address(
isotp.AddressingMode.Normal_29bits, # noqa: E501
txid=0x18DA2AF1,
rxid=0x18DAFA2A)
bus = Bus(bustype='pcan',
channel='PCAN_USBBUS1',
bitrate=500000)
stack = isotp.CanStack(bus=bus, address=tp_addr)
self.conn = PythonIsoTpConnection(stack)
self.requestTitleMenu.grid(row=3, column=0, pady=5)
self.begRequestMenu.grid(row=4, column=0, pady=5)
self.sendBtn.grid(row=5, column=0, pady=5)
self.commStatus = True
self.commButton.config(text='Disconnect')
self.terminalMenu.grid(row=6, column=0)
self.termPrint('Info - connected')
self.sensorOptions.config(state='disabled')
except Exception:
messagebox.showinfo('Error', 'There is no connection')
self.commStatus = False
else:
try:
self.commButton.config(text='Connect')
self.requestTitleMenu.grid_forget()
self.begRequestMenu.grid_forget()
self.terminalMenu.grid_forget()
self.sendBtn.grid_forget()
self.commStatus = False
except Exception:
messagebox.showinfo('Error', 'Unable to disconnect')
self.commStatuss.config(text=str(self.commStatus))
class XCPFlash:
"""A Tool for flashing devices like electronic control units via the xcp-protocol."""
_reader = None
_bus = None
_notifier = None
_tx_id = 0
_rx_id = 0
_conn_mode = 0
_data_len = 0
_max_data_prg = 0
_max_data = 8
def __init__(self, tx_id, rx_id, connection_mode=0, channel=None):
"""Sets up a CAN bus instance with a filter and a notifier.
:param tx_id:
Id for outgoing messages
:param rx_id:
Id for incoming messages
:param connection_mode:
Connection mode for the xcp-protocol. Only set if a custom mode is needed.
:param channel:
The channel for the can adapter. Only needed for Usb2can on Windows.
"""
self._reader = BufferedReader()
if platform.system() == "Windows":
from can.interfaces.usb2can import Usb2canBus
self._bus = Usb2canBus(channel=channel)
else:
from can.interface import Bus
self._bus = Bus()
self._bus.set_filters([{
"can_id": rx_id,
"can_mask": rx_id + 0x100,
"extended": False
}])
self._notifier = Notifier(self._bus, [self._reader])
self._tx_id = tx_id
self._rx_id = rx_id
self._conn_mode = connection_mode
@staticmethod
def print_progress_bar(iteration,
total,
prefix='',
suffix='',
decimals=1,
length=50,
fill='█'):
"""Print a progress bar to the console.
:param iteration:
Actual iteration of the task
:param total:
Total iteration for completing the task
:param prefix:
Text before the progress bar
:param suffix:
Text after the progress bar
:param decimals:
Number of decimal places for displaying the percentage
:param length:
Line length for the progress bar
:param fill:
Char to fill the bar
"""
percent = ("{0:." + str(decimals) + "f}").format(
100 * (iteration / float(total)))
filled_length = int(length * iteration // total)
bar = fill * filled_length + '-' * (length - filled_length)
sys.stdout.write('\r{} |{}| {}% {}'.format(prefix, bar, percent,
suffix))
if iteration == total:
print()
def send_can_msg(self, msg, wait=True, timeout=30):
"""Send a message via can
Send a message over the can-network and may wait for a given timeout for a response.
:param msg:
Message to send
:param wait:
True if the program should be waiting for a response, False otherwise
:param timeout:
Timeout for waiting for a response
:return:
The response if wait is set to True, nothing otherwise
"""
self._bus.send(msg)
if wait:
try:
return self.wait_for_response(timeout, msg)
except ConnectionAbortedError as err:
raise err
def wait_for_response(self, timeout, msg=None):
"""Waiting for a response
:param timeout:
Time to wait
:param msg:
The message which was send. Set this only if a retry should be made in case of a timeout.
:return:
The response from the device
:raises: ConnectionAbortedError
if the response is an error
"""
tries = 1
while True:
if tries == 5:
raise ConnectionAbortedError("Timeout")
received = self._reader.get_message(timeout)
if received is None and msg is not None:
self.send_can_msg(msg, False)
tries += 1
continue
if received is None:
continue
if received.arbitration_id == self._rx_id and received.data[
0] == XCPResponses.SUCCESS.value:
return received
elif received.arbitration_id == self._rx_id and received.data[
0] == XCPResponses.ERROR.value:
raise ConnectionAbortedError(received.data[1])
elif msg is not None:
self.send_can_msg(msg, False)
def execute(self, command, **kwargs):
"""Execute a command
Builds the can-message to execute the given command and sends it to the device.
:param command:
The xcp-command to be executed
:param kwargs:
Needed arguments for the command.
:command SET_MTA:
'addr_ext' and 'addr'
:command PROGRAM_CLEAR:
'range'
:command PROGRAM:
'size' and 'data'
:return: response of the command if waited for
"""
msg = Message(arbitration_id=self._tx_id,
is_extended_id=False,
data=bytes(8))
msg.data[0] = command.value
if command == XCPCommands.CONNECT:
msg.data[1] = self._conn_mode
response = self.send_can_msg(msg)
self._max_data = response.data[4] << 8
self._max_data += response.data[5]
return response
if command == XCPCommands.DISCONNECT:
return self.send_can_msg(msg)
if command == XCPCommands.SET_MTA:
msg.data[3] = kwargs.get('addr_ext', 0)
for i in range(4, 8):
msg.data[i] = (kwargs['addr'] &
(0xFF000000 >> (8 * (i - 4)))) >> (8 * (7 - i))
return self.send_can_msg(msg)
if command == XCPCommands.PROGRAM_START:
response = self.send_can_msg(msg)
max_dto_prg = response.data[3]
max_bs = response.data[4]
self._data_len = (max_dto_prg - 2) * max_bs
self._max_data_prg = max_dto_prg - 2
return response
if command == XCPCommands.PROGRAM_CLEAR:
for i in range(4, 8):
msg.data[i] = (kwargs['range'] &
(0xFF000000 >> (8 * (i - 4)))) >> (8 * (7 - i))
return self.send_can_msg(msg)
if command == XCPCommands.PROGRAM or command == XCPCommands.PROGRAM_NEXT:
msg.data[1] = kwargs["size"]
position = 2
for data in kwargs["data"]:
msg.data[position] = data
position += 1
return self.send_can_msg(msg, kwargs['size'] <= self._max_data_prg)
if command == XCPCommands.PROGRAM_RESET:
return self.send_can_msg(msg)
def program(self, data):
"""Program the device
Program the device with the given firmware
:param data:
the firmware as byte-array
"""
print("flashing new firmware...")
bytes_send = 0
while bytes_send < len(data):
send_length = self._data_len
if bytes_send % 10000 <= self._data_len:
self.print_progress_bar(bytes_send,
len(data),
prefix="Progress:",
suffix="Complete")
sys.stdout.flush()
if send_length > len(data) - bytes_send:
send_length = len(data) - bytes_send
self.execute(XCPCommands.PROGRAM,
size=send_length,
data=data[bytes_send:bytes_send + self._max_data_prg])
send_length -= self._max_data_prg
bytes_send += min(send_length, self._max_data_prg)
while send_length > 0:
self.execute(XCPCommands.PROGRAM_NEXT,
size=send_length,
data=data[bytes_send:bytes_send +
self._max_data_prg])
bytes_send += min(send_length, self._max_data_prg)
send_length -= self._max_data_prg
self.print_progress_bar(bytes_send,
len(data),
prefix="Progress:",
suffix="Complete")
self.execute(XCPCommands.PROGRAM_RESET)
def clear(self, start_addr, length):
"""Clear the memory of the device
Erase all contents of a given range in the device memory.
:param start_addr:
Start address of the range
:param length:
Length of the range
"""
print("erasing device (this may take several minutes)...")
self.execute(XCPCommands.PROGRAM_START)
self.execute(XCPCommands.SET_MTA, addr=start_addr)
self.execute(XCPCommands.PROGRAM_CLEAR, range=length)
def connect(self):
"""Connect to the device
:raises: ConnectionError
if the device doesn't support flash programming or the address granularity is > 1
"""
print("connecting...")
response = self.execute(XCPCommands.CONNECT)
if not response.data[1] & 0b00010000:
raise ConnectionError(
"Flash programming not supported by the connected device")
if response.data[2] & 0b00000110:
raise ConnectionError("Address granularity > 1 not supported")
def disconnect(self):
"""Disconnect from the device"""
print("disconnecting..")
self.execute(XCPCommands.DISCONNECT)
def __call__(self, start_addr, data):
"""Flash the device
Do all the necessary steps for flashing, including connecting to the device and clearing the memory.
:param start_addr:
Start address for the firmware
:param data:
The firmware as byte-array
"""
try:
self.connect()
self.clear(start_addr, len(data))
self.program(data)
except ConnectionAbortedError as err:
if err.args[0] == "Timeout":
print("\nConnection aborted: Timeout")
else:
print("\nConnection aborted: {}".format(
XCPErrors.error_messages[err.args[0]]))
except ConnectionError as err:
print("\nConnection error: {}".format(err))
finally:
try:
self.disconnect()
except ConnectionAbortedError as err:
if err.args[0] == "Timeout":
print("\nConnection aborted: Timeout")
else:
print("\nConnection aborted: {}".format(
XCPErrors.error_messages[err.args[0]]))
from pynq import Overlay
import os
ol = Overlay("./can.bit")
"""Abilitato l'uso dell'interfaccia can su PmodB"""
os.system("ip link set can1 type can bitrate 1000000")
os.system("ip link set can1 up")
"""Configurata ed abilitata l'interfaccia can1"""
# Nella seguente cella ogni messaggio letto sul bus viene stampato a video e scritto su un file di log_result
# In[ ]:
import can
import time
from can.interface import Bus
bus = Bus('can1', bustype='socketcan', fd=True)
"""Abilitazione del bus per la ricezione"""
nome = time.strftime("%d_%m_%y-%H_%M_%S.csv", time.localtime())
f = open(nome, "w")
while (True):
msg = bus.recv()
"""Ricezione del messaggio"""
print(msg)
print(str(msg), file=f)
""" Scrittura dei messaggi ricevuti in in file log_result.txt"""
canbusobj.send(canmsgobj) # Send a predefined message on the specified CAN Bus interface
time.sleep(delay) # Wait for 'delay' time before returning to main application body
return
# Global variables here
CAN_INTERFACE = 'socketcan' # Set CAN Bus interface support type
CAN_CHANNEL = 'can0' # Set CAN Bus interface channel name
CAN_BITRATE = 500000 # Set CAN Bus interface channel bitrate
# Setting up can bus interface
can.rc['interface'] = CAN_INTERFACE # Initiate can interface
can.rc['channel'] = CAN_CHANNEL # Initiate can channel
can.rc['bitrate'] = CAN_BITRATE # Initiate can bitrate
bus = Bus() # Initiate CAN Bus
#
# Configuring Ford Fiesta CAN ID 0x201 to carry the following information:
#
# - Engine revolutions per minute: 870 (0x366) [combustion engine ON at minimum rpms]
# - Leave all other data bytes to their CAN Bus recordings detected value
#
msg = Message(extended_id=False, arbitration_id=0x201, data=[0x3, 0x66, 0x40, 0x0, 0x0, 0x0, 0x0, 0x80])
# Set message delay in seconds
msg_delay = 0.09 # Message delay is 90ms
disc_tout = 0.01 # CAN Bus activity discovery timeout is 10ms
while True: # Loops forever until application is stopped
if bus.recv(disc_tout) != None: # Waits 'disc_tout' seconds to see if monitored CAN Bus channel is active
print(bg.parents)
# for node, cpt in bg.cpt_nodes.items():
# print(node)
# print(cpt)
print(bg.nodes_list)
# Nella cella seguente vengono recuperate le informazioni dai messaggi CAN ricevuti sul bus e viene richiamata la rete bayesiana. Inoltre vengono scritte in un file di log_result informazioni di interesse:
# * Numero della classificazione
# * Tempo in cui la rete bayesiana effettua la predizione
# * Stato dei parametri del veicolo
# * Risultato della predizione effettuata dalla rete
# * Se è in corso o meno un attacco al veicolo
# In[4]:
bus = Bus('can1', bustype='socketcan', fd=True)
db = cantools.database.load_file(db_name)
"""Abilitazione del bus per la ricezione"""
flag_calc_belief = False
if not os.path.isdir("log_result"):
os.mkdir("log_result")
log_file_name = time.strftime("%d_%m_%y-%H_%M_%S.txt", time.localtime())
log_file_name = os.path.abspath(os.path.join("log_result", log_file_name))
log_file = open(log_file_name, "w")
# inizializzare con i nodi a partire dai quali calcolare la probabilità a posteriori
# se i nodi cambiano ad ogni ciclo reinizializzare la lista ad ogni ciclo
list_evidence_node = ['steer', 'abs_steer', 'throttle', 'speed', 'brake']
status_buf = StatusBuffer(bg.nodes_list)
msg_cont = 0
def __init__(self, pdu_type=PDU, broadcast=True, *args, **kwargs):
logger.debug("Creating a new j1939 bus")
#self.rx_can_message_queue = Queue()
self.queue = Queue()
self.node_queue_list = [] # Start with nothing
super(Bus, self).__init__(kwargs.get('channel'),
kwargs.get('can_filters'))
self._pdu_type = pdu_type
self.timeout = 1
self._long_message_throttler = threading.Thread(
target=self._throttler_function)
self._long_message_throttler.daemon = True
self._incomplete_received_pdus = {}
self._incomplete_received_pdu_lengths = {}
self._incomplete_transmitted_pdus = {}
self._long_message_segment_queue = Queue(0)
self._key_generation_fcn = None
if 'keygen' in kwargs and kwargs['keygen'] is not None:
self._key_generation_fcn = kwargs['keygen']
if 'ignoreCanSendError' in kwargs and kwargs[
'ignoreCanSendError'] is not None:
self._ignore_can_send_error = kwargs['ignoreCanSendError']
if broadcast:
self.node_queue_list = [
(None, self)
] # Start with default logger Queue which will receive everything
# Convert J1939 filters into Raw Can filters
if 'j1939_filters' in kwargs and kwargs['j1939_filters'] is not None:
filters = kwargs.pop('j1939_filters')
logger.debug("Got filters: {}".format(filters))
can_filters = []
for filt in filters:
can_id, can_mask = 0, 0
if 'pgn' in filt:
can_id = filt['pgn'] << 8
# The pgn needs to be left shifted by 8 to ignore the CAN_ID's source address
# Look at most significant 4 bits to determine destination specific
if can_id & 0xF00000 == 0xF00000:
logging.info("PDU2 (broadcast message)")
can_mask = 0xFFFF00
else:
logging.info("PDU1 (p2p)")
can_mask = 0xFF0000
if 'source' in filt:
# filter by source
can_mask |= 0xFF
can_id |= filt['source']
logger.info("added source", filt)
logger.info("Adding CAN ID filter: {:0x}:{:0x}".format(
can_id, can_mask))
can_filters.append({"can_id": can_id, "can_mask": can_mask})
kwargs['can_filters'] = can_filters
if 'timeout' in kwargs and kwargs['timeout'] is not None:
if isinstance(kwargs['timeout'], (int, float)):
self.timeout = kwargs['timeout']
else:
raise ValueError("Bad timeout type")
logger.debug("Creating a new can bus")
self.can_bus = RawCanBus(*args, **kwargs)
canListener = j1939Listner(self.notification)
self.can_notifier = canNotifier(self.can_bus, [canListener],
timeout=self.timeout)
self._long_message_throttler.start()
from can import Message
from can.interface import Bus
import time
from settings import(
LOCAL_BUS_TYPE,
LOCAL_VEHICLE_CHANNEL,
RPI_BUS_TYPE,
RPI_FILTERS,
RPI_VEHICLE_CHANNEL,
VEHICLE_LOGS_FILE
)
# Local (Socket-can)
bus = Bus(channel=LOCAL_VEHICLE_CHANNEL, bustype=LOCAL_BUS_TYPE)
# Raspberry Pi (Kvaser)
# bus = Bus(channel=RPI_VEHICLE_CHANNEL, can_filters=RPI_FILTERS, bustype=RPI_BUS_TYPE)
# Play the whole vehicle log file
with open(VEHICLE_LOGS_FILE, "r") as file:
lines = file.readlines()
for line in lines:
words = line.split()
arbitration_id = int(words[1], 16)
data = [int(number, 16) for number in words[3:]]
message = Message(arbitration_id=arbitration_id, data=data, extended_id=False)
bus.send(message)
time.sleep(1)
from can.interface import Bus
from udsoncan.connections import PythonIsoTpConnection
from udsoncan.client import Client
import isotp
from udsoncan import Response
import securityAlgo as sec
from time import sleep
tp_addr = isotp.Address(isotp.AddressingMode.Normal_29bits,
txid=0x18DA2AF1,
rxid=0x18DAFA2A)
bus = Bus(bustype='pcan', channel='PCAN_USBBUS1', bitrate=500000)
stack = isotp.CanStack(bus=bus, address=tp_addr)
conn = PythonIsoTpConnection(stack)
with Client(conn,
request_timeout=1,
config={'exception_on_unexpected_response': False}) as client:
try:
conn.send(b'\x10\x03')
payload = conn.wait_frame(timeout=1)
response = Response.from_payload(payload)
print(response)
conn.send(b'\x27\x63')
payload = conn.wait_frame(timeout=1)
response = Response.from_payload(payload)
print('key: ' + response.data.hex()[2:])
seed = (response.data.hex()[2:])
sA = sec.securityAlgo(seed, 'series')
sleep(.1)
print('calculated key: ' + (sA.calculatedKey).hex())
conn.send(b'\x27\x64' + sA.calculatedKey)
class Bus(BusABC):
"""
A CAN Bus that implements the J1939 Protocol.
:param list j1939_filters:
a list of dictionaries that specify filters that messages must
match to be received by this Bus. Messages can match any of the
filters.
Options are:
* :pgn: An integer PGN to show
"""
channel_info = "j1939 bus"
def __init__(self, pdu_type=PDU, broadcast=True, *args, **kwargs):
logger.debug("Creating a new j1939 bus")
#self.rx_can_message_queue = Queue()
self.queue = Queue()
self.node_queue_list = [] # Start with nothing
super(Bus, self).__init__(kwargs.get('channel'),
kwargs.get('can_filters'))
self._pdu_type = pdu_type
self.timeout = 1
self._long_message_throttler = threading.Thread(
target=self._throttler_function)
self._long_message_throttler.daemon = True
self._incomplete_received_pdus = {}
self._incomplete_received_pdu_lengths = {}
self._incomplete_transmitted_pdus = {}
self._long_message_segment_queue = Queue(0)
self._key_generation_fcn = None
if 'keygen' in kwargs and kwargs['keygen'] is not None:
self._key_generation_fcn = kwargs['keygen']
if 'ignoreCanSendError' in kwargs and kwargs[
'ignoreCanSendError'] is not None:
self._ignore_can_send_error = kwargs['ignoreCanSendError']
if broadcast:
self.node_queue_list = [
(None, self)
] # Start with default logger Queue which will receive everything
# Convert J1939 filters into Raw Can filters
if 'j1939_filters' in kwargs and kwargs['j1939_filters'] is not None:
filters = kwargs.pop('j1939_filters')
logger.debug("Got filters: {}".format(filters))
can_filters = []
for filt in filters:
can_id, can_mask = 0, 0
if 'pgn' in filt:
can_id = filt['pgn'] << 8
# The pgn needs to be left shifted by 8 to ignore the CAN_ID's source address
# Look at most significant 4 bits to determine destination specific
if can_id & 0xF00000 == 0xF00000:
logging.info("PDU2 (broadcast message)")
can_mask = 0xFFFF00
else:
logging.info("PDU1 (p2p)")
can_mask = 0xFF0000
if 'source' in filt:
# filter by source
can_mask |= 0xFF
can_id |= filt['source']
logger.info("added source", filt)
logger.info("Adding CAN ID filter: {:0x}:{:0x}".format(
can_id, can_mask))
can_filters.append({"can_id": can_id, "can_mask": can_mask})
kwargs['can_filters'] = can_filters
if 'timeout' in kwargs and kwargs['timeout'] is not None:
if isinstance(kwargs['timeout'], (int, float)):
self.timeout = kwargs['timeout']
else:
raise ValueError("Bad timeout type")
logger.debug("Creating a new can bus")
self.can_bus = RawCanBus(*args, **kwargs)
canListener = j1939Listner(self.notification)
self.can_notifier = canNotifier(self.can_bus, [canListener],
timeout=self.timeout)
self._long_message_throttler.start()
def notification(self, inboundMessage):
#self.rx_can_message_queue.put(inboundMessage)
if self.can_notifier._running is False:
logger.info('Aborting message %s bus is not running',
inboundMessage)
if isinstance(inboundMessage, Message):
logger.info('\n\nnotification: Got a Message from CAN: %s' %
inboundMessage)
if inboundMessage.id_type:
# Extended ID
# Only J1939 messages (i.e. 29-bit IDs) should go further than this point.
# Non-J1939 systems can co-exist with J1939 systems, but J1939 doesn't care
# about the content of their messages.
logger.debug('notification: Message is j1939 msg')
#
# Need to determine if it's a broadcase message or
# limit to listening nodes only
#
arbitration_id = ArbitrationID()
arbitration_id.can_id = inboundMessage.arbitration_id
logger.debug('notification: ArbitrationID = %s' %
(arbitration_id))
for (node, l_notifier) in self.node_queue_list:
logger.debug("notification: node=%s" % (node))
logger.debug(" notifier=%s" % (l_notifier))
logger.debug(" arbitration_id.pgn=%s" %
(arbitration_id.pgn))
logger.debug(" destination_address=%s" %
(arbitration_id.destination_address))
# redirect the AC stuff to the node processors. the rest can go
# to the main queue.
if node and (arbitration_id.pgn in [
PGN_AC_ADDRESS_CLAIMED, PGN_AC_COMMANDED_ADDRESS,
PGN_REQUEST_FOR_PGN
]):
logger.info("notification: sending to notifier queue")
# send the PDU to the node processor.
l_notifier.queue.put(inboundMessage)
# if node has the destination address, do something with the PDU
elif node and (arbitration_id.destination_address
in node.address_list):
rx_pdu = self._process_incoming_message(inboundMessage)
if rx_pdu:
logger.info(
"WP02: notification: sent to general queue: %s QQ=%s"
% (rx_pdu, self.queue))
self.queue.put(rx_pdu)
elif node and (arbitration_id.destination_address is None):
logger.info(
"notification: sending broadcast to general queue")
rx_pdu = self._process_incoming_message(inboundMessage)
logger.info(
"WP01: notification: sent broadcast to general queue: %s QQ=%s"
% (rx_pdu, self.queue))
self.queue.put(rx_pdu)
elif node is None:
# always send the message to the logging queue
logger.info("notification: sending to general queue")
rx_pdu = self._process_incoming_message(inboundMessage)
logger.info(
"WP03: notification: sent pdu [%s] to general queue"
% rx_pdu)
self.queue.put(rx_pdu)
else:
logger.info("WP04: notification: pdu dropped: %s\n\n" %
inboundMessage)
else:
logger.info("Received non J1939 message (ignoring)")
def connect(self, node):
"""
Attach a listening node (with a dest address) to the J1939 bus
"""
logger.debug("connect: type(node)=%s, node=%s" % (type(node), node))
if not isinstance(node, Node):
raise ValueError(
"bad parameter for node, must be a J1939 node object")
notifier = Notifier(Queue(), node.on_message_received, timeout=None)
self.node_queue_list.append((node, notifier))
def recv(self, timeout=None):
#logger.debug("Waiting for new message")
#logger.debug("Timeout is {}".format(timeout))
logger.debug('J1939 Bus recv(), waiting on QQ=%s with timeout %s' %
(self.queue, timeout))
try:
#m = self.rx_can_message_queue.get(timeout=timeout)
rx_pdu = self.queue.get(timeout=timeout)
logger.info('J1939 Bus recv() successful QQ=%s, pdu:%s' %
(self.queue, rx_pdu))
return rx_pdu
except Empty:
logger.debug('J1939 Bus recv() timed out' % ())
return None
# TODO: Decide what to do with CAN errors
# if m.is_error_frame:
# logger.warning("Appears we got an error frame!")
#
# rx_error = CANError(timestamp=m.timestamp)
# if rx_error is not None:
# logger.info('Sending error "%s" to registered listeners.' % rx_error)
# for listener in self.listeners:
# if hasattr(listener, 'on_error_received'):
# listener.on_error_received(rx_error)
def send(self, msg, timeout=None):
logger.info("j1939.send: msg=%s" % msg)
messages = []
if len(msg.data) > 8:
logger.info("j1939.send: message is > than 8 bytes")
# Making a copy of the PDU so that the original
# is not altered by the data padding.
pdu = copy.deepcopy(msg)
pdu.data = bytearray(pdu.data)
logger.info("j1939.send: Copied msg = %s" % pdu)
pdu_length_lsb, pdu_length_msb = divmod(len(pdu.data), 256)
while len(pdu.data) % 7 != 0:
pdu.data += b'\xFF'
logger.info("j1939.send: padded msg (mod 7) = %s" % pdu)
logger.info("MIL8:---------------------")
#
# segment the longer message into 7 byte segments. We need to prefix each
# data[0] with a sequence number for the transfer
#
for i, segment in enumerate(pdu.data_segments(segment_length=7)):
arbitration_id = copy.deepcopy(pdu.arbitration_id)
arbitration_id.pgn.value = PGN_TP_DATA_TRANSFER
logger.info(
"MIL8: j1939.send: i=%d, pdu.arbitration_id.pgn.is_destination_specific=%d, data=%s"
% (i, pdu.arbitration_id.pgn.is_destination_specific,
segment))
if pdu.arbitration_id.pgn.is_destination_specific and \
pdu.arbitration_id.destination_address != DESTINATION_ADDRESS_GLOBAL:
arbitration_id.pgn.pdu_specific = pdu.arbitration_id.pgn.pdu_specific
else:
arbitration_id.pgn.pdu_specific = DESTINATION_ADDRESS_GLOBAL
arbitration_id.destination_address = DESTINATION_ADDRESS_GLOBAL
logger.info("MIL8: j1939.send: segment=%d, arb = %s" %
(i, arbitration_id))
message = Message(arbitration_id=arbitration_id.can_id,
extended_id=True,
dlc=(len(segment) + 1),
data=(bytearray([i + 1]) + segment))
messages.append(message)
#
# At this point we have the queued messages sequenced in 'messages'
#
logger.info(
"MIL8: j1939.send: is_destination_specific=%d, destAddr=%s" %
(pdu.arbitration_id.pgn.is_destination_specific,
pdu.arbitration_id.destination_address))
logger.info("MIL8: j1939.send: messages=%s" % messages)
if pdu.arbitration_id.pgn.is_destination_specific and \
pdu.arbitration_id.destination_address != DESTINATION_ADDRESS_GLOBAL:
destination_address = pdu.arbitration_id.pgn.pdu_specific
if pdu.arbitration_id.source_address in self._incomplete_transmitted_pdus:
if destination_address in self._incomplete_transmitted_pdus[
pdu.arbitration_id.source_address]:
logger.warning(
"Duplicate transmission of PDU:\n{}".format(pdu))
else:
self._incomplete_transmitted_pdus[
pdu.arbitration_id.source_address] = {}
# append the messages to the 'incomplete' list
self._incomplete_transmitted_pdus[
pdu.arbitration_id.
source_address][destination_address] = messages
else:
destination_address = DESTINATION_ADDRESS_GLOBAL
logger.debug("MIL8: rts arbitration id: src=%s, dest=%s" %
(pdu.source, destination_address))
rts_arbitration_id = ArbitrationID(
pgn=PGN_TP_CONNECTION_MANAGEMENT,
source_address=pdu.source,
destination_address=destination_address)
rts_arbitration_id.pgn.pdu_specific = pdu.arbitration_id.pgn.pdu_specific
temp_pgn = copy.deepcopy(pdu.arbitration_id.pgn)
if temp_pgn.is_destination_specific:
temp_pgn.value -= temp_pgn.pdu_specific
pgn_msb = ((temp_pgn.value & 0xFF0000) >> 16)
pgn_middle = ((temp_pgn.value & 0x00FF00) >> 8)
pgn_lsb = (temp_pgn.value & 0x0000FF)
if pdu.arbitration_id.pgn.is_destination_specific and \
pdu.arbitration_id.destination_address != DESTINATION_ADDRESS_GLOBAL:
# send request to send
logger.debug("MIL8: rts to specific dest: src=%s, dest=%s" %
(pdu.source, destination_address))
rts_msg = Message(extended_id=True,
arbitration_id=rts_arbitration_id.can_id,
data=[
CM_MSG_TYPE_RTS, pdu_length_msb,
pdu_length_lsb,
len(messages), 0xFF, pgn_lsb, pgn_middle,
pgn_msb
],
dlc=8)
try:
logger.info("MIL08: j1939.send: sending TP.RTS to %s: %s" %
(destination_address, rts_msg))
self.can_bus.send(rts_msg)
except CanError:
if self._ignore_can_send_error:
pass
raise
else:
rts_arbitration_id.pgn.pdu_specific = DESTINATION_ADDRESS_GLOBAL
rts_arbitration_id.destination_address = DESTINATION_ADDRESS_GLOBAL
logger.debug("MIL8: rts to Global dest: src=%s, dest=%s" %
(pdu.source, destination_address))
bam_msg = Message(
extended_id=True,
arbitration_id=rts_arbitration_id.can_id | pdu.source,
data=[
CM_MSG_TYPE_BAM, pdu_length_msb, pdu_length_lsb,
len(messages), 0xFF, pgn_lsb, pgn_middle, pgn_msb
],
dlc=8)
bam_msg.destination_address = DESTINATION_ADDRESS_GLOBAL
# bam_msg.arbitration_id.destination_address = DESTINATION_ADDRESS_GLOBAL
# send BAM
try:
logger.info("j1939.send: sending TP.BAM to %s: %s" %
(destination_address, bam_msg))
self.can_bus.send(bam_msg)
time.sleep(0.05)
except CanError:
if self._ignore_can_send_error:
pass
raise
for message in messages:
# send data messages - no flow control, so no need to wait
# for receiving devices to acknowledge
logger.info("j1939.send: queue TP.BAM data to %s: %s" %
(destination_address, message))
self._long_message_segment_queue.put_nowait(message)
else:
msg.display_radix = 'hex'
logger.debug("j1939.send: calling can_bus_send: j1939-msg: %s" %
(msg))
can_message = Message(arbitration_id=msg.arbitration_id.can_id,
extended_id=True,
dlc=len(msg.data),
data=msg.data)
logger.debug("j1939.send: calling can_bus_send: can-msg: %s" %
can_message)
try:
self.can_bus.send(can_message)
except CanError:
if self._ignore_can_send_error:
pass
raise
def shutdown(self):
self.can_notifier._running = False
self.can_bus.shutdown()
#self.j1939_notifier.running.clear()
super(Bus, self).shutdown()
def _send_key_response(self, pdu):
logger.info("PI04: _send_key_response src=%d, pdu=%s" %
(pdu.source, pdu))
src = pdu.destination
dest = pdu.source
logger.info("PI05: new PDU, src=%d, dest=%d" % (src, dest))
pdu.destination = dest
logger.info("PI05: new PDU.dest = %d" % (pdu.destination))
pdu.source = src
logger.info("PI06: newPDU = %s" % (pdu))
logger.info("PI04: _send_key_response src/dest flipped pdu=%s" % (pdu))
assert (pdu.data[0] == 4) # only support long key for now
data = pdu.data
assert (len(data) == 8)
seed = (data[5] << 24) + (data[4] << 16) + (data[3] << 8) + data[2]
if self._key_generation_fcn is None:
return None
key = self._key_generation_fcn(seed)
logger.info(
"PI03: _send_key_response Seed: 0x%08x yields key: 0x%08x" %
(seed, key))
data[5] = (key >> 24) & 0xff
data[4] = (key >> 16) & 0xff
data[3] = (key >> 8) & 0xff
data[2] = (key) & 0xff
data[1] = 1
pdu.data = data
self.send(pdu)
return None
def _process_incoming_message(self, msg):
logger.info(
"PI01: Processing incoming message: instance=%s\n msg= %s" %
(self, msg))
arbitration_id = ArbitrationID()
arbitration_id.can_id = msg.arbitration_id
if arbitration_id.pgn.is_destination_specific:
arbitration_id.pgn.value -= arbitration_id.pgn.pdu_specific
pdu = self._pdu_type(timestamp=msg.timestamp,
data=msg.data,
info_strings=[])
pdu.arbitration_id.can_id = msg.arbitration_id
pdu.info_strings = []
pdu.radix = 16
logger.info("PI02: arbitration_id.pgn.value == 0x%04x" %
arbitration_id.pgn.value)
if arbitration_id.pgn.value == PGN_TP_CONNECTION_MANAGEMENT:
logger.info("PGN_TP_CONNECTION_MANAGEMENT")
retval = self._connection_management_handler(pdu)
elif arbitration_id.pgn.value == PGN_TP_DATA_TRANSFER:
logger.info("PGN_TP_DATA_TRANSFER")
retval = self._data_transfer_handler(pdu)
elif (arbitration_id.pgn.value
== PGN_TP_SEED_REQUEST) and (self._key_generation_fcn
is not None):
logger.info("PGN_TP_SEED_REQUEST")
retval = self._send_key_response(pdu)
else:
logger.info("PGN_PDU")
retval = pdu
logger.info("_process_incoming_message: returning %s" % (retval))
return retval
def _connection_management_handler(self, msg):
logger.debug("MP00: _connection_management_handler: %s, cmd=%s" %
(msg, msg.data[0]))
if len(msg.data) == 0:
msg.info_strings.append(
"Invalid connection management message - no data bytes")
return msg
cmd = msg.data[0]
retval = None
if cmd == CM_MSG_TYPE_RTS:
retval = self._process_rts(msg)
elif cmd == CM_MSG_TYPE_CTS:
retval = self._process_cts(msg)
elif cmd == CM_MSG_TYPE_EOM_ACK:
retval = self._process_eom_ack(msg)
elif cmd == CM_MSG_TYPE_BAM:
retval = self._process_bam(msg)
elif cmd == CM_MSG_TYPE_ABORT:
retval = self._process_abort(msg)
logger.debug("_connection_management_handler: returning %s" % (retval))
return retval
def _data_transfer_handler(self, msg):
logger.debug("_data_transfer_handler:")
msg_source = msg.arbitration_id.source_address
pdu_specific = msg.arbitration_id.pgn.pdu_specific
if msg_source in self._incomplete_received_pdus:
logger.debug("in self._incomplete_received_pdus:")
if pdu_specific in self._incomplete_received_pdus[msg_source]:
logger.debug("in self._incomplete_received_pdus[msg_source]:")
self._incomplete_received_pdus[msg_source][
pdu_specific].data.extend(msg.data[1:])
total = self._incomplete_received_pdu_lengths[msg_source][
pdu_specific]["total"]
if len(self._incomplete_received_pdus[msg_source]
[pdu_specific].data) >= total:
logger.debug(
"allReceived: %s" %
type(self._incomplete_received_pdus[msg_source]))
logger.debug("allReceived: %s" % pprint.pformat(
self._incomplete_received_pdus[msg_source]))
logger.debug(
"allReceived: %s from 0x%x" %
(type(self._incomplete_received_pdus[msg_source]
[pdu_specific]), pdu_specific))
logger.debug("allReceived: %s" %
(self._incomplete_received_pdus[msg_source]
[pdu_specific]))
if pdu_specific == DESTINATION_ADDRESS_GLOBAL:
logger.debug(
"pdu_specific == DESTINATION_ADDRESS_GLOBAL")
# Looks strange but makes sense - in the absence of explicit flow control,
# the last CAN packet in a long message *is* the end of message acknowledgement
return self._process_eom_ack(msg)
# Find a Node object so we can search its list of known node addresses for this node
# so we can find if we are responsible for sending the EOM ACK message
# TODO: Was self.j1939_notifier.listeners
send_ack = any(
True
for (_listener, l_notifier) in self.node_queue_list
if isinstance(_listener, Node) and (
_listener.address == pdu_specific
or pdu_specific in _listener.address_list))
#send_ack = any(True for l in self.can_notifier.listeners
# if isinstance(l, Node) and (l.address == pdu_specific or
# pdu_specific in l.address_list))
if send_ack:
arbitration_id = ArbitrationID()
arbitration_id.pgn.value = PGN_TP_CONNECTION_MANAGEMENT
arbitration_id.pgn.pdu_specific = msg_source
arbitration_id.source_address = pdu_specific
arbitration_id.destination_address = 0x17
total_length = self._incomplete_received_pdu_lengths[
msg_source][pdu_specific]["total"]
_num_packages = self._incomplete_received_pdu_lengths[
msg_source][pdu_specific]["num_packages"]
pgn = self._incomplete_received_pdus[msg_source][
pdu_specific].arbitration_id.pgn
pgn_msb = ((pgn.value & 0xFF0000) >> 16)
_pgn_middle = ((pgn.value & 0x00FF00) >> 8)
_pgn_lsb = 0
logger.debug(
"in send_ack: arbitration_id=[%s], can_id=[%x], destAdder=0x%0x"
% (arbitration_id, int(arbitration_id.can_id),
arbitration_id.destination_address))
logger.debug("in send_ack: " % ())
div, mod = divmod(total_length, 256)
can_message = Message(
arbitration_id=arbitration_id.can_id,
extended_id=True,
dlc=8,
data=[
CM_MSG_TYPE_EOM_ACK,
mod, # total_length % 256,
div, # total_length / 256,
_num_packages,
0xFF,
_pgn_lsb,
_pgn_middle,
pgn_msb
])
try:
self.can_bus.send(can_message)
except CanError:
if self._ignore_can_send_error:
pass
raise
logger.debug("_data_transfer_handler: returning %s" %
(msg))
return self._process_eom_ack(msg)
def _process_rts(self, msg):
logger.debug("process_rts, source=0x%x" %
(msg.arbitration_id.source_address))
if msg.arbitration_id.source_address not in self._incomplete_received_pdus:
self._incomplete_received_pdus[
msg.arbitration_id.source_address] = {}
self._incomplete_received_pdu_lengths[
msg.arbitration_id.source_address] = {}
# Delete any previous messages that were not finished correctly
if msg.arbitration_id.pgn.pdu_specific in self._incomplete_received_pdus[
msg.arbitration_id.source_address]:
del self._incomplete_received_pdus[
msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific]
del self._incomplete_received_pdu_lengths[
msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific]
if msg.data[0] == CM_MSG_TYPE_BAM:
logger.debug("CM_MSG_TYPE_BAM")
self._incomplete_received_pdus[
msg.arbitration_id.source_address][0xFF] = self._pdu_type()
self._incomplete_received_pdus[msg.arbitration_id.source_address][
0xFF].arbitration_id.pgn.value = int(
("%.2X%.2X%.2X" % (msg.data[7], msg.data[6], msg.data[5])),
16)
if self._incomplete_received_pdus[
msg.arbitration_id.source_address][
0xFF].arbitration_id.pgn.is_destination_specific:
self._incomplete_received_pdus[msg.arbitration_id.source_address][
0xFF].arbitration_id.pgn.pdu_specific = msg.arbitration_id.pgn.pdu_specific
self._incomplete_received_pdus[msg.arbitration_id.source_address][
0xFF].arbitration_id.source_address = msg.arbitration_id.source_address
self._incomplete_received_pdus[
msg.arbitration_id.source_address][0xFF].data = []
_message_size = int("%.2X%.2X" % (msg.data[2], msg.data[1]), 16)
self._incomplete_received_pdu_lengths[
msg.arbitration_id.source_address][0xFF] = {
"total": _message_size,
"chunk": 255,
"num_packages": msg.data[3],
}
else:
logger.debug("not CM_MSG_TYPE_BAM, source=0x%x" %
(msg.arbitration_id.source_address))
self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific] = self._pdu_type()
self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.
pdu_specific].arbitration_id.pgn.value = int(
("%.2X%.2X%.2X" % (msg.data[7], msg.data[6], msg.data[5])),
16)
if self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.
pdu_specific].arbitration_id.pgn.is_destination_specific:
self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.
pdu_specific].arbitration_id.pgn.pdu_specific = msg.arbitration_id.pgn.pdu_specific
self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.
pdu_specific].arbitration_id.source_address = msg.arbitration_id.source_address
self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific].data = []
_message_size = int("%.2X%.2X" % (msg.data[2], msg.data[1]), 16)
self._incomplete_received_pdu_lengths[
msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific] = {
"total": _message_size,
"chunk": 255,
"num_packages": msg.data[3],
}
if msg.data[0] != CM_MSG_TYPE_BAM:
logger.debug("not CM_MSG_TYPE_BAM--2")
logger.debug("self.can_notifier.listeners = %s" %
self.can_notifier.listeners)
logger.debug("self.node_queue_list = %s" % self.node_queue_list)
#for _listener in self.can_notifier.listeners:
for (_listener, l_notifier) in self.node_queue_list:
logger.debug("MIL2: _listner/l_notifier = %s/%s" %
(_listener, l_notifier))
if isinstance(_listener, Node):
logger.debug("6, dest=0x%x" %
(msg.arbitration_id.source_address))
# find a Node object so we can search its list of known node addresses
# for this node - if we find it we are responsible for sending the CTS message
logger.debug("MIL3: Node: %s" % (Node))
logger.debug("MIL3: _listener.address: %s" %
(_listener.address))
logger.debug(
"MIL3: msg.arbitration_id.pgn.pdu_specific: %s" %
(msg.arbitration_id.pgn.pdu_specific))
logger.debug("MIL3: _listener.address_list: %s" %
(_listener.address_list))
if _listener.address == msg.arbitration_id.pgn.pdu_specific or \
msg.arbitration_id.pgn.pdu_specific in _listener.address_list:
_cts_arbitration_id = ArbitrationID(
source_address=msg.arbitration_id.pgn.pdu_specific)
_cts_arbitration_id.pgn.value = PGN_TP_CONNECTION_MANAGEMENT
_cts_arbitration_id.pgn.pdu_specific = msg.arbitration_id.source_address
_cts_arbitration_id.destination_address = msg.arbitration_id.source_address
_data = [0x11, msg.data[4], 0x01, 0xFF, 0xFF]
_data.extend(msg.data[5:])
logger.debug("send CTS: AID: %s" % _cts_arbitration_id)
cts_msg = Message(
extended_id=True,
arbitration_id=_cts_arbitration_id.can_id,
data=_data,
dlc=8)
# send clear to send
logger.debug("send CTS: %s" % cts_msg)
try:
self.can_bus.send(cts_msg)
except CanError:
if self._ignore_can_send_error:
pass
raise
return
"""
#
# MIL: This is the wrong way around this, I should have a node assigned.
#
elif _listener is None and l_notifier is not None:
logger.debug("7, dest=0x%x" % (msg.arbitration_id.source_address))
# find a Node object so we can search its list of known node addresses
# for this node - if we find it we are responsible for sending the CTS message
if msg.arbitration_id.pgn.pdu_specific :
_cts_arbitration_id = ArbitrationID(source_address=msg.arbitration_id.pgn.pdu_specific)
_cts_arbitration_id.pgn.value = PGN_TP_CONNECTION_MANAGEMENT
_cts_arbitration_id.pgn.pdu_specific = msg.arbitration_id.source_address
_cts_arbitration_id.destination_address = msg.arbitration_id.source_address
_data = [0x11, msg.data[4], 0x01, 0xFF, 0xFF]
_data.extend(msg.data[5:])
logger.debug("send CTS: AID: %s" % _cts_arbitration_id)
cts_msg = Message(extended_id=True, arbitration_id=_cts_arbitration_id.can_id, data=_data,
dlc=8)
# send clear to send
logger.debug("send CTS: %s" % cts_msg)
self.can_bus.send(cts_msg)
return
"""
def _process_cts(self, msg):
logger.debug("_process_cts")
logger.debug("MIL8: cts message is: %s" % msg)
logger.debug("MIL8: len(pdu-send-buffer) = %d" %
len(self._incomplete_transmitted_pdus[0][23]))
if msg.arbitration_id.pgn.pdu_specific in self._incomplete_transmitted_pdus:
if msg.arbitration_id.source_address in self._incomplete_transmitted_pdus[
msg.arbitration_id.pgn.pdu_specific]:
# Next packet number in CTS message (Packet numbers start at 1 not 0)
start_index = msg.data[2] - 1
# Using total number of packets in CTS message
end_index = start_index + msg.data[1]
for _msg in self._incomplete_transmitted_pdus[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.
source_address][start_index:end_index]:
logger.debug("MIL8: msg=%s" % (_msg))
# TODO: Needs to be pacing if we get this working...
try:
# Shouldent send a J1939 PDU as a CAN Message unless we are careful
canMessage = Message(
arbitration_id=_msg.arbitration_id, data=_msg.data)
self.can_bus.send(canMessage)
except CanError:
if self._ignore_can_send_error:
pass
raise
logger.debug("MIL8: _process_cts complete")
def _process_eom_ack(self, msg):
logger.debug("_process_eom_ack")
if (msg.arbitration_id.pgn.value -
msg.arbitration_id.pgn.pdu_specific) == PGN_TP_DATA_TRANSFER:
logger.debug("_process_eom_ack: PGN_TP_DATA_TRANSFER")
self._incomplete_received_pdus[msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific].timestamp = msg.timestamp
retval = copy.deepcopy(self._incomplete_received_pdus[
msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific])
retval.data = retval.data[:self._incomplete_received_pdu_lengths[
msg.arbitration_id.source_address][msg.arbitration_id.pgn.
pdu_specific]["total"]]
del self._incomplete_received_pdus[
msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific]
del self._incomplete_received_pdu_lengths[
msg.arbitration_id.source_address][
msg.arbitration_id.pgn.pdu_specific]
else:
logger.debug("_process_eom_ack: not PGN_TP_DATA_TRANSFER")
if msg.arbitration_id.pgn.pdu_specific in self._incomplete_received_pdus:
if msg.arbitration_id.source_address in self._incomplete_received_pdus[
msg.arbitration_id.pgn.pdu_specific]:
self._incomplete_received_pdus[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.
source_address].timestamp = msg.timestamp
retval = copy.deepcopy(self._incomplete_received_pdus[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.source_address])
retval.data = retval.data[:self.
_incomplete_received_pdu_lengths[
msg.arbitration_id.pgn.
pdu_specific][
msg.arbitration_id.
source_address]["total"]]
del self._incomplete_received_pdus[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.source_address]
del self._incomplete_received_pdu_lengths[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.source_address]
else:
retval = None
else:
retval = None
if msg.arbitration_id.pgn.pdu_specific in self._incomplete_transmitted_pdus:
if msg.arbitration_id.source_address in self._incomplete_transmitted_pdus[
msg.arbitration_id.pgn.pdu_specific]:
del self._incomplete_transmitted_pdus[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.source_address]
logger.debug("_process_eom_ack: returning %s" % (retval))
return retval
def _process_bam(self, msg):
self._process_rts(msg)
def _process_abort(self, msg):
if msg.arbitration_id.pgn.pdu_specific in self._incomplete_received_pdus:
if msg.source in self._incomplete_received_pdus[
msg.arbitration_id.pgn.pdu_specific]:
del self._incomplete_received_pdus[
msg.arbitration_id.pgn.pdu_specific][
msg.arbitration_id.source_address]
def _throttler_function(self):
while self.can_notifier._running:
_msg = None
try:
_msg = self._long_message_segment_queue.get(timeout=0.1)
except Empty:
pass
if _msg is not None:
try:
self.can_bus.send(_msg)
time.sleep(0.05)
except CanError:
if self._ignore_can_send_error:
pass
raise
@property
def transmissions_in_progress(self):
retval = 0
for _tx_address in self._incomplete_transmitted_pdus:
retval += len(self._incomplete_transmitted_pdus[_tx_address])
for _rx_address in self._incomplete_received_pdus:
retval += len(self._incomplete_received_pdus[_rx_address])
return retval
delay
) # Wait for 'delay' time before returning to main application body
return
# Global variables here
CAN_INTERFACE = 'socketcan' # Set CAN Bus interface support type
CAN_CHANNEL = 'can0' # Set CAN Bus interface channel name
CAN_BITRATE = 500000 # Set CAN Bus interface channel bitrate
# Setting up can bus interface
can.rc['interface'] = CAN_INTERFACE # Initiate can interface
can.rc['channel'] = CAN_CHANNEL # Initiate can channel
can.rc['bitrate'] = CAN_BITRATE # Initiate can bitrate
bus = Bus() # Initiate CAN Bus
#
# Configuring Ford Fiesta CAN ID 0x201 to carry the following information:
#
# - Engine revolutions per minute: 870 (0x366) [combustion engine ON at minimum rpms]
# - Leave all other data bytes to their CAN Bus recordings detected value
#
msg = Message(extended_id=False,
arbitration_id=0x201,
data=[0x3, 0x66, 0x40, 0x0, 0x0, 0x0, 0x0, 0x80])
# Set message delay in seconds
msg_delay = 0.09 # Message delay is 90ms
disc_tout = 0.01 # CAN Bus activity discovery timeout is 10ms
os.system('./can1init.sh')
os.system('./init_wlan0.sh')
db = cantools.database.load_file(db_name)
bb_pi_addr = (udp_ip_bb_pi, udp_port_bb_pi)
sock_pi = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock_pi.bind(bb_pi_addr)
sock_pi.settimeout(0.0001)
i = 0
flag = True
msg_rec = False
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
can_list = []
bus = Bus('can1', bustype='socketcan', fd=False)
print("While Loop")
# start_timer = timer()
while True:
msg = bus.recv(timeout=0.001)
if msg is not None:
# msg.timestamp = timer() - start_timer
print(msg)
try:
data = sock_pi.recv(2048)
except KeyboardInterrupt:
raise
except:
class CAN_Control():
def __init__(self):
self.bus = None
self.bufferedReader = None
self.notifier = None
self.BMS = self.BMS_Control()
self.MCU = self.MCU_Control()
self.Blinkers = self.Blinkers_Control()
self.initCAN()
def initCAN(self):
"""Initialize the profile for the CAN controller."""
try:
# send out command to set up can0 from the shell to interface CAN controller hardware
call('sudo ip link set can0 up type can bitrate 500000',
shell=True)
# initialize Bus object
self.bus = Bus(channel='can0', bustype='socketcan_native')
# initialize message buffer to store incoming CAN messages
self.bufferedReader = can.BufferedReader()
# notifier will notify when new message arrives on the bus and places it in the buffer
self.notifier = can.Notifier(self.bus, [self.bufferedReader])
except:
print(traceback.print_exc())
def readMessage(self, timeout=0.1):
"""Grabs a message from the buffered reader and returns the arbitration ID and data fields"""
try:
arbitrationID = None
data = None
msg = self.bufferedReader.get_message(timeout=timeout)
if msg is not None:
# arbitration id is the priority number of the CAN message. the lower the id, the higher the priority
arbitrationID = msg.arbitration_id
# data is a list of bytes. data[0] is byte 1 ..... all the way to data[7] is byte 8
data = msg.data
return arbitrationID, data
except:
print(traceback.print_exc())
def sendMessage(self, msg, timeout=0.1):
"""Sends a CAN message on the bus."""
try:
self.bus.send(msg, timeout=timeout)
except:
print(traceback.print_exc())
class BMS_Control():
def __init__(self):
# BMS CAN message1
self.msg1ID = 0x001
self.failsafeStat = 0
self.voltage = 0 # instantaneous voltage of battery pack
self.current = 0 # instantaneous current battery pack
self.highestTemperature = 0 # current highest temperature of pack
self.highestTemperatureThermistorID = 0 # id of thermistor with highest temperature
# BMS CAN message2
self.msg2ID = 0x002
self.avgPackCurrent = 0 # average temperature of pack
self.avgBatteryTemperature = 0 # average temperature of pack
self.stateOfCharge = 0 # state of charge
self.fanSpeed = 0 # speed of battery fan
# others
self.milesRange = 0
def getVoltage(self):
return self.voltage
def getCurrent(self):
return self.current
def getHighestTemp(self):
return self.highestTemperature
def getHighetTempThermistorID(self):
return self.highestTemperatureThermistorID
def getSOC(self):
return self.stateOfCharge
def getAvgBatteryTemp(self):
return self.avgBatteryTemperature
def getAvgPackCurrent(self):
return self.avgPackCurrent
def getFanSpeed(self):
return self.fanSpeed
def decodeMessage1(self, data):
"""Decode CAN message from BMS with ID# 0x001. Message includes Failsafe Statuses, Inst. Pack Voltage,
Inst. Pack Current, Highest Temperature, Thermistor ID with Highest Temperature"""
try:
# failsafe statuses
self.failsafeStat = (data[1] | data[0])
self.voltageFailsafeActive = self.failsafeStat & 0x1
self.currentFailsafeActive = (self.failsafeStat >> 1) & 0x1
self.relayFailsafeActive = (self.failsafeStat >> 2) & 0x1
self.cellBalancingActive = (self.failsafeStat >> 3) & 0x1
self.chargeInterlockFailsafeActive = (
self.failsafeStat >> 4) & 0x1
self.thermistorBValueTableInvalid = (
self.failsafeStat >> 5) & 0x1
self.inputPowerSupplyFailsafeActive = (
self.failsafeStat >> 6) & 0x1
# pack instantaneous voltage
self.voltage = float(s16(data[2] << 8 | data[3]) / 10)
# pack instantaneous current
self.current = float(s16(data[4] << 8 | data[5]) / 10)
# highest temperature
self.highestTemperature = data[6]
# id of thermistor with highest temperature
self.highestTemperatureThermistorID = data[7]
except:
print(traceback.format_exc())
def decodeMessage2(self, data):
"""Decode CAN message from BMS with ID# 0x002. Message includes State of Charge, Average Temperature,
Average Pack Current. """
try:
# state of charge
self.stateOfCharge = float(data[0] / 2)
# average battery temperature
self.avgBatteryTemperature = data[1]
# average pack current
self.avgPackCurrent = float(s16(data[2] << 8 | data[3]) / 10)
# fan speed
self.fanSpeed = data[4]
except:
print(traceback.format_exc())
class MCU_Control():
def __init__(self):
self.msgID = 0x003
self.speed = 0
def getSpeed(self):
return self.speed
def decodeMessage(self, data):
"""Decode CAN message from MCU. Message includes Speed"""
try:
# speed
self.speed = data[0]
except:
print(traceback.format_exc())
class Blinkers_Control():
def __init__(self):
self.msgID = 0x004
self.rightBlinker = 0
self.leftBlinker = 0
self.hazardBlinker = 0
def getRightBlinker(self):
return self.rightBlinker
def getLeftBlinker(self):
return self.leftBlinker
def getHazardBlinker(self):
return self.hazardBlinker
def decodeMessage(self, data):
"""Decode CAN message from blinker. Message includes status of hazards, right, and left blinker"""
try:
# hazard
self.hazardBlinker = data[0]
# right
self.rightBlinker = data[1]
# left
self.leftBlinker = data[2]
except:
print(traceback.format_exc())
def __init__(self,
tx_id: int,
rx_id: int,
connection_mode: int = 0,
**kwargs):
"""Sets up a CAN bus instance with a filter and a notifier.
:param tx_id:
Id for outgoing messages
:param rx_id:
Id for incoming messages
:param connection_mode:
Connection mode for the xcp-protocol. Only set if a custom mode is needed.
"""
self._tx_id = tx_id
self._rx_id = rx_id
self._ag = 1
self._ag_override = False
self._byte_order = XCPFlash.BYTE_ORDER_LE
self._queue_size = 0
self._max_block_size = 0
self._conn_mode = connection_mode
self._initial_comm_mode = connection_mode
self._extended_id = False
self._master_block_mode_supported = False
self._master_block_mode_supported_override = False
self._min_separation_time_us = 0
self._base_delay_ms = max(kwargs.get("base_delay_ms", 0), 0)
interface = kwargs.get("interface", "")
channel = kwargs.get("channel", "")
bus_kwargs = kwargs.get("bus_kwargs", {})
self._extended_id = kwargs.get("extended_id", False)
ag = kwargs.get("ag", 0)
if ag <= 0:
self._ag_override = False
else:
self._ag_override = True
self._ag = ag
mbm = kwargs.get("master_block_mode", -1)
if mbm < 0:
self._master_block_mode_supported_override = False
else:
self._master_block_mode_supported_override = True
self._master_block_mode_supported = mbm != 0
from can.interface import Bus
if None is not interface and "" != interface:
self._bus = Bus(channel=channel, interface=interface, **bus_kwargs)
else:
self._bus = Bus(channel=channel, **bus_kwargs)
self._bus.set_filters([{
"can_id": rx_id,
"can_mask": rx_id,
"extended": self._extended_id
}])
class XCPFlash:
"""A Tool for flashing devices like electronic control units via the xcp-protocol."""
BYTE_ORDER_LE = 0
BYTE_ORDER_BE = 1
def __init__(self,
tx_id: int,
rx_id: int,
connection_mode: int = 0,
**kwargs):
"""Sets up a CAN bus instance with a filter and a notifier.
:param tx_id:
Id for outgoing messages
:param rx_id:
Id for incoming messages
:param connection_mode:
Connection mode for the xcp-protocol. Only set if a custom mode is needed.
"""
self._tx_id = tx_id
self._rx_id = rx_id
self._ag = 1
self._ag_override = False
self._byte_order = XCPFlash.BYTE_ORDER_LE
self._queue_size = 0
self._max_block_size = 0
self._conn_mode = connection_mode
self._initial_comm_mode = connection_mode
self._extended_id = False
self._master_block_mode_supported = False
self._master_block_mode_supported_override = False
self._min_separation_time_us = 0
self._base_delay_ms = max(kwargs.get("base_delay_ms", 0), 0)
interface = kwargs.get("interface", "")
channel = kwargs.get("channel", "")
bus_kwargs = kwargs.get("bus_kwargs", {})
self._extended_id = kwargs.get("extended_id", False)
ag = kwargs.get("ag", 0)
if ag <= 0:
self._ag_override = False
else:
self._ag_override = True
self._ag = ag
mbm = kwargs.get("master_block_mode", -1)
if mbm < 0:
self._master_block_mode_supported_override = False
else:
self._master_block_mode_supported_override = True
self._master_block_mode_supported = mbm != 0
from can.interface import Bus
if None is not interface and "" != interface:
self._bus = Bus(channel=channel, interface=interface, **bus_kwargs)
else:
self._bus = Bus(channel=channel, **bus_kwargs)
self._bus.set_filters([{
"can_id": rx_id,
"can_mask": rx_id,
"extended": self._extended_id
}])
@staticmethod
def sys_byte_order():
return XCPFlash.BYTE_ORDER_LE if sys.byteorder == 'little' else XCPFlash.BYTE_ORDER_BE
@staticmethod
def swap16(value: int):
value &= 0xffff # ensure 16 bit
i0 = (value >> 8) & 0xff
i1 = value & 0xff
return i0 | (i1 << 8)
@staticmethod
def swap32(value: int):
value &= 0xffffffff # ensure 32 bit
i0 = (value >> 24) & 0xff
i1 = (value >> 16) & 0xff
i2 = (value >> 8) & 0xff
i3 = value & 0xff
return i0 | (i1 << 8) | (i2 << 16) | (i3 << 24)
@staticmethod
def print_progress_bar(iteration,
total,
prefix='',
suffix='',
decimals=1,
length=50,
fill='█'):
"""Print a progress bar to the console.
:param iteration:
Actual iteration of the task
:param total:
Total iteration for completing the task
:param prefix:
Text before the progress bar
:param suffix:
Text after the progress bar
:param decimals:
Number of decimal places for displaying the percentage
:param length:
Line length for the progress bar
:param fill:
Char to fill the bar
"""
percent = ("{0:." + str(decimals) + "f}").format(
100 * (iteration / float(total)))
filled_length = int(length * iteration // total)
bar = fill * filled_length + '-' * (length - filled_length)
sys.stdout.write('\r{} |{}| {}% {}'.format(prefix, bar, percent,
suffix))
if iteration == total:
sys.stdout.write('\n')
def _drain_bus(self):
rx_msgs = []
while True:
received = self._bus.recv(0)
if received is None:
break
if received.arbitration_id == self._rx_id:
rx_msgs.append(received)
return rx_msgs
def _wait_for_rx(self, timeout_s: float):
time_left = timeout_s
while time_left >= 0:
start = time.perf_counter()
received = self._bus.recv(time_left)
stop = time.perf_counter()
time_left -= stop - start
if received is not None and received.arbitration_id == self._rx_id:
return received
return None
def _timeout_s(self, no: int):
return 1e-3 * (self._base_delay_ms + xcp_timeout_ms(no))
@staticmethod
def _is_can_device_tx_queue_full(e: CanError) -> bool:
if XCP_HOST_IS_LINUX:
return XCPFlash._is_linux_enobufs(e)
return False
@staticmethod
def _is_linux_enobufs(e: CanError) -> bool:
# Unfortunately, CanError doesn't pass the error code the base
# so we can't check the errno attribute :/
#
# Failed to transmit: [Errno 105] No buffer space available
return e.args[0].find(str(LinuxErrors.ENOBUFS.value)) >= 0
def program(self, data):
"""Program the device
Program the device with the given firmware
:param data:
the firmware as byte-array
"""
# 0 BYTE Command Code = 0xD0
# 1 Number of data elements [AG] [1..(MAX_CTO-2)/AG]
# 2..AG-1 BYTE Used for alignment, only if AG >2
# AG=1: 2..MAX_CTO-2 AG>1: AG MAX_CTO-AG ELEMENT Data elements
# The MASTER_BLOCK_MODE flag indicates whether the Master Block Mode is available.
# If the master device block mode is supported,
# MAX_BS indicates the maximum allowed block size as the number of
# consecutive command packets (DOWNLOAD_NEXT or PROGRAM_NEXT) in a block sequence.
# MIN_ST indicates the required minimum separation time between the packets of a block
# transfer from the master device to the slave device in units of 100 microseconds.
data_elements_per_message = int((self._max_cto - 2) / self._ag)
bytes_per_message = data_elements_per_message * self._ag
offset = 0
data_offset_start = 4 if self._ag == 4 else 2
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
count = 0
if self._master_block_mode_supported:
max_bytes_per_block = self._max_block_size * bytes_per_message
block_count = int(ceil(len(data) / max_bytes_per_block))
iteration = 0
bytes_left_in_block = 0
while offset < len(data):
if bytes_left_in_block:
msg.data[0] = XCPCommands.PROGRAM_NEXT.value
else:
bytes_left_in_block = min(max_bytes_per_block,
len(data) - offset)
msg.data[0] = XCPCommands.PROGRAM.value
this_block_bytes = bytes_left_in_block
XCPFlash.print_progress_bar(iteration, block_count,
'flashing')
iteration += 1
msg_bytes = min(bytes_left_in_block, bytes_per_message)
msg.data[1] = bytes_left_in_block
bytes_left_in_block -= msg_bytes
msg.data[data_offset_start:data_offset_start +
msg_bytes] = data[offset:offset + msg_bytes]
offset += msg_bytes
rx_msgs = self._drain_bus()
for response in rx_msgs:
if response.data[0] == XCPResponses.ERROR.value:
if response.data[1] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
count = 0
else:
message = 'fix me'
if XCPErrors.ERR_SEQUENCE == response.data[1]:
message = 'invalid sequence error'
else:
message = f'0x{response.data[1]:02x}'
raise ConnectionError(
f'{"PROGRAM" if msg.data[0] == XCPCommands.PROGRAM.value else "PROGRAM_NEXT"} failed with {message} (0x{response.data[1]:02x})'
)
while True:
try:
self._bus.send(msg)
break
except CanError as e:
if XCPFlash._is_can_device_tx_queue_full(e):
time.sleep(self._timeout_s(1))
else:
raise e
if 0 == bytes_left_in_block:
response = self._wait_for_rx(self._timeout_s(5))
if None is response:
offset -= this_block_bytes
if count >= 3:
raise ConnectionError(
f'PROGRAM_NEXT failed timeout')
else:
self._sync_or_die()
count += 1
continue
elif response.data[0] == XCPResponses.ERROR.value:
if response.data[1] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
continue
else:
raise ConnectionError(
f'PROGRAM failed with error code: 0x{response.data[1]:02x}'
)
# wait
if self._min_separation_time_us > 0:
time.sleep(self._min_separation_time_us * 1e-6)
XCPFlash.print_progress_bar(block_count, block_count, 'flashing')
else:
raise NotImplementedError('PROGRAM path not implemented')
msg.data[0] = XCPCommands.PROGRAM.value
msg.data[1] = data_elements_per_message
steps = int(len(data) / bytes_per_message)
for i in range(0, steps):
msg.data[data_offset_start:data_offset_start +
bytes_per_message] = data[offset:offset +
bytes_per_message]
offset += bytes_per_message
self.send_can_msg(msg)
def _sync_or_die(self):
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.SYNCH.value
self._drain_bus()
self._bus.send(msg)
response = self._wait_for_rx(self._timeout_s(7))
if None is response:
raise ConnectionError("timeout SYNCH")
if response.data[0] == XCPResponses.ERROR.value:
if response.data[1] != XCPErrors.ERR_CMD_SYNCH:
raise ConnectionAbortedError(response.data[1])
def program_start(self):
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.PROGRAM_START.value
timeout_s = self._timeout_s(1)
count = 0
while True:
self._drain_bus()
self._bus.send(msg)
response = self._wait_for_rx(timeout_s)
if None is response:
if count >= 3:
raise ConnectionError(f'PROGRAM_START failed timeout')
else:
self._sync_or_die()
count += 1
continue
if response.data[0] == XCPResponses.ERROR.value:
if response.data[1] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
count = 0
continue
else:
raise ConnectionError(
f'PROGRAM_START failed with error code: 0x{response.data[1]:02x}'
)
break
prog_comm_mode = response.data[2]
# if prog_comm_mode != self._conn_mode:
# if self._initial_comm_mode != self._conn_mode:
# # prevent a ring around the rosy situation
# raise ConnectionError(f'communication mode already switch once from {self._initial_comm_mode} to {self._conn_mode} with new request for {prog_comm_mode}')
# self._conn_mode = prog_comm_mode
# # fix me: do this automatically
# raise ConnectionError(f'device requires comm mode 0x{prog_comm_mode:02x} for programming')
self._max_block_size = response.data[4]
self._max_cto = response.data[3]
if self._max_cto == 0 or self._max_cto % self._ag:
raise ConnectionError(
f'inconsistent device params: MAX_CTO_PGM={self._max_cto}, ADDRESS_GRANULARITY={self._ag}'
)
self._queue_size = response.data[6]
self._min_separation_time_us = 100 * response.data[5]
if not self._master_block_mode_supported_override:
self._master_block_mode_supported = (response.data[2] & 1) == 1
def set_mta(self, addr: int, addr_ext: int = 0):
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.SET_MTA.value
addr = self._swap32(addr)
msg.data[3] = addr_ext
msg.data[4] = addr & 0xff
msg.data[5] = (addr >> 8) & 0xff
msg.data[6] = (addr >> 16) & 0xff
msg.data[7] = (addr >> 24) & 0xff
timeout_s = self._timeout_s(1)
count = 0
while True:
self._drain_bus()
self._bus.send(msg)
response = self._wait_for_rx(timeout_s)
if None is response:
if count >= 3:
raise ConnectionError(f'SET_MTA failed timeout')
else:
self._sync_or_die()
count += 1
continue
if response.data[0] == XCPResponses.ERROR.value:
if response.data[1] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
count = 0
continue
if response.data[1] == XCPErrors.ERR_PGM_ACTIVE:
timeout_s = self._timeout_s(7)
continue
else:
raise ConnectionError(
f'SET_MTA failed with error code: 0x{response.data[1]:02x}'
)
break
def program_clear(self, range: int):
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.PROGRAM_CLEAR.value
range = self._swap32(range)
msg.data[1] = 0 # absolute address
msg.data[2] = 0 # reserved
msg.data[3] = 0 # reserved
msg.data[4] = range & 0xff
msg.data[5] = (range >> 8) & 0xff
msg.data[6] = (range >> 16) & 0xff
msg.data[7] = (range >> 24) & 0xff
timeout_s = self._timeout_s(1)
count = 0
while True:
self._drain_bus()
self._bus.send(msg)
response = self._wait_for_rx(timeout_s)
if None is response:
if count >= 3:
raise ConnectionError(f'PROGRAM_CLEAR failed timeout')
else:
self._sync_or_die()
count += 1
continue
if response.data[0] == XCPResponses.ERROR.value:
if response.data[0] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
count = 0
continue
else:
raise ConnectionError(
f'PROGRAM_CLEAR failed with error code: 0x{response.data[1]:02x}'
)
break
def program_reset(self):
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.PROGRAM_RESET.value
timeout_s = self._timeout_s(5)
count = 0
while True:
self._drain_bus()
self._bus.send(msg)
response = self._wait_for_rx(timeout_s)
if None is response:
if count >= 3:
raise ConnectionError(f'PROGRAM_CLEAR failed timeout')
else:
self._sync_or_die()
count += 1
continue
if response.data[0] == XCPResponses.ERROR.value:
if response.data[0] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
count = 0
continue
if response.data[0] == XCPErrors.ERR_PGM_ACTIVE:
timeout_s = self._timeout_s(7)
continue
if response.data[0] == XCPErrors.ERR_CMD_UNKNOWN:
break
else:
raise ConnectionError(
f'PROGRAM_CLEAR failed with error code: 0x{response.data[1]:02x}'
)
break
def connect(self):
"""Connect to the device"""
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.CONNECT.value
msg.data[1] = self._conn_mode
# spec, p138
if self._conn_mode == 0:
timeout_s = xcp_timeout_s(
1) # some bootloader require fast startup
else:
timeout_s = self._timeout_s(7)
while True:
self._drain_bus()
try:
self._bus.send(msg)
response = self._wait_for_rx(timeout_s)
except CanError as e:
if not XCPFlash._is_can_device_tx_queue_full(e):
raise e
response = None
time.sleep(self._timeout_s(7))
if None is response:
continue
if response.data[0] == XCPResponses.ERROR.value:
raise ConnectionAbortedError(response.data[1])
break
self._pgm = (response.data[1] & 0b00010000) != 0
self._stm = (response.data[1] & 0b00001000) != 0
self._daq = (response.data[1] & 0b00000100) != 0
self._cal_pag = (response.data[1] & 0b00000001) != 0
if not self._pgm:
raise ConnectionError(
"Flash programming not supported by the connected device")
self._max_data = response.data[4] << 8
self._max_data += response.data[5]
self._byte_order = response.data[2] & 1
if not self._ag_override:
self._ag = 1 << ((response.data[2] >> 1) & 0x3)
if self._byte_order == self.sys_byte_order():
self._swap16 = lambda x: x & 0xffff
self._swap32 = lambda x: x & 0xffffffff
else:
self._swap16 = self.swap16
self._swap32 = self.swap32
def disconnect(self, ignore_timeout: bool):
"""Disconnect from the device"""
msg = Message(arbitration_id=self._tx_id,
is_extended_id=self._extended_id,
data=bytes(8))
msg.data[0] = XCPCommands.DISCONNECT.value
timeout_s = self._timeout_s(1)
count = 0
while True:
self._drain_bus()
self._bus.send(msg)
response = self._wait_for_rx(timeout_s)
if None is response:
if ignore_timeout:
break
if count >= 3:
raise ConnectionError(f'PROGRAM_CLEAR failed timeout')
else:
self._sync_or_die()
count += 1
continue
if response.data[0] == XCPResponses.ERROR.value:
if response.data[0] == XCPErrors.ERR_CMD_BUSY:
timeout_s = self._timeout_s(7)
count = 0
continue
if response.data[0] == XCPErrors.ERR_PGM_ACTIVE:
timeout_s = self._timeout_s(7)
count = 0
continue
if response.data[0] == XCPErrors.ERR_CMD_UNKNOWN:
break
else:
raise ConnectionError(
f'PROGRAM_CLEAR failed with error code: 0x{response.data[1]:02x}'
)
break
def __call__(self, start_addr, data):
"""Flash the device
Do all the necessary steps for flashing, including connecting to the device and clearing the memory.
:param start_addr:
Start address for the firmware
:param data:
The firmware as byte-array
"""
try:
logger.info("connecting...")
self.connect()
logger.info("connected")
logger.info("start of programming")
self.program_start()
logger.info(f"set MTA to {start_addr:08x}")
self.set_mta(start_addr)
logger.info(f"clear range {len(data):08x}")
self.program_clear(len(data))
logger.info(f"program data")
self.program(data)
logger.info(f"reset")
self.program_reset()
logger.info(f"disconnect")
self.disconnect(True)
except ConnectionAbortedError as err:
if err.args[0] == "Timeout":
logger.error("\nConnection aborted: Timeout")
else:
logging.error("\nConnection aborted: {}".format(
logger.error_messages[err.args[0]]))
except ConnectionError as err:
logger.error("\nConnection error: {}".format(err))
except OSError as err:
logger.error(err)
def handle_enable_motors_homing(req):
# set up connection to hardware
can.rc['interface'] = "kvaser"
can.rc['channel'] = '0'
can.rc['bitrate'] = 500000
bus = Bus()
# Clear fault commands
for i in range(1, 8):
msg = can.Message(arbitration_id=0x600 + i,
data=[int("40", 16), int("41", 16), int("60", 16), 0, 0, 0, 0, 0],
is_extended_id=False)
bus.send(msg)
# Start remote node via NMT
# different commands can be used to set operation mode (pre-op, start, etc). For all of them the
# Cob Id is 0 in NMT mode. Data has two bytes. First byte is a desired command, one of
# the five following commands can be used
# 80 is pre-operational
# 81 is reset node
# 82 is reset communication
# 01 is start
# 02 is stop
# second byte is node id, can be 0 (all nodes) or a number between 1 to 256.
for i in range(1, 8):
msg = can.Message(arbitration_id=0x0,
data=[0x01, int("0%d\n" % i, 16)],
is_extended_id=False)
bus.send(msg)
# Enable All Motors
for i in range(1, 8):
msg = can.Message(arbitration_id=0x220 + i,
data=[0x00, 0x00],
is_extended_id=False)
bus.send(msg)
for i in range(1, 8):
msg = can.Message(arbitration_id=0x220 + i,
data=[0x06, 0x00],
is_extended_id=False)
bus.send(msg)
for i in range(1, 8):
msg = can.Message(arbitration_id=0x220 + i,
data=[0x0F, 0x00],
is_extended_id=False)
bus.send(msg)
# Set all motors to pos mode
for i in range(1, 8):
msg = can.Message(arbitration_id=0x320 + i,
data=[0x0F, 0x00, 0x01],
is_extended_id=False)
bus.send(msg)
bus.shutdown()
print("Motors ready for homing.")
return EnableMotorsResponse(req.a)
def handle_enable_motors(req):
# set up connection to hardware
can.rc['interface'] = "kvaser"
can.rc['channel'] = '0'
can.rc['bitrate'] = 500000
bus = Bus()
# Clear fault commands
for i in range(1, 8):
msg = can.Message(
arbitration_id=0x600 + i,
data=[int("40", 16),
int("41", 16),
int("60", 16), 0, 0, 0, 0, 0],
is_extended_id=False)
bus.send(msg)
# Start remote node via NMT
# different commands can be used to set operation mode (pre-op, start, etc). For all of them the
# Cob Id is 0 in NMT mode. Data has two bytes. First byte is a desired command, one of
# the five following commands can be used
# 80 is pre-operational
# 81 is reset node
# 82 is reset communication
# 01 is start
# 02 is stop
# second byte is node id, can be 0 (all nodes) or a number between 1 to 256.
for i in range(1, 8):
msg = can.Message(arbitration_id=0x0,
data=[0x01, int("0%d\n" % i, 16)],
is_extended_id=False)
bus.send(msg)
# Enable All Motors
for i in range(1, 8):
msg = can.Message(arbitration_id=0x220 + i,
data=[0x00, 0x00],
is_extended_id=False)
bus.send(msg)
for i in range(1, 8):
msg = can.Message(arbitration_id=0x220 + i,
data=[0x06, 0x00],
is_extended_id=False)
bus.send(msg)
for i in range(1, 8):
msg = can.Message(arbitration_id=0x220 + i,
data=[0x0F, 0x00],
is_extended_id=False)
bus.send(msg)
# Set all motors to position mode
# If you want to set the motors to velocity mode change
# the comand from [hex2dec('0F') hex2dec('0') hex2dec('01')] to
# [hex2dec('0F') hex2dec('0') hex2dec('03')];
for i in range(1, 8):
msg = can.Message(arbitration_id=0x320 + i,
data=[0x0F, 0x00, 0x01],
is_extended_id=False)
bus.send(msg)
# Set rotational speed of motors
for i in range(2, 8):
msg = can.Message(
arbitration_id=0x600 + i,
data=[0x22, 0x81, 0x60, 0x0, 0x88, 0x13, 0x0, 0x0], # 5000 rpm
is_extended_id=False)
bus.send(msg)
# motor 1 acceleration and deceleration to 100,000, velocity to 10,000
msg = can.Message(
arbitration_id=0x601,
data=[0x22, 0x81, 0x60, 0x0, 0x10, 0x27, 0x0, 0x0], #
is_extended_id=False)
bus.send(msg)
msg = can.Message(
arbitration_id=0x601,
data=[0x22, 0x83, 0x60, 0x0, 0xA0, 0x86, 0x01, 0x0], #
is_extended_id=False)
bus.send(msg)
msg = can.Message(
arbitration_id=0x601,
data=[0x22, 0x84, 0x60, 0x0, 0xA0, 0x86, 0x01, 0x0], #
is_extended_id=False)
bus.send(msg)
bus.shutdown()
print("Motors Enabled")
return EnableMotorsResponse(req.a)
