LocalNode improvements for TPDOs

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

canopen/node/local.py

@@ -34,6 +34,9 @@ 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.add_write_callback(self._tpdo_configuration_write)
+
     def associate_network(self, network):
         self.network = network
         self.sdo.network = network
@@ -127,3 +130,39 @@ 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()
+
+    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

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()