Merge remote-tracking branch 'origin/SecondHandsDemo' into Ice3.7

# Conflicts:
#	source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp

source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp

@@ -28,6 +28,7 @@
 #include <osal_defs.h>
 #include <ethercatmain.h>
 #include <ethercatbase.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
 
 //EtherCAT definitions only used for the ethercat stuff
 #define SDO_READ_TIMEOUT 100000
@@ -719,10 +720,15 @@ void EtherCAT::errorHandling()
     uint16 slave;
     //if the bus is in already in safe op then we have an error and don't need any more stuff to do
     error = ec_slave[0].state == EC_STATE_SAFE_OP;
+
     if (lastWorkCounter == expectedWKC)
     {
         noErrorIterations++;
     }
+    ARMARX_ON_SCOPE_EXIT
+    {
+        firstErrorCheck = false;
+    };
     if (((lastWorkCounter < expectedWKC) || ec_group[currentGroup].docheckstate) && !error)
     {
 
@@ -733,7 +739,7 @@ void EtherCAT::errorHandling()
 
         //actually there is something wrong so we have an error lets see if we can find an fix it
         error = true;
-        if (checkErrorCountersOnWKCError)
+        if (checkErrorCountersOnWKCError || (!firstErrorCheck && noErrorIterations == 0))
         {
             IceUtil::Time start = IceUtil::Time::now(IceUtil::Time::Monotonic);
             for (uint16 slaveIndex = 1; slaveIndex <= *(ecx_context.slavecount); slaveIndex++)

source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h

@@ -212,6 +212,7 @@ namespace armarx
         /** the working counter from the last transmission */
         int lastWorkCounter;
         int noErrorIterations = 0;
+        bool firstErrorCheck = true;
 
         /** List of all functional/internal Units */
         std::vector<AbstractFunctionalDevicePtr> functionalDevices;