Support server pdo and added api

canopen/network.py

@@ -75,6 +75,9 @@ def unsubscribe(self, can_id, callback=None) -> None:
             If given, remove only this callback.  Otherwise all callbacks for
             the CAN ID.
         """
+        if can_id not in self.subscribers:
+            return
+
         if callback is None:
             del self.subscribers[can_id]
         else:

canopen/node/local.py

@@ -51,6 +51,7 @@ def associate_network(self, network: canopen.network.Network):
     def remove_network(self) -> None:
         self.network.unsubscribe(self.sdo.rx_cobid, self.sdo.on_request)
         self.network.unsubscribe(0, self.nmt.on_command)
+        self.stop_pdo_services()
         self.network = canopen.network._UNINITIALIZED_NETWORK
         self.sdo.network = canopen.network._UNINITIALIZED_NETWORK
         self.tpdo.network = canopen.network._UNINITIALIZED_NETWORK
@@ -64,6 +65,28 @@ def add_read_callback(self, callback):
     def add_write_callback(self, callback):
         self._write_callbacks.append(callback)
 
+    def start_pdo_services(
+        self, period: float, enable_rpdo: bool = True, enable_tpdo: bool = True
+    ):
+        """
+        Start the PDO related services of the node.
+        :param period: Service interval in seconds.
+        :param enable_rpdo: Enable RPDO service.
+        :param enable_tpdo: Enable TPDO service.
+        """
+        if enable_rpdo:
+            self.rpdo.read(from_od=True, subscribe=True)
+        if enable_tpdo:
+            self.tpdo.read(from_od=True, subscribe=False)
+            self.tpdo.start(period=period)
+
+    def stop_pdo_services(self):
+        """
+        Stop the PDO related services of the node.
+        """
+        self.rpdo.unsubscribe()
+        self.tpdo.stop()
+
     def get_data(
         self, index: int, subindex: int, check_readable: bool = False
     ) -> bytes:
@@ -118,6 +141,7 @@ def set_data(
         # Store data
         self.data_store.setdefault(index, {})
         self.data_store[index][subindex] = bytes(data)
+        self.tpdo.update()
 
     def _find_object(self, index, subindex):
         if index not in self.object_dictionary:

canopen/pdo/__init__.py

@@ -2,7 +2,7 @@
 
 from canopen import node
 from canopen.pdo.base import PdoBase, PdoMap, PdoMaps, PdoVariable
-
+from canopen.sdo import SdoAbortedError
 
 __all__ = [
     "PdoBase",
@@ -74,6 +74,67 @@ def __init__(self, node):
         self.map = PdoMaps(0x1800, 0x1A00, self, 0x180)
         logger.debug('TPDO Map as %d', len(self.map))
 
+    def start(self, period: float):
+        """Start transmission of all TPDOs.
+
+        :param float period: Transmission period in seconds.
+        :raises TypeError: Exception is thrown if the node associated with the PDO does not
+        support this function.
+        """
+        if isinstance(self.node, node.LocalNode):
+            for pdo in self.map.values():
+                pdo.start(period)
+        else:
+            raise TypeError("The node type does not support this function.")
+
+    def pack_data(self, data: bytearray, variable: PdoVariable):
+        """Pack new data into data array if new data is available. 
+
+        :param list data: List of data to pack.
+        :param PdoVariable variable: Variable to pack.
+        """
+        length_bytes = variable.length // 8
+        offset_bytes = variable.offset // 8
+        if length_bytes > 8:
+            raise ValueError("Data length is greater than 64 bits.")
+
+        if length_bytes == 0:
+            return data
+
+        if (
+            self.node.data_store.get(variable.index, {}).get(variable.subindex)
+            is not None
+        ):
+            try:
+                variable_data = self.node.get_data(variable.index, variable.subindex)
+                # reverse list to be able to be transmitted properly
+                sanitized_data = variable_data[::-1]
+                # pack new data into data array
+                for i in range(length_bytes):
+                    data[offset_bytes + i] = sanitized_data[i]
+            except SdoAbortedError:
+                logger.warning(
+                    "Failed to get data from index: 0x%X, subindex: 0x%X",
+                    variable.index,
+                    variable.subindex,
+                )
+
+        return data
+
+    def update(self):
+        """Update the data of all TPDOs.
+
+        :raises TypeError: Exception is thrown if the node associated with the PDO does not
+        support this function.
+        """
+        if isinstance(self.node, node.LocalNode):
+            for pdo in self.map.values():
+                for variable in pdo:
+                    self.pack_data(pdo.data, variable)
+                pdo.update()
+        else:
+            raise TypeError("The node type does not support this function.")
+
     def stop(self):
         """Stop transmission of all TPDOs.
 

canopen/pdo/base.py

@@ -56,10 +56,14 @@ def __getitem__(self, key):
     def __len__(self):
         return len(self.map)
 
-    def read(self, from_od=False):
-        """Read PDO configuration from node using SDO."""
+    def read(self, from_od=False, subscribe=True):
+        """Read PDO configuration from node using SDO.
+
+        :param from_od: Read using SDO if False, read from object dictionary if True.
+        :param subscribe: Subscribe to the network for this PDO if True.
+        """
         for pdo_map in self.map.values():
-            pdo_map.read(from_od=from_od)
+            pdo_map.read(from_od=from_od, subscribe=subscribe)
 
     def save(self):
         """Save PDO configuration to node using SDO."""
@@ -77,6 +81,11 @@ def subscribe(self):
         for pdo_map in self.map.values():
             pdo_map.subscribe()
 
+    def unsubscribe(self) -> None:
+        """Unregister the node's PDOs for reception on the network."""
+        for pdo_map in self.map.values():
+            pdo_map.unsubscribe()
+
     def export(self, filename):
         """Export current configuration to a database file.
 
@@ -331,13 +340,16 @@ def add_callback(self, callback: Callable[[PdoMap], None]) -> None:
         """
         self.callbacks.append(callback)
 
-    def read(self, from_od=False) -> None:
+    def read(self, from_od=False, subscribe=True) -> None:
         """Read PDO configuration for this map.
-        
+
         :param from_od:
             Read using SDO if False, read from object dictionary if True.
             When reading from object dictionary, if DCF populated a value, the
             DCF value will be used, otherwise the EDS default will be used instead.
+        :param subscribe:
+            Subscribe to the network for this PDO if True.
+            Don't subscribe if the PDO is a PDO meant for transmission.
         """
 
         def _raw_from(param):
@@ -360,6 +372,7 @@ def _raw_from(param):
         if self.trans_type >= 254:
             try:
                 self.inhibit_time = _raw_from(self.com_record[3])
+                self.period = self.inhibit_time if self.inhibit_time else None
             except (KeyError, SdoAbortedError) as e:
                 logger.info("Could not read inhibit time (%s)", e)
             else:
@@ -394,8 +407,8 @@ def _raw_from(param):
                 size = (value >> 24) & 0x7F
             if index and size:
                 self.add_variable(index, subindex, size)
-
-        self.subscribe()
+        if subscribe:
+            self.subscribe()
 
     def save(self) -> None:
         """Save PDO configuration for this map using SDO."""
@@ -469,6 +482,14 @@ def subscribe(self) -> None:
             logger.info("Subscribing to enabled PDO 0x%X on the network", self.cob_id)
             self.pdo_node.network.subscribe(self.cob_id, self.on_message)
 
+    def unsubscribe(self) -> None:
+        """Unregister the PDO for reception on the network."""
+        if self.enabled:
+            logger.info(
+                "Unsubscribing from enabled PDO 0x%X on the network", self.cob_id
+            )
+            self.pdo_node.network.unsubscribe(self.cob_id, self.on_message)
+
     def clear(self) -> None:
         """Clear all variables from this map."""
         self.map = []
@@ -534,6 +555,13 @@ def start(self, period: Optional[float] = None) -> None:
             raise ValueError("A valid transmission period has not been given")
         logger.info("Starting %s with a period of %s seconds", self.name, self.period)
 
+        if self.cob_id is None and self.predefined_cob_id is not None:
+            self.cob_id = self.predefined_cob_id
+            logger.info("Using predefined COB-ID 0x%X", self.cob_id)
+
+        if self.cob_id is None:
+            raise ValueError("COB-ID has not been set")
+
         self._task = self.pdo_node.network.send_periodic(
             self.cob_id, self.data, self.period)
 

test/sample.eds

@@ -664,7 +664,7 @@ ObjectType=7
 DataType=7
 AccessType=RW
 PDOMapping=0
-DefaultValue=1614872592
+DefaultValue=0x30100020
 
 [1a00sub2]
 ParameterName=TPDO 1 mapping information 2
@@ -945,7 +945,7 @@ SubNumber=1
 ParameterName=Temperature
 ObjectType=0x7
 DataType=0x0008
-AccessType=ro
+AccessType=rw
 DefaultValue=0
 PDOMapping=1
 

test/test_local.py

@@ -196,6 +196,19 @@ def tearDownClass(cls):
     def test_read(self):
         # TODO: Do some more checks here. Currently it only tests that they
         # can be called without raising an error.
+        local_node_original_subscribers = list(self.local_node.network.subscribers)
+        remote_node_original_subscribers = list(self.remote_node.network.subscribers)
+        self.remote_node.pdo.read(from_od=True, subscribe=False)
+        self.local_node.pdo.read(from_od=True, subscribe=False)
+        assert len(self.local_node.network.subscribers) == len(local_node_original_subscribers)
+        assert len(self.remote_node.network.subscribers) == len(remote_node_original_subscribers)
+
+        self.remote_node.pdo.read(from_od=False, subscribe=True)
+        self.local_node.pdo.read(from_od=False, subscribe=True)
+        assert len(self.local_node.network.subscribers) != len(local_node_original_subscribers)
+        assert len(self.remote_node.network.subscribers) != len(remote_node_original_subscribers)
+
+        # just check to make sure calling without args can work without raising an error
         self.remote_node.pdo.read()
         self.local_node.pdo.read()
 
@@ -205,6 +218,19 @@ def test_save(self):
         self.remote_node.pdo.save()
         self.local_node.pdo.save()
 
+    def test_set_data(self):
+        data = [0xDE, 0xAD, 0xBE, 0xEF]
+        self.local_node.set_data(0x3010,0, bytearray(data))
+        assert self.local_node.get_data(0x3010, 0, False) == bytearray(data)
+
+    def test_update_tpdo(self):
+        data = [0xDE, 0xAD, 0xBE, 0xEF]
+
+        self.local_node.tpdo[1].add_variable(index=0x3010, subindex=0, length=32)
+        self.local_node.set_data(0x3010, 0, bytearray(data))
+
+        assert self.local_node.tpdo[1].data == bytearray(data[::-1])
+
 
 if __name__ == "__main__":
     unittest.main()

test/test_network.py

@@ -144,6 +144,8 @@ def hook(*args, i=i):
         self.assertEqual(accumulators[2], [(2, bytes([4, 5, 6]), 1003)])
 
         self.network.unsubscribe(0)
+        # Should not raise an error.
+        self.network.unsubscribe(10)
         self.network.notify(0, bytes([7, 7, 7]), 1004)
         # Verify that no new data was added to the accumulator.
         self.assertEqual(accumulators[0], [(0, bytes([1, 2, 3]), 1000)])

test/test_pdo.py

@@ -80,6 +80,16 @@ def test_pdo_export(self):
                         self.assertIn("ID", header)
                         self.assertIn("Frame Name", header)
 
+    def test_tpdo_start_stop(self):
+        network = canopen.Network()
+        network.connect("test", interface="virtual")
+        self.node.associate_network(network)
+        self.node.tpdo[1].start(period=0.01)
+        self.node.tpdo[1].stop()
+
+    def test_rpdo_subscribe_unsubscribe(self):
+        self.node.rpdo.subscribe()
+        self.node.rpdo.unsubscribe()
 
 if __name__ == "__main__":
     unittest.main()