Index: trunk/lib/FlairCore/src/Thread.cpp
===================================================================
--- trunk/lib/FlairCore/src/Thread.cpp	(revision 201)
+++ trunk/lib/FlairCore/src/Thread.cpp	(revision 202)
@@ -91,6 +91,6 @@
   if (status != 0) {
 		char errorMsg[256];
-    Err("%s, error rt_task_sleep_until (%s), resume time: %lld, actual time: "
-        "%lld\n", ObjectName().c_str(), strerror_r(-status, errorMsg, sizeof(errorMsg)), time, GetTime());
+    Err("%error rt_task_sleep_until (%s), resume time: %lld, actual time: "
+        "%lld\n",strerror_r(-status, errorMsg, sizeof(errorMsg)), time, GetTime());
 	}
 // Printf("rt_task_sleep_until, resume time: %lld, actual time:
@@ -99,5 +99,13 @@
   Time current = GetTime();
   if (current < time) {
-    usleep((time - current) / 1000);
+    //usleep((time - current) / 1000);
+    struct timespec req;
+    req.tv_nsec = time - current;
+    req.tv_sec = req.tv_nsec / 1000000000;
+    req.tv_nsec %= 1000000000;
+    struct timespec rem;
+    if(nanosleep(&req,&rem)!=0) Err("error in nanosleep\n");//todo, handle EINTR
+  } else {
+    Err("error resume time is passed by %lldns\n", ( current-time));
   }
 #endif
@@ -112,5 +120,11 @@
 	}
 #else
-  usleep(time * 1000);
+  //usleep(time * 1000);
+  struct timespec req;
+  req.tv_nsec = time * 1000000;
+  req.tv_sec = req.tv_nsec / 1000000000;
+  req.tv_nsec %= 1000000000;
+  struct timespec rem;
+  if(nanosleep(&req,&rem)!=0) Err("error in nanosleep\n");//todo, handle EINTR
 #endif
 }
@@ -124,5 +138,11 @@
 	}
 #else
-  usleep(time);
+  //usleep(time);
+  struct timespec req;
+  req.tv_nsec = time * 1000;
+  req.tv_sec = req.tv_nsec / 1000000000;
+  req.tv_nsec %= 1000000000;
+  struct timespec rem;
+  if(nanosleep(&req,&rem)!=0) Err("error in nanosleep\n");//todo, handle EINTR
 #endif
 }
Index: trunk/lib/FlairCore/src/Thread_impl.cpp
===================================================================
--- trunk/lib/FlairCore/src/Thread_impl.cpp	(revision 201)
+++ trunk/lib/FlairCore/src/Thread_impl.cpp	(revision 202)
@@ -232,5 +232,9 @@
 #else
   self->SleepUntil(next_time);
-  next_time += period;
+  Time current = GetTime();
+  if(current>next_time+period) self->Err("overrun of %lld\n", current-next_time);
+  while (next_time < current) {
+    next_time += period;
+  }
 #endif
   ComputeLatency(GetTime());
@@ -326,5 +330,6 @@
   if(last!=0)
 #endif
-    { Printf("Thread::%s :\n", self->ObjectName().c_str()); }
+    { Printf("Thread::%s :\n", self->ObjectName().c_str()); 
+    if(period_set) {Printf("    period (ns): %lld\n", period);}}
 #ifdef __XENO__
     Printf("    number of context switches: %i\n", info.ctxswitches);
@@ -349,5 +354,5 @@
       Printf("    max latency (ns): %lld\n", max_latency);
       Printf("    latency moy (ns): %lld\n", mean_latency / cpt);
-      Printf("    itertions: %lld\n", cpt);
+      Printf("    iterations: %lld\n", cpt);
     }
   }
Index: trunk/lib/FlairSensorActuator/src/EmulatedController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/EmulatedController.cpp	(revision 201)
+++ trunk/lib/FlairSensorActuator/src/EmulatedController.cpp	(revision 202)
@@ -19,6 +19,4 @@
 #include <cvmatrix.h>
 #include <stdexcept>
-
-#include <unistd.h> // for sleep (debug)
 
 using namespace flair::core;
Index: trunk/lib/FlairSensorActuator/src/SimuImu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 201)
+++ trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 202)
@@ -34,4 +34,5 @@
 namespace sensor {
 
+//Construct a SimuImu. Control part.
 SimuImu::SimuImu(string name, uint32_t modelId,uint32_t deviceId,
                  uint8_t priority)
@@ -46,4 +47,5 @@
 }
 
+//Construct a SimuImu. Simulation part.
 SimuImu::SimuImu(const IODevice *parent, string name, uint32_t modelId,uint32_t deviceId)
     : Imu(parent,name), Thread(parent, name, 0) {
@@ -109,8 +111,6 @@
       SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));
     }
-
     shmem->Read((char *)&state, sizeof(imu_states_t));
-    
-    Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
+    Quaternion quaternion(state.q0, state.q1, state.q2, state.q3);
     Vector3Df angRate(state.wx, state.wy, state.wz);
     Vector3Df rawAcc(state.ax, state.ay, state.az);
@@ -128,5 +128,4 @@
     imuData->SetDataTime(GetTime());
     ahrsData->SetDataTime(GetTime());
-
     ProcessUpdate(ahrsData);
   }
Index: trunk/lib/FlairSensorActuator/src/TargetController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 201)
+++ trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 202)
@@ -24,6 +24,4 @@
 #include <cstring>
 #include <string>
-
-#include <unistd.h> //for usleep
 
 using namespace flair::core;
Index: trunk/lib/FlairSensorActuator/src/TargetEthController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetEthController.cpp	(revision 201)
+++ trunk/lib/FlairSensorActuator/src/TargetEthController.cpp	(revision 202)
@@ -27,6 +27,4 @@
 #include <stdexcept>
 
-#include <unistd.h> // for sleep (debug)
-
 using namespace flair::core;
 using namespace flair::gui;
Index: trunk/lib/FlairSimulator/src/Model_impl.cpp
===================================================================
--- trunk/lib/FlairSimulator/src/Model_impl.cpp	(revision 201)
+++ trunk/lib/FlairSimulator/src/Model_impl.cpp	(revision 202)
@@ -54,9 +54,9 @@
 Model_impl::Model_impl(Model *self, std::string name,uint32_t modelId)
     : ISceneNode(getGui()->getSceneManager()->getRootSceneNode(), getGui()->getSceneManager(), -1),
-      Thread(self, name, 50), vrpn_Tracker(name.c_str(), getSimulator()->pimpl_)
+      Thread(self, name, 99), vrpn_Tracker(name.c_str(), getSimulator()->pimpl_)
 
 #else
 Model_impl::Model_impl(Model *self, std::string name,uint32_t modelId)
-    : Thread(self, name, 50), vrpn_Tracker(name.c_str(), getSimulator()->pimpl_)
+    : Thread(self, name, 99), vrpn_Tracker(name.c_str(), getSimulator()->pimpl_)
 #endif
 {
