DS402: Allow handling the operation mode via PDO. (#257)

* pdo: Document relation between start() and the period attribute.

* pdo: Add a property to check if period updates are transmitted.

* p402: Skip sending the controlword RPDO if configured with a period.

Transmitting the RPDO only makes sense if the value takes effect
immediately.  If there is already a cyclic task configured, or the
transmission type configuration requires a SYNC to be sent for the
change to apply, there is no sense in sending the PDO automatically in
the controlword setter.

* p402: Use RPDO to set the operation mode if possible.

Fall back to the previous behavior using SDO if the relevant object
0x6060 is not mapped to an RPDO.

* p402: Use TPDO to get the operation mode if possible.

Fall back to the previous behavior using SDO if the relevant object
0x6061 is not mapped to a TPDO.  The property getter still blocks
until an up-to-date value was received, by waiting for the respective
TPDO up to a configurable timeout of 0.2 seconds by default.  If the
TPDO does not look like it will be transmitted regularly (from its
is_periodic property), the method will not block and just return the
last received TPDO's value.

A lookup cache tpdo_pointers is added to keep track of the needed
pdo.Map instance, analog to the rpdo_pointers.

* p402: Improve documentation on PDO tracking dicts.

Consistently use empty dict literal for initialization.  Provide more
useful comments about the expected contents.

* p402: Check PDO configuration for the Operation Mode objects.

Switching operation modes for several drives simultaneously must be
done via PDO.  There is still a fallback mechanism via SDO, but a
warning should be issued when that is about to be used.

Co-authored-by: André Filipe Silva <[email protected]>

Author: acolomb

canopen/pdo/base.py

@@ -186,7 +186,8 @@ def __init__(self, pdo_node, com_record, map_array):
         self.data = bytearray()
         #: Timestamp of last received message
         self.timestamp = None
-        #: Period of receive message transmission in seconds
+        #: Period of receive message transmission in seconds.
+        #: Set explicitly or using the :meth:`start()` method.
         self.period = None
         self.callbacks = []
         self.receive_condition = threading.Condition()
@@ -273,6 +274,23 @@ def name(self):
         node_id = self.cob_id & 0x7F
         return "%sPDO%d_node%d" % (direction, map_id, node_id)
 
+    @property
+    def is_periodic(self):
+        """Indicate whether PDO updates will be transferred regularly.
+
+        If some external mechanism is used to transmit the PDO regularly, its cycle time
+        should be written to the :attr:`period` member for this property to work.
+        """
+        if self.period is not None:
+            # Configured from start() or externally
+            return True
+        elif self.trans_type is not None and self.trans_type <= 0xF0:
+            # TPDOs will be transmitted on SYNC, RPDOs need a SYNC to apply, so
+            # assume that the SYNC service is active.
+            return True
+        # Unknown transmission type, assume non-periodic
+        return False
+
     def on_message(self, can_id, data, timestamp):
         is_transmitting = self._task is not None
         if can_id == self.cob_id and not is_transmitting:
@@ -459,7 +477,10 @@ def transmit(self):
     def start(self, period=None):
         """Start periodic transmission of message in a background thread.
 
-        :param float period: Transmission period in seconds
+        :param float period:
+            Transmission period in seconds.  Can be omitted if :attr:`period` has been set
+            on the object before.
+        :raises ValueError: When neither the argument nor the :attr:`period` is given.
         """
         # Stop an already running transmission if we have one, otherwise we
         # overwrite the reference and can lose our handle to shut it down

canopen/profiles/p402.py

@@ -202,13 +202,15 @@ class BaseNode402(RemoteNode):
     TIMEOUT_SWITCH_OP_MODE = 0.5        # seconds
     TIMEOUT_SWITCH_STATE_FINAL = 0.8    # seconds
     TIMEOUT_SWITCH_STATE_SINGLE = 0.4   # seconds
+    TIMEOUT_CHECK_TPDO = 0.2            # seconds
     INTERVAL_CHECK_STATE = 0.01         # seconds
     TIMEOUT_HOMING_DEFAULT = 30         # seconds
 
     def __init__(self, node_id, object_dictionary):
         super(BaseNode402, self).__init__(node_id, object_dictionary)
-        self.tpdo_values = dict()  # { index: TPDO_value }
-        self.rpdo_pointers = dict()  # { index: RPDO_pointer }
+        self.tpdo_values = {}  # { index: value from last received TPDO }
+        self.tpdo_pointers = {}  # { index: pdo.Map instance }
+        self.rpdo_pointers = {}  # { index: pdo.Map instance }
 
     def setup_402_state_machine(self):
         """Configure the state machine by searching for a TPDO that has the StatusWord mapped.
@@ -220,6 +222,7 @@ def setup_402_state_machine(self):
         self.setup_pdos()
         self._check_controlword_configured()
         self._check_statusword_configured()
+        self._check_op_mode_configured()
         self.nmt.state = 'OPERATIONAL'
         self.state = 'SWITCH ON DISABLED' # Why change state?
 
@@ -236,6 +239,7 @@ def _init_tpdo_values(self):
                     logger.debug('Configured TPDO: {0}'.format(obj.index))
                     if obj.index not in self.tpdo_values:
                         self.tpdo_values[obj.index] = 0
+                        self.tpdo_pointers[obj.index] = obj
 
     def _init_rpdo_pointers(self):
         # If RPDOs have overlapping indecies, rpdo_pointers will point to
@@ -259,6 +263,16 @@ def _check_statusword_configured(self):
                 "Statusword not configured in node {0}'s PDOs. Using SDOs can cause slow performance.".format(
                     self.id))
 
+    def _check_op_mode_configured(self):
+        if 0x6060 not in self.rpdo_pointers:  # Operation Mode
+            logger.warning(
+                "Operation Mode not configured in node {0}'s PDOs. Using SDOs can cause slow performance.".format(
+                    self.id))
+        if 0x6061 not in self.tpdo_values:  # Operation Mode Display
+            logger.warning(
+                "Operation Mode Display not configured in node {0}'s PDOs. Using SDOs can cause slow performance.".format(
+                    self.id))
+
     def reset_from_fault(self):
         """Reset node from fault and set it to Operation Enable state."""
         if self.state == 'FAULT':
@@ -335,7 +349,7 @@ def homing(self, timeout=TIMEOUT_HOMING_DEFAULT):
     def op_mode(self):
         """The node's Operation Mode stored in the object 0x6061.
 
-        Uses SDO to access the current value.  The modes are passed as one of the
+        Uses SDO or PDO to access the current value.  The modes are passed as one of the
         following strings:
 
         - 'NO MODE'
@@ -354,7 +368,18 @@ def op_mode(self):
         :raises TypeError: When setting a mode not advertised as supported by the node.
         :raises RuntimeError: If the switch is not confirmed within the configured timeout.
         """
-        return OperationMode.CODE2NAME[self.sdo[0x6061].raw]
+        try:
+            pdo = self.tpdo_pointers[0x6061].pdo_parent
+            if pdo.is_periodic:
+                timestamp = pdo.wait_for_reception(timeout=self.TIMEOUT_CHECK_TPDO)
+                if timestamp is None:
+                    raise RuntimeError("Timeout getting node {0}'s mode of operation.".format(
+                        self.id))
+            code = self.tpdo_values[0x6061]
+        except KeyError:
+            logger.warning('The object 0x6061 is not a configured TPDO, fallback to SDO')
+            code = self.sdo[0x6061].raw
+        return OperationMode.CODE2NAME[code]
 
     @op_mode.setter
     def op_mode(self, mode):
@@ -435,7 +460,9 @@ def controlword(self):
     def controlword(self, value):
         if 0x6040 in self.rpdo_pointers:
             self.rpdo_pointers[0x6040].raw = value
-            self.rpdo_pointers[0x6040].pdo_parent.transmit()
+            pdo = self.rpdo_pointers[0x6040].pdo_parent
+            if not pdo.is_periodic:
+                pdo.transmit()
         else:
             self.sdo[0x6040].raw = value