Merge branch 'EmergencyStopFix' into 'master'

robot unit: fixed wrong setting of emergency stop setting

See merge request ArmarX/RobotAPI!44

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp

@@ -163,7 +163,7 @@ namespace armarx
             {
                 const Management& m = p->_module<Management>();
                 long now = TimeUtil::GetTime(true).toMilliSeconds();
-                if(!m.heartbeatRequired || now < m.controlLoopStartTime + m.heartbeatStartupMarginMS)
+                if (!m.heartbeatRequired || now < m.controlLoopStartTime + m.heartbeatStartupMarginMS)
                 {
                     return false;
                 }
@@ -209,16 +209,18 @@ namespace armarx
 
 
             rtSwitchControllerSetupChangedControllers = false;
-            // !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
-            // !emergencyStop &&  rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
-            //  emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
-            //  emergencyStop &&  rtIsInEmergencyStop() -> nothing to do
+
             // a missing hearbeat (if required by the config) is interpreted as emergencyStop == true
-            if(!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
+            if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
             {
-                emergencyStop = true;
+                rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
                 ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because hearbeat is missing!");
             }
+
+            // !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
+            // !emergencyStop &&  rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
+            //  emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
+            //  emergencyStop &&  rtIsInEmergencyStop() -> nothing to do
             if (emergencyStop)
             {
                 if (rtIsInEmergencyStop())