DS402: Replace delay loops with waiting for TPDO reception. (#263)

* ds402: Increase delay to check status on homing start.

The Statusword is examined immediately after setting the Controlword
command to start homing.  That is very likely to fail because of the
round-trip time until the Statusword is actually updated from a TPDO.
To work around that, the delay between each check of the Statusword
should be moved before the actual comparison, and its default value
increased.

Introduce a new constant TIMEOUT_CHECK_HOMING to configure that with a
default value of 100 ms.  This replaces the previously used
INTERVAL_CHECK_STATE which is only 10 ms by default.

An even better solution would be to wait for the Statusword to be
updated by a received PDO, but that would be much more complex.

* Apply interval to is_homed() method as well.

Same problem as in the homing() method, PDO updates of the Statusword
need at least one SYNC / PDO cycle duration.

* Factor out common _homing_status() method.

Move the common code from is_homed() and homing() to a method for
internal use.  Add a comment why the delay is necessary and how it
should possibly be replaced by an RPDO reception check.  Should the
latter be implemented, there will be only one place to change it.

* p402: Add blocking method check_statusword().

In contrast to the property getter, this method will not simply return
the cached last value, but actually wait for an updated TPDO
reception.  If no TPDO is configured, or it is not expected
periodically, the usual statusword getter takes over and returns
either the last received value or queries via SDO as a fallback.

The timeout for receiving a TPDO can be configured individually per
call, defaulting to 0.2 seconds (TIMEOUT_CHECK_TPDO).

* p402: Wait for TPDO instead of timed statusword checking.

Several methods loop around waiting for some expected statusword
value, usually calling time.sleep() with the INTERVAL_CHECK_STATE or
INTERVAL_CHECK_HOMING constants.  Replace that with a call to
check_statusword(), which will only block until the TPDO is received.

This allows for possibly shorter delays, because the delay can be cut
short when the TPDO arrives in between.  But rate limiting the
checking loops is still achieved through the blocking behavior of
check_statusword().

If no TPDO is configured, the statusword will fall back to checking
via SDO, which will also result in periodic checks, but only rate
limited by the SDO round-trip time.  Anyway this is not a recommended
mode of operation, as the warning in _check_statusword_configured()
points out.

* p402: Fix wrong reports of INTERRUPTED homing procedure.

While the statusword checks work properly now, there is no delay
anymore to make sure the controlword RPDO which starts the homing
procedure is already received before we start checking the homing
status.  So the first reading might easily return INTERRUPTED, meaning
that the controlword change has just not been applied yet.

Add one extra call for check_statusword() to wait for one extra cycle
in case of periodic transmissions.  In effect, the success checking
logic starts looking at the second received statusword after setting
the controlword.

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

Author: acolomb

canopen/profiles/p402.py

@@ -293,13 +293,24 @@ def reset_from_fault(self):
             while self.is_faulted():
                 if time.monotonic() > timeout:
                     break
-                time.sleep(self.INTERVAL_CHECK_STATE)
+                self.check_statusword()
             self.state = 'OPERATION ENABLED'
 
     def is_faulted(self):
         bitmask, bits = State402.SW_MASK['FAULT']
         return self.statusword & bitmask == bits
 
+    def _homing_status(self):
+        """Interpret the current Statusword bits as homing state string."""
+        # Wait to make sure a TPDO was received
+        self.check_statusword()
+        status = None
+        for key, value in Homing.STATES.items():
+            bitmask, bits = value
+            if self.statusword & bitmask == bits:
+                status = key
+        return status
+
     def is_homed(self, restore_op_mode=False):
         """Switch to homing mode and determine its status.
 
@@ -310,12 +321,8 @@ def is_homed(self, restore_op_mode=False):
         previous_op_mode = self.op_mode
         if previous_op_mode != 'HOMING':
             logger.info('Switch to HOMING from %s', previous_op_mode)
-            self.op_mode = 'HOMING'
-        homingstatus = None
-        for key, value in Homing.STATES.items():
-            bitmask, bits = value
-            if self.statusword & bitmask == bits:
-                homingstatus = key
+            self.op_mode = 'HOMING'  # blocks until confirmed
+        homingstatus = self._homing_status()
         if restore_op_mode:
             self.op_mode = previous_op_mode
         return homingstatus in ('TARGET REACHED', 'ATTAINED')
@@ -335,17 +342,14 @@ def homing(self, timeout=TIMEOUT_HOMING_DEFAULT, restore_op_mode=False):
         self.op_mode = 'HOMING'
         # The homing process will initialize at operation enabled
         self.state = 'OPERATION ENABLED'
-        homingstatus = 'IN PROGRESS'
-        self.controlword = State402.CW_OPERATION_ENABLED | Homing.CW_START
+        homingstatus = 'UNKNOWN'
+        self.controlword = State402.CW_OPERATION_ENABLED | Homing.CW_START  # does not block
+        # Wait for one extra cycle, to make sure the controlword was received
+        self.check_statusword()
         t = time.monotonic() + timeout
         try:
             while homingstatus not in ('TARGET REACHED', 'ATTAINED'):
-                for key, value in Homing.STATES.items():
-                    # check if the Statusword after applying the bitmask
-                    # corresponds with the needed bits to determine the current status
-                    bitmask, bits = value
-                    if self.statusword & bitmask == bits:
-                        homingstatus = key
+                homingstatus = self._homing_status()
                 if homingstatus in ('INTERRUPTED', 'ERROR VELOCITY IS NOT ZERO',
                                     'ERROR VELOCITY IS ZERO'):
                     raise RuntimeError('Unable to home. Reason: {0}'.format(homingstatus))
@@ -463,6 +467,26 @@ def statusword(self):
             logger.warning('The object 0x6041 is not a configured TPDO, fallback to SDO')
             return self.sdo[0x6041].raw
 
+    def check_statusword(self, timeout=None):
+        """Report an up-to-date reading of the statusword (0x6041) from the device.
+
+        If the TPDO with the statusword is configured as periodic, this method blocks
+        until one was received.  Otherwise, it uses the SDO fallback of the ``statusword``
+        property.
+
+        :param timeout: Maximum time in seconds to wait for TPDO reception.
+        :raises RuntimeError: Occurs when the given timeout expires without a TPDO.
+        :return: Updated value of the ``statusword`` property.
+        :rtype: int
+        """
+        if 0x6041 in self.tpdo_pointers:
+            pdo = self.tpdo_pointers[0x6041].pdo_parent
+            if pdo.is_periodic:
+                timestamp = pdo.wait_for_reception(timeout or self.TIMEOUT_CHECK_TPDO)
+                if timestamp is None:
+                    raise RuntimeError('Timeout waiting for updated statusword')
+        return self.statusword
+
     @property
     def controlword(self):
         """Send a state change command using PDO or SDO.
@@ -518,7 +542,7 @@ def state(self, target_state):
                 continue
             if time.monotonic() > timeout:
                 raise RuntimeError('Timeout when trying to change state')
-            time.sleep(self.INTERVAL_CHECK_STATE)
+            self.check_statusword()
 
     def _next_state(self, target_state):
         if target_state == 'OPERATION ENABLED':
@@ -536,5 +560,5 @@ def _change_state(self, target_state):
         while self.state != target_state:
             if time.monotonic() > timeout:
                 return False
-            time.sleep(self.INTERVAL_CHECK_STATE)
+            self.check_statusword()
         return True