DS402: Fix logic errors in the power state machine and generalize it.

canopen/profiles/p402.py

@@ -46,14 +46,14 @@ class State402(object):
         'QUICK STOP ACTIVE':            (0x6F, 0x07),
     }
 
-    # Transition path to get to the 'OPERATION ENABLED' state
-    NEXTSTATE2ENABLE = {
+    # Transition path to reach and state without a direct transition
+    NEXTSTATE2ANY = {
         ('START'):                                                      'NOT READY TO SWITCH ON',
-        ('FAULT', 'NOT READY TO SWITCH ON'):                            'SWITCH ON DISABLED',
+        ('FAULT', 'NOT READY TO SWITCH ON', 'QUICK STOP ACTIVE'):       'SWITCH ON DISABLED',
         ('SWITCH ON DISABLED'):                                         'READY TO SWITCH ON',
         ('READY TO SWITCH ON'):                                         'SWITCHED ON',
-        ('SWITCHED ON', 'QUICK STOP ACTIVE', 'OPERATION ENABLED'):      'OPERATION ENABLED',
-        ('FAULT REACTION ACTIVE'):                                      'FAULT'
+        ('SWITCHED ON'):                                                'OPERATION ENABLED',
+        ('FAULT REACTION ACTIVE'):                                      'FAULT',
     }
 
     # Tansition table from the DS402 State Machine
@@ -78,22 +78,25 @@ class State402(object):
         ('SWITCHED ON', 'OPERATION ENABLED'):             CW_OPERATION_ENABLED,  # transition 4
         ('QUICK STOP ACTIVE', 'OPERATION ENABLED'):       CW_OPERATION_ENABLED,  # transition 16
         # quickstop ---------------------------------------------------------------------------
-        ('READY TO SWITCH ON', 'QUICK STOP ACTIVE'):      CW_QUICK_STOP,  # transition 7
-        ('SWITCHED ON', 'QUICK STOP ACTIVE'):             CW_QUICK_STOP,  # transition 10
         ('OPERATION ENABLED', 'QUICK STOP ACTIVE'):       CW_QUICK_STOP,  # transition 11
         # fault -------------------------------------------------------------------------------
         ('FAULT', 'SWITCH ON DISABLED'):                  CW_SWITCH_ON_DISABLED,  # transition 15
     }
 
     @staticmethod
-    def next_state_for_enabling(_from):
-        """Return the next state needed for reach the state Operation Enabled.
+    def next_state_indirect(_from):
+        """Return the next state needed to reach any state indirectly.
+
+        The chosen path always points toward the OPERATION ENABLED state, except when
+        coming from QUICK STOP ACTIVE.  In that case, it will cycle through SWITCH ON
+        DISABLED first, as there would have been a direct transition if the opposite was
+        desired.
 
         :param str target: Target state.
         :return: Next target to change.
         :rtype: str
         """
-        for cond, next_state in State402.NEXTSTATE2ENABLE.items():
+        for cond, next_state in State402.NEXTSTATE2ANY.items():
             if _from in cond:
                 return next_state
 
@@ -497,10 +500,16 @@ def state(self, target_state):
             time.sleep(self.INTERVAL_CHECK_STATE)
 
     def _next_state(self, target_state):
-        if target_state == 'OPERATION ENABLED':
-            return State402.next_state_for_enabling(self.state)
-        else:
+        if target_state in ('NOT READY TO SWITCH ON',
+                            'FAULT REACTION ACTIVE',
+                            'FAULT'):
+            raise ValueError(
+                'Target state {} cannot be entered programmatically'.format(target_state))
+        from_state = self.state
+        if (from_state, target_state) in State402.TRANSITIONTABLE:
             return target_state
+        else:
+            return State402.next_state_indirect(from_state)
 
     def _change_state(self, target_state):
         try:

canopen/profiles/tools/test_p402_states.py

@@ -0,0 +1,37 @@
+"""Verification script to diagnose automatic state transitions.
+
+This is meant to be run for verifying changes to the DS402 power state
+machine code.  For each target state, it just lists the next
+intermediate state which would be set automatically, depending on the
+assumed current state.
+"""
+
+from canopen.objectdictionary import ObjectDictionary
+from canopen.profiles.p402 import State402, BaseNode402
+
+
+if __name__ == '__main__':
+    n = BaseNode402(1, ObjectDictionary())
+
+    for target_state in State402.SW_MASK:
+        print('\n--- Target =', target_state, '---')
+        for from_state in State402.SW_MASK:
+            if target_state == from_state:
+                continue
+            if (from_state, target_state) in State402.TRANSITIONTABLE:
+                print('direct:\t{} -> {}'.format(from_state, target_state))
+            else:
+                next_state = State402.next_state_indirect(from_state)
+                if not next_state:
+                    print('FAIL:\t{} -> {}'.format(from_state, next_state))
+                else:
+                    print('\t{} -> {} ...'.format(from_state, next_state))
+
+            try:
+                while from_state != target_state:
+                    n.tpdo_values[0x6041] = State402.SW_MASK[from_state][1]
+                    next_state = n._next_state(target_state)
+                    print('\t\t-> {}'.format(next_state))
+                    from_state = next_state
+            except ValueError:
+                print('\t\t-> disallowed!')