aniketSaha/NorthStar-HMI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added tpdo reading mechanism for faster data logging

Browse Source
This commit is contained in:
aniketSaha 2025-07-24 15:40:06 +02:00
parent e3492dd5de
commit 3db1f96489
2 changed files with 133 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

228
classCAN.py
View File

@ -1,24 +1,24 @@
import threading import threading
import canopen import canopen
import can
import time import time
import os import os
class CANBackend: class CANBackend:
def __init__(self, eds_file=None): def __init__(self, eds_file=None):
self.network = 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.connected = False
self.lock = threading.Lock() self.lock = threading.Lock()
self.polling_thread = None
self.polling_active = False
self.latest_data = { self.latest_data = {
1: {}, # PU1 1: {}, # PU1
2: {}, # PU2 2: {}, # PU2
3: {} # PU3
} }
if eds_file is None: 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: else:
self.eds_path = eds_file self.eds_path = eds_file
@ -27,20 +27,18 @@ class CANBackend:
self.network = canopen.Network() self.network = canopen.Network()
self.network.connect(channel='can0', bustype='socketcan') self.network.connect(channel='can0', bustype='socketcan')
# PU mapping: PU1->0x02, PU2->0x04, PU3->0x127 self.master_node = canopen.RemoteNode(self.master_node_id, self.eds_path)
node_map = { self.network.add_node(self.master_node)
1: 0x02, self.master_node.nmt.state = 'OPERATIONAL'
2: 0x04, self.nodes[0] = self.master_node
3: 0x127,
}
for pu_number, node_id in node_map.items(): # Start TPDO listener thread
node = canopen.RemoteNode(node_id, self.eds_path) self.listener_active = True
self.network.add_node(node) self.bus = can.interface.Bus(channel='can0', bustype='socketcan')
self.nodes[pu_number] = node self.listener_thread = threading.Thread(target=self._can_listener_loop, daemon=True)
self.listener_thread.start()
self.connected = True self.connected = True
self._start_sdo_polling()
return True return True
except Exception as e: except Exception as e:
@ -48,101 +46,132 @@ class CANBackend:
return False return False
def shutdown(self): def shutdown(self):
self.polling_active = False self.listener_active = False
if self.network: if self.network:
self.network.disconnect() self.network.disconnect()
if hasattr(self, 'bus'):
self.bus.shutdown()
self.nodes.clear() self.nodes.clear()
self.connected = False self.connected = False
def _start_sdo_polling(self): def _can_listener_loop(self):
if self.polling_thread and self.polling_thread.is_alive(): while self.listener_active:
return msg = self.bus.recv(1.0)
self.polling_active = True if msg is None:
self.polling_thread = threading.Thread(target=self._sdo_polling_loop, daemon=True) continue
self.polling_thread.start()
def _sdo_polling_loop(self):
while self.polling_active:
with self.lock:
try: try:
for pu_number, node in self.nodes.items(): cob_id = msg.arbitration_id
try: data = msg.data
fm1 = node.sdo[0x2004][1].raw
fm2 = node.sdo[0x2004][2].raw
fm3 = node.sdo[0x2004][3].raw
fm4 = node.sdo[0x2004][4].raw
ps1 = node.sdo[0x2005][1].raw with self.lock: # Ensure thread safety
ps2 = node.sdo[0x2005][2].raw
ps3 = node.sdo[0x2005][3].raw
ps4 = node.sdo[0x2005][4].raw
mv02Cmd = node.sdo[0x2014][1].raw if cob_id == 0x2A6 and len(data) >= 8:
mv03Cmd = node.sdo[0x2012][1].raw self.latest_data[1].update({
mv04Cmd = node.sdo[0x2019][1].raw "FM1": int.from_bytes(data[0:2], 'little') / 100.0 * 60.0,
mv05Cmd = node.sdo[0x2020][1].raw "FM2": int.from_bytes(data[2:4], 'little') / 100.0 * 60.0,
mv06Cmd = node.sdo[0x2021][1].raw "FM3": int.from_bytes(data[4:6], 'little') / 100.0 * 60.0,
mv07Cmd = node.sdo[0x2015][1].raw "FM4": int.from_bytes(data[6:8], 'little') / 100.0 * 60.0,
mv08Cmd = node.sdo[0x2022][1].raw })
# mv02fb = node.sdo[0x3000][2].raw elif cob_id == 0x2A7 and len(data) == 6:
# mv03fb = node.sdo[0x3000][3].raw self.latest_data[1].update({
# mv04fb = node.sdo[0x3000][4].raw "PS1": int.from_bytes(data[0:2], 'little') / 1000.0,
# mv05fb = node.sdo[0x3000][5].raw "PS2": int.from_bytes(data[2:4], 'little') / 1000.0,
# mv06fb = node.sdo[0x3000][6].raw "PS3": int.from_bytes(data[4:6], 'little') / 1000.0,
# mv07fb = node.sdo[0x3000][7].raw })
# mv08fb = node.sdo[0x3000][8].raw
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, elif cob_id == 0x2A8 and len(data) >= 8:
"PS2": ps2 / 1000.0, self.latest_data[1].update({
"PS3": ps3 / 1000.0, "MV02_sp": int.from_bytes(data[0:2], 'little') / 100.0,
"PS4": ps4 / 1000.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, elif cob_id == 0x2A9 and len(data) >= 8:
"MV03_sp" : mv03Cmd / 100.0, self.latest_data[1].update({
"MV04_sp" : mv04Cmd / 100.0, "MV06_sp": int.from_bytes(data[0:2], 'little') / 100.0,
"MV05_sp" : mv05Cmd / 100.0, "MV07_sp": int.from_bytes(data[2:4], 'little') / 100.0,
"MV06_sp" : mv06Cmd / 100.0, "MV08_sp": int.from_bytes(data[4:6], 'little') / 100.0,
"MV07_sp" : mv07Cmd / 100.0, "Pump_sp": int.from_bytes(data[6:8], 'little') / 100.0,
"MV08_sp" : mv08Cmd / 100.0, })
# "MV02" : mv02fb, elif cob_id == 0x2AA and len(data) >= 8:
# "MV03" : mv03fb, self.latest_data[1].update({
# "MV04" : mv04fb, "MV02": data[0],
# "MV05" : mv05fb, "MV03": data[1],
# "MV06" : mv06fb, "MV04": data[2],
# "MV07" : mv07fb, "MV05": data[3],
# "MV08" : mv08fb, "MV06": data[4],
} "MV07": data[5],
"MV08": data[6],
"MV09": data[7], # if used
})
print(f"[PU{pu_number}] FM1: {fm1}, PS1: {ps1}") elif cob_id == 0x2AB and len(data) >= 1:
except Exception as inner_e: self.latest_data[1]["PU1_STATE"] = data[0]
print(f"[SDO READ ERROR] PU{pu_number}: {inner_e}")
except Exception as outer_e: # ======== PU2 COB IDs ===============
print(f"[SDO POLL ERROR] {outer_e}") 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): def get_latest_data(self, pu_number: int):
with self.lock: with self.lock:
return self.latest_data.get(pu_number, {}).copy() return self.latest_data.get(pu_number, {}).copy()
def read_current_state(self, pu_number: int): def read_current_state(self, pu_number: int):
try: state_val = self.latest_data.get(pu_number, {}).get(f"PU{pu_number}_STATE")
node = self.nodes.get(pu_number) return self.decode_state(state_val) if state_val is not None else "Offline"
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"
def decode_state(self, state_val: int) -> str: def decode_state(self, state_val: int) -> str:
state_map = { state_map = {
@ -182,18 +211,21 @@ class CANBackend:
raise ValueError(f"Invalid state: {state}") raise ValueError(f"Invalid state: {state}")
try: try:
node = self.nodes.get(pu_number) master_node = self.nodes.get(0)
if node is None: if master_node is None:
raise ValueError(f"PU{pu_number} not connected") raise ValueError("Master node not connected")
print(f"[DEBUG] Writing state {state_map[state]} to 0x2024:{pu_number}") state_index = 0x3000
node.sdo[0x2024][0x01].raw = state_map[state] setpoint_index = 0x3001
print(f"[DEBUG] Writing ploop_setpoint {ploop_setpoint} to 0x2007") print(f"[DEBUG] Writing state {state_map[state]} to master OD 0x{state_index:04X}:{pu_number:02X}")
node.sdo[0x2007].raw = int(ploop_setpoint * 100) 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: except Exception as e:
print(f"[SDO WRITE ERROR] PU{pu_number}: {e}") print(f"[MASTER SDO WRITE ERROR] PU{pu_number}: {e}")
raise raise
def send_thermal_loop_cleaning(self, mode: str, pu_number: int): def send_thermal_loop_cleaning(self, mode: str, pu_number: int):

5
main.py
View File

@ -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}") 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: try:
can_backend.send_state_command(state, pu_number, ploop_setpoint) can_backend.send_state_command(state, pu_number, ploop_setpoint)
current_state = can_backend.read_current_state(pu_number) current_state = can_backend.read_current_state(pu_number)