diff --git a/can/interfaces/__init__.py b/can/interfaces/__init__.py index f220d28e5..f65d9c741 100644 --- a/can/interfaces/__init__.py +++ b/can/interfaces/__init__.py @@ -27,6 +27,7 @@ "slcan", "socketcan", "socketcand", + "morphle_eth2can", "systec", "udp_multicast", "usb2can", @@ -59,6 +60,7 @@ "neousys": ("can.interfaces.neousys", "NeousysBus"), "etas": ("can.interfaces.etas", "EtasBus"), "socketcand": ("can.interfaces.socketcand", "SocketCanDaemonBus"), + "morphle_eth2can": ("can.interfaces.morphle_eth2can", "MorphleCanBus"), } diff --git a/can/interfaces/morphle_eth2can/__init__.py b/can/interfaces/morphle_eth2can/__init__.py new file mode 100644 index 000000000..b7e894482 --- /dev/null +++ b/can/interfaces/morphle_eth2can/__init__.py @@ -0,0 +1,14 @@ +""" +Interface to socketcand +see https://github.com/linux-can/socketcand + +Copyright (C) 2021 DOMOLOGIC GmbH +http://www.domologic.de +""" + +__all__ = [ + "MorphleCanBus", + "morphle_eth2can", +] + +from .morphle_eth2can import MorphleCanBus diff --git a/can/interfaces/morphle_eth2can/morphle_eth2can.py b/can/interfaces/morphle_eth2can/morphle_eth2can.py new file mode 100644 index 000000000..4f89053eb --- /dev/null +++ b/can/interfaces/morphle_eth2can/morphle_eth2can.py @@ -0,0 +1,157 @@ +""" +Interface to morphle socket eth2can +Authors: Ashish Manmode +https://www.uotek.com/pro_view-236.html +https://www.uotek.com/Uploads/file/20230210/20230210143551_12219.pdf +""" +import logging +import select +import socket +import struct +import time +import traceback +from collections import deque + +import can + +log = logging.getLogger(__name__) + + +def connect_to_server(s, host, port): + timeout_ms = 10000 + now = time.time() * 1000 + end_time = now + timeout_ms + while now < end_time: + try: + s.connect((host, port)) + return + except Exception as e: + log.warning(f"Failed to bind to server: {type(e)} Message: {e}") + now = time.time() * 1000 + raise TimeoutError( + f"connect_to_server: Failed to connect server for {timeout_ms} ms" + ) + + +class MorphleCanBus(can.BusABC): + def __init__(self, host, port, can_filters=None, **kwargs): + """Connects to a CAN bus served by socketcand. + + 1. Make UOTEK can port as server + 2. Connect from MorphleCanBus + 3. Create multiple can handlers for connecting to different can bus + + It will attempt to connect to the server for up to 10s, after which a + TimeoutError exception will be thrown. + + If the handshake with the socketcand server fails, a CanError exception + is thrown. + + :param host: + The host address of the socketcand server. + :param port: + The port of the socketcand server. + :param can_filters: + See :meth:`can.BusABC.set_filters`. + """ + + # Below parameters are taken from UOTek documentation + # refer to https://www.uotek.com/Uploads/file/20230210/20230210143551_12219.pdf + self.__COMMAND_STRUCT_HEADER = ">BI" + self.__ethcan_message_fixed_len = 13 + self.__ethcan_message_head = 0x08 + + self.__host = host + self.__port = port + + self.__socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.__message_buffer = deque() + self.__receive_buffer = [] + self.channel_info = f"morphle_eth2can connecting on {host}:{port}" + connect_to_server(self.__socket, self.__host, self.__port) + + log.info( + f"morphle_eth2can: started socket server at address {self.__socket.getsockname()}" + ) + + super().__init__(channel=None, can_filters=can_filters, **kwargs) + + def _recv_internal(self, timeout): + if len(self.__message_buffer) != 0: + can_message = self.__message_buffer.popleft() + return can_message, False + + try: + # get all sockets that are ready (can be a list with a single value + # being self.socket or an empty list if self.socket is not ready) + ready_receive_sockets, _, _ = select.select( + [self.__socket], [], [], timeout + ) + except OSError as exc: + # something bad happened (e.g. the interface went down) + log.error(f"Failed to receive: {exc}") + raise can.CanError(f"Failed to receive: {exc}") + + try: + if not ready_receive_sockets: + # socket wasn't readable or timeout occurred + log.debug("Socket not ready") + return None, False + + msg = self.__socket.recv(1024) # may contain multiple messages + log.debug("received message-len={}, message={}".format(len(msg), msg)) + self.__receive_buffer += msg + + for i in range(int(len(self.__receive_buffer) / self.__ethcan_message_fixed_len)): + can_frame = self.__receive_buffer[ + i * self.__ethcan_message_fixed_len:(i + 1) * self.__ethcan_message_fixed_len] + if self.__receive_buffer[i * self.__ethcan_message_fixed_len] == self.__ethcan_message_head: + print("[{}] full eth2can message: {}".format(i, can_frame)) + self.__message_buffer.append(can.Message( + arbitration_id=struct.unpack(self.__COMMAND_STRUCT_HEADER, bytes(can_frame[:5]))[1], + data=can_frame[5:], + is_extended_id=False, + timestamp=0.0, + )) + else: + print("[{}] invalid eth2can message, Please check the eth2can configuration. " + " Contact the Author more details: {}".format(i, can_frame)) + + self.__receive_buffer = self.__receive_buffer[ + int(len(self.__receive_buffer) / self.__ethcan_message_fixed_len) * + self.__ethcan_message_fixed_len:] + + can_message = ( + None + if len(self.__message_buffer) == 0 + else self.__message_buffer.popleft() + ) + log.info("received can message: " + str(can_message)) + return can_message, False + + except Exception as exc: + log.error(f"Failed to receive: {exc} {traceback.format_exc()}") + raise can.CanError(f"Failed to receive: {exc} {traceback.format_exc()}") + + def _tcp_send(self, msg): + log.info(f"Sending TCP Message: '{msg}'") + self.__socket.sendall(msg) + + def send(self, msg, timeout=None): + """Transmit a message to the CAN bus. + + :param msg: A message object. + :param timeout: Ignored + """ + log.info("canMessage arbitration_id={} data={} dlc={} timestamp={}".format(msg.arbitration_id, + msg.data, msg.dlc, msg.timestamp)) + header_payload = struct.pack(self.__COMMAND_STRUCT_HEADER, self.__ethcan_message_head, msg.arbitration_id) + homing_payload = header_payload + msg.data + + log.debug("payload to be sent=" + str([hex(a) for a in homing_payload])) + self._tcp_send(homing_payload) + + def shutdown(self): + """Stops all active periodic tasks and closes the socket.""" + super().shutdown() + self.__socket.close()