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,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

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