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 diff --git a/canopen/node/local.py b/canopen/node/local.py index eb74b98d..7fce33ec 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 @@ -115,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 @@ -127,3 +138,32 @@ 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 pdo in self.tpdo.map.values(): + if pdo.enabled: + try: + pdo.start() + logger.info("Successfully started %s", pdo.name) + except ValueError: + 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("%s not enabled", pdo.name) + else: + self.tpdo.stop() + + 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 diff --git a/test/test_local.py b/test/test_local.py index 9c5fc0c1..554697a0 100644 --- a/test/test_local.py +++ b/test/test_local.py @@ -200,6 +200,37 @@ 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) + + 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()