diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 0ab4790e9bf6e6227448594bb17da2ef7f4e0e2d..c9279c4ed0f0110579bfc8dfc7c5e6f06eac2b3f 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -88,7 +88,7 @@ namespace armarx
             {
                 auto nodesets = localReportRobot->getRobotNodeSets();
 
-                for (RobotNodeSetPtr & set : nodesets)
+                for (RobotNodeSetPtr& set : nodesets)
                 {
                     if (set->getTCP())
                     {
@@ -104,7 +104,7 @@ namespace armarx
                              boost::is_any_of(","),
                              boost::token_compress_on);
 
-                for (auto & name : nodesetNames)
+                for (auto& name : nodesetNames)
                 {
                     auto node = localReportRobot->getRobotNode(name);
 
@@ -256,20 +256,20 @@ namespace armarx
 
     void TCPControlUnit::request(const Ice::Current& c)
     {
-        ARMARX_IMPORTANT << "Requesting TCPControlUnit";
-        ScopedLock lock(dataMutex);
-        requested = true;
-
         if (execTask)
         {
-            while (calculationRunning)
-            {
-                IceUtil::ThreadControl::yield();
-            }
+            //            while (calculationRunning)
+            //            {
+            //                IceUtil::ThreadControl::yield();
+            //            }
 
             execTask->stop();
         }
 
+        ARMARX_IMPORTANT << "Requesting TCPControlUnit";
+        ScopedLock lock(dataMutex);
+        requested = true;
+
         lastCommandTime = IceUtil::Time::now();
         execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
         execTask->start();
@@ -282,10 +282,10 @@ namespace armarx
         ARMARX_IMPORTANT << "Releasing TCPControlUnit";
         ScopedLock lock(dataMutex);
 
-        while (calculationRunning)
-        {
-            IceUtil::ThreadControl::yield();
-        }
+        //        while (calculationRunning)
+        //        {
+        //            IceUtil::ThreadControl::yield();
+        //        }
 
         tcpVelocitiesMap.clear();
         localTcpVelocitiesMap.clear();
@@ -304,8 +304,9 @@ namespace armarx
 
     void TCPControlUnit::periodicExec()
     {
+        ScopedTryLock lock(dataMutex);
+
         {
-            ScopedTryLock lock(dataMutex);
 
             if (lock.owns_lock())
             {