DS402: Increase delay to check status on homing start. (#252)

* 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.

Author: acolomb

canopen/profiles/p402.py

@@ -204,6 +204,7 @@ class BaseNode402(RemoteNode):
     TIMEOUT_SWITCH_STATE_SINGLE = 0.4   # seconds
     INTERVAL_CHECK_STATE = 0.01         # seconds
     TIMEOUT_HOMING_DEFAULT = 30         # seconds
+    INTERVAL_CHECK_HOMING = 0.1         # seconds
 
     def __init__(self, node_id, object_dictionary):
         super(BaseNode402, self).__init__(node_id, object_dictionary)
@@ -275,6 +276,18 @@ 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 an RPDO was received.  Should better check for reception
+        # instead of this hard-coded delay, but at least it can be configured per node.
+        time.sleep(self.INTERVAL_CHECK_HOMING)
+        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.
 
@@ -286,11 +299,7 @@ def is_homed(self, restore_op_mode=False):
         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
+        homingstatus = self._homing_status()
         if restore_op_mode:
             self.op_mode = previous_op_mode
         return homingstatus in ('TARGET REACHED', 'ATTAINED')
@@ -311,16 +320,10 @@ def homing(self, timeout=TIMEOUT_HOMING_DEFAULT):
         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))
-                time.sleep(self.INTERVAL_CHECK_STATE)
                 if time.monotonic() > t:
                     raise RuntimeError('Unable to home, timeout reached')
             logger.info('Homing mode carried out successfully.')