Added tpdo reading mechanism for faster data logging
This commit is contained in:
parent
e3492dd5de
commit
3db1f96489
234
classCAN.py
234
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):
|
||||
|
|
|
|||
5
main.py
5
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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user