From e184bf5ec112369f7b858eabdefc4a204e1ce655 Mon Sep 17 00:00:00 2001 From: Grant Weiss Date: Mon, 29 Jul 2024 13:22:25 -0400 Subject: [PATCH 1/6] Add nmt state change callbacks --- canopen/nmt.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/canopen/nmt.py b/canopen/nmt.py index 8ce737ea..2d5a75d7 100644 --- a/canopen/nmt.py +++ b/canopen/nmt.py @@ -47,6 +47,15 @@ def __init__(self, node_id: int): self.id = node_id self.network = None self._state = 0 + self._state_change_callbacks = [] + + def add_state_change_callback(self, callback: Callable[[str, str], None]): + """Add function to be called on nmt state change. + + :param callback: + Function that should accept an old NMT state and new NMT state as arguments. + """ + self._state_change_callbacks.append(callback) def on_command(self, can_id, data, timestamp): cmd, node_id = struct.unpack_from("BB", data) @@ -57,6 +66,8 @@ def on_command(self, can_id, data, timestamp): if new_state != self._state: logger.info("New NMT state %s, old state %s", NMT_STATES[new_state], NMT_STATES[self._state]) + for callback in self._state_change_callbacks: + callback(old_state = NMT_STATES[self._state], new_state = NMT_STATES[new_state]) self._state = new_state def send_command(self, code: int): @@ -69,6 +80,9 @@ def send_command(self, code: int): new_state = COMMAND_TO_STATE[code] logger.info("Changing NMT state on node %d from %s to %s", self.id, NMT_STATES[self._state], NMT_STATES[new_state]) + if new_state != self._state: + for callback in self._state_change_callbacks: + callback(old_state = NMT_STATES[self._state], new_state = NMT_STATES[new_state]) self._state = new_state @property From 853f7eb099d5100c2a6720c1ceb9695aa27373d7 Mon Sep 17 00:00:00 2001 From: Grant Weiss Date: Mon, 29 Jul 2024 13:30:32 -0400 Subject: [PATCH 2/6] Enable local node PDOs on transition to operational --- canopen/node/local.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/canopen/node/local.py b/canopen/node/local.py index eb74b98d..27a9a3d3 100644 --- a/canopen/node/local.py +++ b/canopen/node/local.py @@ -34,6 +34,8 @@ def __init__( self.add_write_callback(self.nmt.on_write) self.emcy = EmcyProducer(0x80 + self.id) + self.nmt.add_state_change_callback(self.nmt_state_changed) + def associate_network(self, network): self.network = network self.sdo.network = network @@ -127,3 +129,19 @@ def _find_object(self, index, subindex): raise SdoAbortedError(0x06090011) obj = obj[subindex] return obj + + def nmt_state_changed(self, old_state, new_state): + if new_state == "OPERATIONAL": + for i, pdo in self.tpdo.map.items(): + if pdo.enabled: + try: + pdo.start() + logger.info(f"Successfully started TPDO {i}") + except ValueError: + logger.warning(f"Failed to start TPDO {i} due to missing period") + except Exception as e: + logger.error(f"Unknown error starting TPDO {i}: {str(e)}") + else: + logger.info(f"TPDO {i} not enabled") + elif old_state == "OPERATIONAL": + self.tpdo.stop() \ No newline at end of file From 290a358d2b27dedcac0462c2965442dd3f8f9684 Mon Sep 17 00:00:00 2001 From: Grant Weiss Date: Mon, 29 Jul 2024 13:47:58 -0400 Subject: [PATCH 3/6] Add unit test for local node pdo transmissions --- test/test_local.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/test/test_local.py b/test/test_local.py index 9c5fc0c1..fe2e3a67 100644 --- a/test/test_local.py +++ b/test/test_local.py @@ -200,6 +200,15 @@ def test_save(self): self.remote_node.pdo.save() self.local_node.pdo.save() + def test_send_pdo_on_operational(self): + self.local_node.tpdo[1].period = 0.5 + + self.local_node.nmt.state = 'INITIALISING' + self.local_node.nmt.state = 'PRE-OPERATIONAL' + self.local_node.nmt.state = 'OPERATIONAL' + + self.assertNotEqual(self.local_node.tpdo[1]._task, None) + if __name__ == "__main__": unittest.main() From b5e546f3138a1ed978a79f6ea1ebba63ed9079f2 Mon Sep 17 00:00:00 2001 From: Grant Weiss Date: Mon, 29 Jul 2024 15:44:52 -0400 Subject: [PATCH 4/6] Make new callback method private --- canopen/node/local.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/canopen/node/local.py b/canopen/node/local.py index 27a9a3d3..46b281e1 100644 --- a/canopen/node/local.py +++ b/canopen/node/local.py @@ -34,7 +34,7 @@ def __init__( self.add_write_callback(self.nmt.on_write) self.emcy = EmcyProducer(0x80 + self.id) - self.nmt.add_state_change_callback(self.nmt_state_changed) + self.nmt.add_state_change_callback(self._nmt_state_changed) def associate_network(self, network): self.network = network @@ -130,7 +130,7 @@ def _find_object(self, index, subindex): obj = obj[subindex] return obj - def nmt_state_changed(self, old_state, new_state): + def _nmt_state_changed(self, old_state, new_state): if new_state == "OPERATIONAL": for i, pdo in self.tpdo.map.items(): if pdo.enabled: From 822009ce5ccde18ac2887f364ef4ad10e52c82da Mon Sep 17 00:00:00 2001 From: Grant Weiss Date: Mon, 29 Jul 2024 15:56:52 -0400 Subject: [PATCH 5/6] Add sdo write callback to local node for tpdo configuration --- canopen/node/local.py | 23 ++++++++++++++++++++++- test/test_local.py | 22 ++++++++++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/canopen/node/local.py b/canopen/node/local.py index 46b281e1..20be5cb3 100644 --- a/canopen/node/local.py +++ b/canopen/node/local.py @@ -35,6 +35,7 @@ def __init__( self.emcy = EmcyProducer(0x80 + self.id) self.nmt.add_state_change_callback(self._nmt_state_changed) + self.add_write_callback(self._tpdo_configuration_write) def associate_network(self, network): self.network = network @@ -144,4 +145,24 @@ def _nmt_state_changed(self, old_state, new_state): else: logger.info(f"TPDO {i} not enabled") elif old_state == "OPERATIONAL": - self.tpdo.stop() \ No newline at end of file + self.tpdo.stop() + + def _tpdo_configuration_write(self, index, subindex, od, data): + if 0x1800 <= index <= 0x19FF: + # Only allowed to edit pdo configuration in pre-op + if self.nmt.state != "PRE-OPERATIONAL": + logger.warning("Tried to configure tpdo when not in pre-op") + return + + if subindex == 0x01: + if len(data) == 4: + tpdoNum = (index - 0x1800) + 1 + if tpdoNum in self.tpdo.map.keys(): + PDO_NOT_VALID = 1 << 31 + RTR_NOT_ALLOWED = 1 << 30 + + cob_id = int.from_bytes(data, 'little') & 0x7FF + + self.tpdo.map[tpdoNum].cob_id = cob_id & 0x1FFFFFFF + self.tpdo.map[tpdoNum].enabled = cob_id & PDO_NOT_VALID == 0 + self.tpdo.map[tpdoNum].rtr_allowed = cob_id & RTR_NOT_ALLOWED == 0 \ No newline at end of file diff --git a/test/test_local.py b/test/test_local.py index fe2e3a67..554697a0 100644 --- a/test/test_local.py +++ b/test/test_local.py @@ -209,6 +209,28 @@ def test_send_pdo_on_operational(self): self.assertNotEqual(self.local_node.tpdo[1]._task, None) + def test_config_pdo(self): + # Disable tpdo 1 + self.local_node.tpdo[1].enabled = False + self.local_node.tpdo[1].cob_id = 0 + self.local_node.tpdo[1].period = 0.5 # manually assign a period + + self.local_node.nmt.state = 'INITIALISING' + self.local_node.nmt.state = 'PRE-OPERATIONAL' + + # Attempt to re-enable tpdo 1 via sdo writing + PDO_NOT_VALID = 1 << 31 + odDefaultVal = self.local_node.object_dictionary["Transmit PDO 0 communication parameters.COB-ID use by TPDO 1"].default + enabledCobId = odDefaultVal & ~PDO_NOT_VALID # Ensure invalid bit is not set + + self.remote_node.sdo["Transmit PDO 0 communication parameters.COB-ID use by TPDO 1"].raw = enabledCobId + + # Transition to operational + self.local_node.nmt.state = 'OPERATIONAL' + + # Ensure tpdo automatically started with transition + self.assertNotEqual(self.local_node.tpdo[1]._task, None) + if __name__ == "__main__": unittest.main() From 43e32a78f10191c0df3d6613a2704111835451fa Mon Sep 17 00:00:00 2001 From: Grant Weiss Date: Mon, 12 Aug 2024 13:40:10 -0400 Subject: [PATCH 6/6] Fix styling to match project. Simplify applying changes to PDO mapping by using pdo.read(from_od=True). --- canopen/node/local.py | 55 ++++++++++++++++++++++--------------------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/canopen/node/local.py b/canopen/node/local.py index 20be5cb3..7fce33ec 100644 --- a/canopen/node/local.py +++ b/canopen/node/local.py @@ -35,7 +35,6 @@ def __init__( self.emcy = EmcyProducer(0x80 + self.id) self.nmt.add_state_change_callback(self._nmt_state_changed) - self.add_write_callback(self._tpdo_configuration_write) def associate_network(self, network): self.network = network @@ -118,6 +117,15 @@ def set_data( self.data_store.setdefault(index, {}) self.data_store[index][subindex] = bytes(data) + if 0x1800 <= index <= 0x19FF: + # TPDO Communication parameter changed + tpdoNum = (index - 0x1800) + 1 + self._tpdo_configuration_write(tpdoNum) + elif 0x1A00 <= index <= 0x1BFF: + # TPDO Mapping parameter changed + tpdoNum = (index - 0x1A00) + 1 + self._tpdo_configuration_write(tpdoNum) + def _find_object(self, index, subindex): if index not in self.object_dictionary: # Index does not exist @@ -133,36 +141,29 @@ def _find_object(self, index, subindex): def _nmt_state_changed(self, old_state, new_state): if new_state == "OPERATIONAL": - for i, pdo in self.tpdo.map.items(): + for pdo in self.tpdo.map.values(): if pdo.enabled: try: pdo.start() - logger.info(f"Successfully started TPDO {i}") + logger.info("Successfully started %s", pdo.name) except ValueError: - logger.warning(f"Failed to start TPDO {i} due to missing period") - except Exception as e: - logger.error(f"Unknown error starting TPDO {i}: {str(e)}") + logger.warning("Failed to start %s due to missing period", pdo.name) + except Exception: + logger.exception("Unknown error starting %s", pdo.name) else: - logger.info(f"TPDO {i} not enabled") - elif old_state == "OPERATIONAL": + logger.info("%s not enabled", pdo.name) + else: self.tpdo.stop() - def _tpdo_configuration_write(self, index, subindex, od, data): - if 0x1800 <= index <= 0x19FF: - # Only allowed to edit pdo configuration in pre-op - if self.nmt.state != "PRE-OPERATIONAL": - logger.warning("Tried to configure tpdo when not in pre-op") - return - - if subindex == 0x01: - if len(data) == 4: - tpdoNum = (index - 0x1800) + 1 - if tpdoNum in self.tpdo.map.keys(): - PDO_NOT_VALID = 1 << 31 - RTR_NOT_ALLOWED = 1 << 30 - - cob_id = int.from_bytes(data, 'little') & 0x7FF - - self.tpdo.map[tpdoNum].cob_id = cob_id & 0x1FFFFFFF - self.tpdo.map[tpdoNum].enabled = cob_id & PDO_NOT_VALID == 0 - self.tpdo.map[tpdoNum].rtr_allowed = cob_id & RTR_NOT_ALLOWED == 0 \ No newline at end of file + def _tpdo_configuration_write(self, tpdoNum): + pdo = self.tpdo.map[tpdoNum] + + # Only allowed to edit pdo configuration in pre-op or operational + if self.nmt.state not in ("PRE-OPERATIONAL", "OPERATIONAL"): + logger.warning("Tried to configure %s when not in pre-op or operational", pdo.name) + return + + try: + pdo.read(from_od=True) + except: + pass