diff --git a/canopen/network.py b/canopen/network.py index 02bec899..57667eab 100644 --- a/canopen/network.py +++ b/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: diff --git a/canopen/node/local.py b/canopen/node/local.py index 8f2493d9..e93efcfa 100644 --- a/canopen/node/local.py +++ b/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: diff --git a/canopen/pdo/__init__.py b/canopen/pdo/__init__.py index 533309f8..baafe536 100644 --- a/canopen/pdo/__init__.py +++ b/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. diff --git a/canopen/pdo/base.py b/canopen/pdo/base.py index 0ba65199..572e4a39 100644 --- a/canopen/pdo/base.py +++ b/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) diff --git a/test/sample.eds b/test/sample.eds index 1afe9965..fe1c0716 100644 --- a/test/sample.eds +++ b/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 diff --git a/test/test_local.py b/test/test_local.py index e184c040..be9b58b0 100644 --- a/test/test_local.py +++ b/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() diff --git a/test/test_network.py b/test/test_network.py index 1d45a1c2..e9892e75 100644 --- a/test/test_network.py +++ b/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)]) diff --git a/test/test_pdo.py b/test/test_pdo.py index 1badc89d..0c99598c 100644 --- a/test/test_pdo.py +++ b/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()