From 3db1f96489db6e6d8e959a5ffce31f3d653234f4 Mon Sep 17 00:00:00 2001 From: aniketSaha Date: Thu, 24 Jul 2025 15:40:06 +0200 Subject: [PATCH] Added tpdo reading mechanism for faster data logging --- classCAN.py | 234 +++++++++++++++++++++++++++++----------------------- main.py | 5 -- 2 files changed, 133 insertions(+), 106 deletions(-) diff --git a/classCAN.py b/classCAN.py index 9775185..2736c65 100644 --- a/classCAN.py +++ b/classCAN.py @@ -1,24 +1,24 @@ import threading import canopen +import can import time import os - class CANBackend: - def __init__(self, eds_file =None): + def __init__(self, eds_file=None): self.network = None - self.nodes = {} # {1: RemoteNode(0x02), 2: RemoteNode(0x03), ...} + self.master_node = None + self.master_node_id = 0x16 + self.nodes = {} self.connected = False self.lock = threading.Lock() - self.polling_thread = None - self.polling_active = False self.latest_data = { 1: {}, # PU1 2: {}, # PU2 - 3: {} # PU3 } + if eds_file is None: - self.eds_path = os.path.join(os.path.dirname(__file__), "eds_file", "processBoard_0.eds") + self.eds_path = os.path.join(os.path.dirname(__file__), "eds_file", "dockingBoard_0.eds") else: self.eds_path = eds_file @@ -27,20 +27,18 @@ class CANBackend: self.network = canopen.Network() self.network.connect(channel='can0', bustype='socketcan') - # PU mapping: PU1->0x02, PU2->0x04, PU3->0x127 - node_map = { - 1: 0x02, - 2: 0x04, - 3: 0x127, - } + self.master_node = canopen.RemoteNode(self.master_node_id, self.eds_path) + self.network.add_node(self.master_node) + self.master_node.nmt.state = 'OPERATIONAL' + self.nodes[0] = self.master_node - for pu_number, node_id in node_map.items(): - node = canopen.RemoteNode(node_id, self.eds_path) - self.network.add_node(node) - self.nodes[pu_number] = node + # Start TPDO listener thread + self.listener_active = True + self.bus = can.interface.Bus(channel='can0', bustype='socketcan') + self.listener_thread = threading.Thread(target=self._can_listener_loop, daemon=True) + self.listener_thread.start() self.connected = True - self._start_sdo_polling() return True except Exception as e: @@ -48,101 +46,132 @@ class CANBackend: return False def shutdown(self): - self.polling_active = False + self.listener_active = False if self.network: self.network.disconnect() + if hasattr(self, 'bus'): + self.bus.shutdown() self.nodes.clear() self.connected = False - def _start_sdo_polling(self): - if self.polling_thread and self.polling_thread.is_alive(): - return - self.polling_active = True - self.polling_thread = threading.Thread(target=self._sdo_polling_loop, daemon=True) - self.polling_thread.start() + def _can_listener_loop(self): + while self.listener_active: + msg = self.bus.recv(1.0) + if msg is None: + continue - def _sdo_polling_loop(self): - while self.polling_active: - with self.lock: - try: - for pu_number, node in self.nodes.items(): - try: - fm1 = node.sdo[0x2004][1].raw - fm2 = node.sdo[0x2004][2].raw - fm3 = node.sdo[0x2004][3].raw - fm4 = node.sdo[0x2004][4].raw + try: + cob_id = msg.arbitration_id + data = msg.data - ps1 = node.sdo[0x2005][1].raw - ps2 = node.sdo[0x2005][2].raw - ps3 = node.sdo[0x2005][3].raw - ps4 = node.sdo[0x2005][4].raw + with self.lock: # Ensure thread safety - mv02Cmd = node.sdo[0x2014][1].raw - mv03Cmd = node.sdo[0x2012][1].raw - mv04Cmd = node.sdo[0x2019][1].raw - mv05Cmd = node.sdo[0x2020][1].raw - mv06Cmd = node.sdo[0x2021][1].raw - mv07Cmd = node.sdo[0x2015][1].raw - mv08Cmd = node.sdo[0x2022][1].raw + if cob_id == 0x2A6 and len(data) >= 8: + self.latest_data[1].update({ + "FM1": int.from_bytes(data[0:2], 'little') / 100.0 * 60.0, + "FM2": int.from_bytes(data[2:4], 'little') / 100.0 * 60.0, + "FM3": int.from_bytes(data[4:6], 'little') / 100.0 * 60.0, + "FM4": int.from_bytes(data[6:8], 'little') / 100.0 * 60.0, + }) - # mv02fb = node.sdo[0x3000][2].raw - # mv03fb = node.sdo[0x3000][3].raw - # mv04fb = node.sdo[0x3000][4].raw - # mv05fb = node.sdo[0x3000][5].raw - # mv06fb = node.sdo[0x3000][6].raw - # mv07fb = node.sdo[0x3000][7].raw - # mv08fb = node.sdo[0x3000][8].raw + elif cob_id == 0x2A7 and len(data) == 6: + self.latest_data[1].update({ + "PS1": int.from_bytes(data[0:2], 'little') / 1000.0, + "PS2": int.from_bytes(data[2:4], 'little') / 1000.0, + "PS3": int.from_bytes(data[4:6], 'little') / 1000.0, + }) - self.latest_data[pu_number] = { - "FM1": (fm1 / 100.0) * 60.0, - "FM2": (fm2 / 100.0) * 60.0, - "FM3": (fm3 / 100.0) * 60.0, - "FM4": (fm4 / 100.0) * 60.0, - "PS1": ps1 / 1000.0, - "PS2": ps2 / 1000.0, - "PS3": ps3 / 1000.0, - "PS4": ps4 / 1000.0, + elif cob_id == 0x2A8 and len(data) >= 8: + self.latest_data[1].update({ + "MV02_sp": int.from_bytes(data[0:2], 'little') / 100.0, + "MV03_sp": int.from_bytes(data[2:4], 'little') / 100.0, + "MV04_sp": int.from_bytes(data[4:6], 'little') / 100.0, + "MV05_sp": int.from_bytes(data[6:8], 'little') / 100.0, + }) - "MV02_sp" : mv02Cmd / 100.0, - "MV03_sp" : mv03Cmd / 100.0, - "MV04_sp" : mv04Cmd / 100.0, - "MV05_sp" : mv05Cmd / 100.0, - "MV06_sp" : mv06Cmd / 100.0, - "MV07_sp" : mv07Cmd / 100.0, - "MV08_sp" : mv08Cmd / 100.0, + elif cob_id == 0x2A9 and len(data) >= 8: + self.latest_data[1].update({ + "MV06_sp": int.from_bytes(data[0:2], 'little') / 100.0, + "MV07_sp": int.from_bytes(data[2:4], 'little') / 100.0, + "MV08_sp": int.from_bytes(data[4:6], 'little') / 100.0, + "Pump_sp": int.from_bytes(data[6:8], 'little') / 100.0, + }) - # "MV02" : mv02fb, - # "MV03" : mv03fb, - # "MV04" : mv04fb, - # "MV05" : mv05fb, - # "MV06" : mv06fb, - # "MV07" : mv07fb, - # "MV08" : mv08fb, - } + elif cob_id == 0x2AA and len(data) >= 8: + self.latest_data[1].update({ + "MV02": data[0], + "MV03": data[1], + "MV04": data[2], + "MV05": data[3], + "MV06": data[4], + "MV07": data[5], + "MV08": data[6], + "MV09": data[7], # if used + }) - print(f"[PU{pu_number}] FM1: {fm1}, PS1: {ps1}") - except Exception as inner_e: - print(f"[SDO READ ERROR] PU{pu_number}: {inner_e}") - except Exception as outer_e: - print(f"[SDO POLL ERROR] {outer_e}") + elif cob_id == 0x2AB and len(data) >= 1: + self.latest_data[1]["PU1_STATE"] = data[0] + + # ======== PU2 COB IDs =============== + elif cob_id == 0x2AD and len(data) >= 8: + self.latest_data[2].update({ + "FM1": int.from_bytes(data[0:2], 'little') / 100.0 * 60.0, + "FM2": int.from_bytes(data[2:4], 'little') / 100.0 * 60.0, + "FM3": int.from_bytes(data[4:6], 'little') / 100.0 * 60.0, + "FM4": int.from_bytes(data[6:8], 'little') / 100.0 * 60.0, + }) + + elif cob_id == 0x2AE and len(data) == 6: + self.latest_data[2].update({ + "PS1": int.from_bytes(data[0:2], 'little') / 1000.0, + "PS2": int.from_bytes(data[2:4], 'little') / 1000.0, + "PS3": int.from_bytes(data[4:6], 'little') / 1000.0, + }) + + + elif cob_id == 0x2AF and len(data) >= 8: + self.latest_data[2].update({ + "MV02_sp": int.from_bytes(data[0:2], 'little') / 100.0, + "MV03_sp": int.from_bytes(data[2:4], 'little') / 100.0, + "MV04_sp": int.from_bytes(data[4:6], 'little') / 100.0, + "MV05_sp": int.from_bytes(data[6:8], 'little') / 100.0, + }) + + elif cob_id == 0x2B0 and len(data) >= 8: + self.latest_data[2].update({ + "MV06_sp": int.from_bytes(data[0:2], 'little') / 100.0, + "MV07_sp": int.from_bytes(data[2:4], 'little') / 100.0, + "MV08_sp": int.from_bytes(data[4:6], 'little') / 100.0, + "Pump_sp": int.from_bytes(data[6:8], 'little') / 100.0, + }) + + elif cob_id == 0x2B1 and len(data) >= 8: + self.latest_data[2].update({ + "MV02": data[0], + "MV03": data[1], + "MV04": data[2], + "MV05": data[3], + "MV06": data[4], + "MV07": data[5], + "MV08": data[6], + "MV09": data[7], # if used + }) + + elif cob_id == 0x2B2 and len(data) >= 1: + self.latest_data[2]["PU2_STATE"] = data[0] + + except Exception as e: + print(f"[TPDO PARSE ERROR] {e}") - time.sleep(1.0) def get_latest_data(self, pu_number: int): with self.lock: return self.latest_data.get(pu_number, {}).copy() def read_current_state(self, pu_number: int): - try: - node = self.nodes.get(pu_number) - if node is None: - return "Offline" - state_raw = node.sdo[0x2000].raw - return self.decode_state(state_raw) - except Exception as e: - print(f"[PU{pu_number} READ ERROR] {e}") - return "Offline" + state_val = self.latest_data.get(pu_number, {}).get(f"PU{pu_number}_STATE") + return self.decode_state(state_val) if state_val is not None else "Offline" def decode_state(self, state_val: int) -> str: state_map = { @@ -172,7 +201,7 @@ class CANBackend: state_map = { "IDLE": 1, "PRE-PRODUCTION": 2, - "PRODUCTION" : 3, + "PRODUCTION": 3, "MAINTENANCE": 8, "EMERGENCY_STOP": 9, "FIRST_START": 10 @@ -182,18 +211,21 @@ class CANBackend: raise ValueError(f"Invalid state: {state}") try: - node = self.nodes.get(pu_number) - if node is None: - raise ValueError(f"PU{pu_number} not connected") + master_node = self.nodes.get(0) + if master_node is None: + raise ValueError("Master node not connected") - print(f"[DEBUG] Writing state {state_map[state]} to 0x2024:{pu_number}") - node.sdo[0x2024][0x01].raw = state_map[state] + state_index = 0x3000 + setpoint_index = 0x3001 - print(f"[DEBUG] Writing ploop_setpoint {ploop_setpoint} to 0x2007") - node.sdo[0x2007].raw = int(ploop_setpoint * 100) + print(f"[DEBUG] Writing state {state_map[state]} to master OD 0x{state_index:04X}:{pu_number:02X}") + master_node.sdo[state_index][pu_number].raw = state_map[state] & 0xFF + + print(f"[DEBUG] Writing ploop_setpoint {ploop_setpoint} to master OD 0x{setpoint_index:04X}:{pu_number:02X}") + master_node.sdo[setpoint_index][pu_number].raw = int(ploop_setpoint * 100) except Exception as e: - print(f"[SDO WRITE ERROR] PU{pu_number}: {e}") + print(f"[MASTER SDO WRITE ERROR] PU{pu_number}: {e}") raise def send_thermal_loop_cleaning(self, mode: str, pu_number: int): diff --git a/main.py b/main.py index eaedc77..fb32254 100644 --- a/main.py +++ b/main.py @@ -205,11 +205,6 @@ def send_command(state: str, pu_number: int, ploop_setpoint: float = Query(...)) logging.info(f"Sending state '{state}' to PU {pu_number}") - if state == "PRE-PRODUCTION": - valve_backend.send_command(70) - elif "IDLE": - valve_backend.send_command(DEFAULT_FEED_VALVE) - try: can_backend.send_state_command(state, pu_number, ploop_setpoint) current_state = can_backend.read_current_state(pu_number)