Index: trunk/lib/FlairCore/src/io_data.cpp
===================================================================
--- trunk/lib/FlairCore/src/io_data.cpp	(revision 221)
+++ trunk/lib/FlairCore/src/io_data.cpp	(revision 223)
@@ -46,4 +46,12 @@
 void io_data::SetDataTime(Time time) {
   GetMutex();
+  if(pimpl_->time!=TIME_INFINITE) pimpl_->deltaTime = time-pimpl_->time;
+  pimpl_->time = time;
+  ReleaseMutex();
+}
+
+void io_data::SetDataTime(Time time,Time deltaTime) {
+  GetMutex();
+  pimpl_->deltaTime = deltaTime;
   pimpl_->time = time;
   ReleaseMutex();
@@ -56,4 +64,19 @@
   ReleaseMutex();
   return tmp;
+}
+
+Time io_data::DataDeltaTime(void) const {
+  Time tmp;
+  GetMutex();
+  tmp = pimpl_->deltaTime;
+  ReleaseMutex();
+  return tmp;
+}
+
+void io_data::GetDataTime(Time &time,Time &deltaTime) const{
+  GetMutex();
+  deltaTime=pimpl_->deltaTime ;
+  time=pimpl_->time ;
+  ReleaseMutex();
 }
 
Index: trunk/lib/FlairCore/src/io_data.h
===================================================================
--- trunk/lib/FlairCore/src/io_data.h	(revision 221)
+++ trunk/lib/FlairCore/src/io_data.h	(revision 223)
@@ -116,16 +116,49 @@
 
   /*!
-  * \brief Set data time
-  *
+  * \brief Set data time, also caluculates the delta time based on last call
+  *
+  * TIME_INFINITE represents an unitialized time
+  * 
   * \param time time
   */
   void SetDataTime(Time time);
+  
+  /*!
+  * \brief Set data time and  delta time (thus delta time is not based on last call)
+  *
+  * TIME_INFINITE represents an unitialized time
+  * 
+  * \param time time
+  * \param deltaTime delta time
+  */
+  void SetDataTime(Time time,Time deltaTime);
 
   /*!
   * \brief Data time
   *
+  * TIME_INFINITE represents an unitialized time.
+  * 
   * \return data time
   */
   Time DataTime(void) const;
+  
+  /*!
+  * \brief Data delta time
+  *
+  * TIME_INFINITE represents an unitialized time.
+  * 
+  * \return data delta time
+  */
+  Time DataDeltaTime(void) const;
+  
+  /*!
+  * \brief Get data time and  delta time
+  *
+  * TIME_INFINITE represents an unitialized time
+  * 
+  * \param time time
+  * \param deltaTime delta time
+  */
+  void GetDataTime(Time &time,Time &deltaTime) const;
 
   /*!
Index: trunk/lib/FlairCore/src/io_data_impl.cpp
===================================================================
--- trunk/lib/FlairCore/src/io_data_impl.cpp	(revision 221)
+++ trunk/lib/FlairCore/src/io_data_impl.cpp	(revision 223)
@@ -31,4 +31,6 @@
   is_consistent = false;
   circle_ptr = NULL;
+  deltaTime=TIME_INFINITE;//init
+  time=TIME_INFINITE;//init
 }
 
Index: trunk/lib/FlairCore/src/unexported/io_data_impl.h
===================================================================
--- trunk/lib/FlairCore/src/unexported/io_data_impl.h	(revision 221)
+++ trunk/lib/FlairCore/src/unexported/io_data_impl.h	(revision 223)
@@ -37,5 +37,5 @@
                             flair::core::DataType const &datatype);
   size_t size;
-  flair::core::Time time;
+  flair::core::Time time,deltaTime;
   void **circle_ptr;
 
Index: trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp
===================================================================
--- trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp	(revision 223)
@@ -35,5 +35,5 @@
 namespace flair { namespace filter {
 
-AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): isInit(false), Ahrs(parent,name) {
+AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): Ahrs(parent,name) {
 
   QHat.q0=1;
@@ -76,6 +76,5 @@
     input->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
 
-    delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
-    previous_time=data->DataTime();
+    delta_t=(float)(data->DataDeltaTime())/1000000000.;
 
     Vector3Df aBar,aHat,aTilde;
@@ -89,5 +88,5 @@
     magRef=this->magRef->Value();
 
-    if(isInit==true) {
+    if(delta_t!=0) {
         // CORRECTION FROM ACCELEROMETER
         aBar = rawAcc;
@@ -153,7 +152,5 @@
         ahrsData->SetQuaternionAndAngularRates(QHat,rawGyr - BHat);
 
-    } else {
-        isInit=true;
-    }
+    } 
 
     ahrsData->SetDataTime(data->DataTime());
Index: trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h
===================================================================
--- trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h	(revision 221)
+++ trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h	(revision 223)
@@ -63,7 +63,4 @@
             void UpdateFrom(const core::io_data *data);
 
-            core::Time previous_time;
-
-            bool isInit;
             core::Quaternion QHat;
             core::Vector3Df BHat;
Index: trunk/lib/FlairFilter/src/AhrsKalman_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/AhrsKalman_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/AhrsKalman_impl.cpp	(revision 223)
@@ -65,5 +65,4 @@
 
 AhrsKalman_impl::AhrsKalman_impl(const Layout* layout,AhrsData *inAhrsData): ahrsData(inAhrsData) {
-    is_init=false;
     euler.yaw=0;
 
@@ -90,6 +89,5 @@
     input->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
 
-    delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
-    previous_time=data->DataTime();
+    delta_t=(float)(data->DataDeltaTime())/1000000000.;
 
     if(Q_angle->ValueChanged() || Q_gyro->ValueChanged() || R_angle->ValueChanged()) {
@@ -98,5 +96,5 @@
     }
 
-    if(is_init==true) {
+    if(delta_t!=0) {
         //execute kalman roll filter
         ars_predict(&ars_roll, rawGyr.x, delta_t);
@@ -116,8 +114,5 @@
         ahrsData->SetQuaternionAndAngularRates(euler.ToQuaternion(),
                                                rawGyr-Vector3Df(ars_roll.x_bias,ars_pitch.x_bias,0));
-    } else {
-        is_init=true;
     }
-
     ahrsData->SetDataTime(data->DataTime());
 }
Index: trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp	(revision 223)
@@ -56,5 +56,4 @@
   }
 
-  first_update = true;
   this->order=order;
   this->nbRow=nbRow;
@@ -91,5 +90,5 @@
 
   if (T->Value() == 0) {
-    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+    delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
     for(uint32_t i=0;i<nbRow;i++) {
       for(uint32_t j=0;j<nbCol;j++) {
@@ -114,7 +113,5 @@
   }
 
-  if (first_update == true) {
-    first_update = false;
-  } else {
+  if (delta_t!=0) {
     for(uint32_t i=0;i<nbRow;i++) {
       for(uint32_t j=0;j<nbCol;j++) {
@@ -129,5 +126,4 @@
 
   output->SetDataTime(data->DataTime());
-  previous_time = data->DataTime();
 }
 
Index: trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp	(revision 223)
@@ -32,5 +32,4 @@
                                            string name,
                                            const Matrix *init_value) {
-  first_update = true;
   this->self = self;
   
@@ -94,15 +93,17 @@
   } else {
     if (T->Value() == 0) {
-      delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+      delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
     } else {
       delta_t = T->Value();
     }
 
-    for (int i = 0; i < input->Rows(); i++) {
-      for (int j = 0; j < input->Cols(); j++) {
-        output->SetValueNoMutex(
-            i, j, (input->ValueNoMutex(i, j) - prev_value->ValueNoMutex(i, j)) /
-                      delta_t);
-        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+    if(delta_t!=0) {
+      for (int i = 0; i < input->Rows(); i++) {
+        for (int j = 0; j < input->Cols(); j++) {
+          output->SetValueNoMutex(
+              i, j, (input->ValueNoMutex(i, j) - prev_value->ValueNoMutex(i, j)) /
+                        delta_t);
+          prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+        }
       }
     }
@@ -113,4 +114,3 @@
 
   output->SetDataTime(data->DataTime());
-  previous_time = data->DataTime();
 }
Index: trunk/lib/FlairFilter/src/JoyReference_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/JoyReference_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/JoyReference_impl.cpp	(revision 223)
@@ -69,5 +69,4 @@
 
   z_ref = 0;
-  previous_time = 0;
   trim_roll = 0;
   trim_pitch = 0;
@@ -154,8 +153,5 @@
   }
 
-  if (previous_time == 0)
-    previous_time = data->DataTime(); // pour la premiere iteration
-  float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
-  previous_time = data->DataTime();
+  float delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
 
   if (button_roll->Clicked() == true) {
Index: trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp	(revision 223)
@@ -67,5 +67,4 @@
                            " Hz", 0, 10000, 0.1, 2, 1);
                            
-  previous_time=GetTime();
   this->self = self;
 }
@@ -87,18 +86,21 @@
 
   if (T->Value() == 0) {
-    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+    delta_t = (float)(data->DataDeltaTime() ) / 1000000000.;
   } else {
     delta_t = T->Value();
   }
-  for (int i = 0; i < input->Rows(); i++) {
-    for (int j = 0; j < input->Cols(); j++) {
-      float cutoff=freq->Value();
-      if (cutoff == 0 || freq->ValueChanged()) {
-        output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
-      } else {
-        output->SetValueNoMutex(i, j, (1 - 2 * PI * cutoff * delta_t) *
-                                              output->ValueNoMutex(i, j) +
-                                          2 * PI * cutoff * delta_t *
-                                              input->ValueNoMutex(i, j));
+  
+  if(delta_t!=0) {
+    for (int i = 0; i < input->Rows(); i++) {
+      for (int j = 0; j < input->Cols(); j++) {
+        float cutoff=freq->Value();
+        if (cutoff == 0 || freq->ValueChanged()) {
+          output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
+        } else {
+          output->SetValueNoMutex(i, j, (1 - 2 * PI * cutoff * delta_t) *
+                                                output->ValueNoMutex(i, j) +
+                                            2 * PI * cutoff * delta_t *
+                                                input->ValueNoMutex(i, j));
+        }
       }
     }
@@ -109,4 +111,3 @@
 
   output->SetDataTime(data->DataTime());
-  previous_time = data->DataTime();
 }
Index: trunk/lib/FlairFilter/src/PidThrust_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/PidThrust_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/PidThrust_impl.cpp	(revision 223)
@@ -84,5 +84,5 @@
 
   if (T->Value() == 0) {
-    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+    delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
   } else {
     delta_t = T->Value();
@@ -121,5 +121,3 @@
   self->output->SetValue(0, 0, total - offset_g * offset_g);
   self->output->SetDataTime(data->DataTime());
-
-  previous_time = data->DataTime();
 }
Index: trunk/lib/FlairFilter/src/Pid_impl.cpp
===================================================================
--- trunk/lib/FlairFilter/src/Pid_impl.cpp	(revision 221)
+++ trunk/lib/FlairFilter/src/Pid_impl.cpp	(revision 223)
@@ -79,5 +79,5 @@
 
   if (T->Value() == 0) {
-    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
+    delta_t = (float)(data->DataDeltaTime() ) / 1000000000.;
   } else {
     delta_t = T->Value();
@@ -113,5 +113,3 @@
   self->output->SetValue(0, 0, total);
   self->output->SetDataTime(data->DataTime());
-
-  previous_time = data->DataTime();
 }
Index: trunk/lib/FlairFilter/src/unexported/AhrsKalman_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/AhrsKalman_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/AhrsKalman_impl.h	(revision 223)
@@ -79,6 +79,4 @@
         ars_Gyro1DKalman ars_roll;
         ars_Gyro1DKalman ars_pitch;
-        flair::core::Time previous_time;
-        bool is_init;
         flair::gui::DoubleSpinBox *Q_angle,*Q_gyro,*R_angle;
         flair::core::Euler euler;
Index: trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h	(revision 223)
@@ -131,6 +131,4 @@
   flair::gui::DoubleSpinBox *cutoff, *T;
   PoleFilter **f;
-  bool first_update;
-  flair::core::Time previous_time;
   uint32_t order;
   uint32_t nbRow, nbCol;
Index: trunk/lib/FlairFilter/src/unexported/EulerDerivative_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/EulerDerivative_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/EulerDerivative_impl.h	(revision 223)
@@ -45,5 +45,4 @@
 private:
   flair::gui::DoubleSpinBox *T;
-  flair::core::Time previous_time;
   bool first_update;
   flair::core::Matrix *prev_value;
Index: trunk/lib/FlairFilter/src/unexported/JoyReference_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/JoyReference_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/JoyReference_impl.h	(revision 223)
@@ -80,5 +80,4 @@
   flair::core::Quaternion q_z = flair::core::Quaternion(1, 0, 0, 0);
   float trim_roll, trim_pitch;
-  flair::core::Time previous_time;
 
   flair::filter::JoyReference *self;
Index: trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.h	(revision 223)
@@ -42,5 +42,4 @@
 
 private:
-  flair::core::Time previous_time;
   flair::gui::DoubleSpinBox *freq, *T;
   const flair::filter::LowPassFilter *self;
Index: trunk/lib/FlairFilter/src/unexported/PidThrust_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/PidThrust_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/PidThrust_impl.h	(revision 223)
@@ -46,5 +46,4 @@
 private:
   flair::filter::PidThrust *self;
-  flair::core::Time previous_time;
   bool first_update;
 
Index: trunk/lib/FlairFilter/src/unexported/Pid_impl.h
===================================================================
--- trunk/lib/FlairFilter/src/unexported/Pid_impl.h	(revision 221)
+++ trunk/lib/FlairFilter/src/unexported/Pid_impl.h	(revision 223)
@@ -46,5 +46,4 @@
 private:
   flair::filter::Pid *self;
-  flair::core::Time previous_time;
 
   // matrix
Index: trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp	(revision 221)
+++ trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp	(revision 223)
@@ -93,5 +93,6 @@
     parent->pimpl_->AddTrackable(this);
   }
- 
+  
+  previousTime=TIME_INFINITE;
 }
 
@@ -143,5 +144,13 @@
   Time time = GetTime();
   //Printf("%s %lld %lld\n",caller->self->ObjectName().c_str(),time,t.msg_time.tv_sec*1000000+t.msg_time.tv_usec);
-
+  Time vrpnTime=t.msg_time.tv_sec*1000000000+t.msg_time.tv_usec*1000;
+  Time deltaTime;
+  if(caller->previousTime!=TIME_INFINITE) {
+    deltaTime=vrpnTime-caller->previousTime;
+  } else {
+    deltaTime=TIME_INFINITE;
+  }
+  caller->previousTime=vrpnTime;
+  
   // check if something is nan
   for (int i = 0; i < 3; i++) {
@@ -178,5 +187,5 @@
   caller->output->SetValueNoMutex(6, 0, pos.z);
 
-  caller->output->SetDataTime(time);
+  caller->output->SetDataTime(time,deltaTime);
   caller->output->ReleaseMutex();
 
Index: trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h	(revision 221)
+++ trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h	(revision 223)
@@ -67,4 +67,5 @@
   vrpn_Tracker_Remote *tracker;
   void Update(void);
+  flair::core::Time previousTime;
 };
 
