tcp_can_bridge.py

import socket
import can
import threading
import time
import logging
import sys

# --- KONFIGURACJA LOGOWANIA (Zoptymalizowana pod systemd / journald) ---
logging.basicConfig(
    level=logging.INFO,
    format='[%(levelname)s] %(message)s',
    handlers=[
        logging.StreamHandler(sys.stdout)
    ]
)

# --- STAŁE I USTAWIENIA SIECIOWE ---
TCP_HOST = '0.0.0.0'
TCP_PORTS = [8888, 8889, 8890]

# --- CAN IDS & RANGES (Zgodnie z dokumentacją MD) ---
PLATFORM_MAX_ID = 127
MANIP_LAB_MIN_ID = 128
MANIP_FRAME_1 = 129
MANIP_FRAME_2 = 130
MANIP_FRAME_3 = 131
FRAME_LENGTH = 19

# --- ZMIENNE GLOBALNE ---
active_connections = []
conn_lock = threading.Lock()

# Bufor dla manipulatora
manipulator_buffer = {}
manip_buffer_lock = threading.Lock()
last_manip_frame_time = 0.0
MANIP_TIMEOUT = 0.1  # 100 ms na komplet ramek

def setup_can_buses():
    """Inicjalizacja magistral CAN."""
    buses = {}
    try:
        buses['platform'] = [
            can.interface.Bus(channel='can_r', bustype='socketcan'),
            can.interface.Bus(channel='can_l', bustype='socketcan')
        ]
        buses['manip_lab'] = [
            can.interface.Bus(channel='can_mani', bustype='socketcan')
        ]
        logging.info("Pomyślnie zainicjowano interfejsy can_r, can_l oraz can_mani.")
        return buses
    except Exception as e:
        logging.error(f"Krytyczny błąd inicjalizacji CAN: {e}")
        exit(1)

def can_to_tcp_callback(msg):
    """Odbiera ramki ze wszystkich magistral CAN i rozsyła je do podłączonych aplikacji TCP."""
    global active_connections
    
    id_hex = f"{msg.arbitration_id:02X}"[-2:] 
    data_hex = "".join([f"{b:02X}" for b in msg.data]) 
    padding = "x" * (16 - len(data_hex)) 
    
    frame_str = f"#{id_hex}{data_hex}{padding}"
    frame_bytes = frame_str.encode('ascii')
    
    with conn_lock:
        for conn in list(active_connections):
            try:
                conn.sendall(frame_bytes)
            except Exception as e:
                # Gniazdo umarło - wyrzucamy z listy (Broken pipe)
                active_connections.remove(conn)
                logging.warning(f"Usunięto martwe gniazdo po błędzie wysyłania: {e}")

def parse_and_send_to_can(frame_string, buses):
    """Dekodowanie 19-znakowej ramki z TCP i routing na odpowiedni interfejs CAN."""
    global manipulator_buffer, last_manip_frame_time
    
    try:
        can_id = int(frame_string[1:3], 16)
        data_hex_part = frame_string[3:19]
        
        # Szybka konwersja C-level, ignorująca znaki paddingu 'x'
        clean_hex = data_hex_part.lower().replace('x', '')
        can_data = bytes.fromhex(clean_hex)
            
        msg = can.Message(arbitration_id=can_id, data=can_data, is_extended_id=False)
        
        # --- ROUTING PLATFORMY ---
        if 0 <= can_id <= PLATFORM_MAX_ID:
            for bus in buses['platform']:
                bus.send(msg)
                
        # --- ROUTING MANIPULATORA I LABORATORIUM ---
        elif can_id >= MANIP_LAB_MIN_ID:
            target_buses = buses['manip_lab']
            
            # Obsługa specjalna dla trójek ramek manipulatora (129, 130, 131)
            if can_id in [MANIP_FRAME_1, MANIP_FRAME_2, MANIP_FRAME_3]:
                with manip_buffer_lock:
                    current_time = time.time()
                    
                    # Zabezpieczenie przed sklejaniem starych ramek (Timeout / Reset na 129)
                    if current_time - last_manip_frame_time > MANIP_TIMEOUT or can_id == MANIP_FRAME_1:
                        manipulator_buffer.clear()
                        
                    manipulator_buffer[can_id] = msg
                    last_manip_frame_time = current_time
                    
                    # Jeśli mamy komplet, puszczamy na magistralę
                    if MANIP_FRAME_1 in manipulator_buffer and \
                       MANIP_FRAME_2 in manipulator_buffer and \
                       MANIP_FRAME_3 in manipulator_buffer:
                        
                        for bus in target_buses:
                            bus.send(manipulator_buffer[MANIP_FRAME_1])
                            bus.send(manipulator_buffer[MANIP_FRAME_2])
                            bus.send(manipulator_buffer[MANIP_FRAME_3])
                            
                        manipulator_buffer.clear()
            else:
                # Wszelkie inne ramki (np. 128 Start/Stop) puszczamy od razu
                for bus in target_buses:
                    bus.send(msg)
            
    except (ValueError, IndexError):
        # Ignorowanie źle sformatowanych ramek (np. uszkodzonych w locie)
        pass
    except can.CanError as e:
        logging.error(f"Błąd nadawania magistrali CAN (ID {can_id}): {e}")

def handle_client(conn, addr, port, buses):
    """Wątek obsługujący pojedynczego klienta TCP."""
    with conn_lock:
        active_connections.append(conn)
        
    buffer = ""
    try:
        while True:
            data = conn.recv(1024)
            if not data:
                break 
                
            buffer += data.decode('ascii', errors='ignore')
            
            # Szybkie wycinanie ramek z bufora
            while '#' in buffer:
                start_idx = buffer.find('#')
                buffer = buffer[start_idx:] 
                
                if len(buffer) >= FRAME_LENGTH:
                    frame = buffer[:FRAME_LENGTH]
                    parse_and_send_to_can(frame, buses)
                    buffer = buffer[FRAME_LENGTH:] 
                else:
                    break 
                    
    except ConnectionResetError:
        logging.info(f"[{addr}:{port}] Aplikacja zerwała połączenie.")
    except Exception as e:
        logging.error(f"[{addr}:{port}] Błąd strumienia: {e}")
    finally:
        with conn_lock:
            if conn in active_connections:
                active_connections.remove(conn)
        conn.close()
        logging.info(f"[{addr}:{port}] Zamknięto połączenie.")

def listen_on_port(port, buses):
    """Uruchamia serwer nasłuchujący z aktywnym mechanizmem Keep-Alive."""
    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    
    # 1. Włączenie Keep-Alive na gnieździe
    server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
    
    # 2. Agresywne parametry dla Linuksa (wykrycie zerwania po ~6 sek)
    if hasattr(socket, 'TCP_KEEPIDLE'):
        server_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 3)
        server_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 1)
        server_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 3)
    
    try:
        server_socket.bind((TCP_HOST, port))
        server_socket.listen(5)
        logging.info(f"Nasłuchiwanie na porcie {port} (SO_KEEPALIVE aktywny)...")
        
        while True:
            conn, addr = server_socket.accept()
            logging.info(f"Nowe połączenie: {addr} na porcie {port}")
            threading.Thread(target=handle_client, args=(conn, addr, port, buses), daemon=True).start()
            
    except Exception as e:
        logging.error(f"Błąd na porcie {port}: {e}")
    finally:
        server_socket.close()

def main():
    buses = setup_can_buses()
    all_buses = buses['platform'] + buses['manip_lab']
    
    # Uruchomienie nasłuchu z CAN
    notifier = can.Notifier(all_buses, [can_to_tcp_callback])
    
    # Start wątków serwerowych dla portów TCP
    server_threads = []
    for port in TCP_PORTS:
        t = threading.Thread(target=listen_on_port, args=(port, buses), daemon=True)
        t.start()
        server_threads.append(t)
    
    logging.info(f"System gotowy. Nasłuch na portach: {TCP_PORTS}. Zatrzymanie przez SIGTERM/SIGINT.")
    
    try:
        # Utrzymanie głównego wątku przy życiu
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        logging.info("Otrzymano sygnał zamknięcia. Zamykanie usług...")
    finally:
        notifier.stop()
        
        with conn_lock:
            for conn in active_connections:
                try:
                    conn.close()
                except:
                    pass
            active_connections.clear()
            
        for bus in all_buses:
            bus.shutdown()
        logging.info("Zamknięto pomyślnie. Do widzenia.")

if __name__ == "__main__":
    main()