Index: trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp	(revision 180)
+++ trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp	(revision 186)
@@ -80,4 +80,5 @@
   Time imuTime;
   ImuData *imuData;
+  Quaternion prevQuat;prevQuat.q0=-2;//for init
   self->GetDatas(&imuData);
 
@@ -115,7 +116,4 @@
   }
 
-  // periode a passer en argument (reglable)
-  // ou plutot laisser la periode geree par le centrale (polling)
-  // self->SetPeriodMS(2);
   SetContinuousMode(command);
 
@@ -169,4 +167,28 @@
 
       Quaternion quaternion=matrix.ToQuaternion();
+      
+      //try to keep quaternion continuous (select between quaternion or -quaternion)
+      if(prevQuat.q0!=-2) {//skip first iteration
+        //search the greatest absolute value to avoid problems near 0 (quaternion's norm=1)
+        if(fabs(prevQuat.q0)>=fabs(prevQuat.q1) && fabs(prevQuat.q0)>=fabs(prevQuat.q2) && fabs(prevQuat.q0)>=fabs(prevQuat.q3)) {
+          if(prevQuat.q0*quaternion.q0<0) {//if signs are different
+            quaternion=-quaternion;
+          }
+        } else if(fabs(prevQuat.q1)>=fabs(prevQuat.q0) && fabs(prevQuat.q1)>=fabs(prevQuat.q2) && fabs(prevQuat.q1)>=fabs(prevQuat.q3)) {
+          if(prevQuat.q1*quaternion.q1<0) {//if signs are different
+            quaternion=-quaternion;
+          }
+        } else if(fabs(prevQuat.q2)>=fabs(prevQuat.q0) && fabs(prevQuat.q2)>=fabs(prevQuat.q1) && fabs(prevQuat.q2)>=fabs(prevQuat.q3)) {
+          if(prevQuat.q2*quaternion.q2<0) {//if signs are different
+            quaternion=-quaternion;
+          }
+        } else if(fabs(prevQuat.q3)>=fabs(prevQuat.q0) && fabs(prevQuat.q3)>=fabs(prevQuat.q1) && fabs(prevQuat.q3)>=fabs(prevQuat.q2)) {
+          if(prevQuat.q3*quaternion.q3<0) {//if signs are different
+            quaternion=-quaternion;
+          }
+        }
+      }
+      prevQuat=quaternion;
+      
       self->ApplyRotation(filteredAngRates);
       self->ApplyRotation(quaternion);
Index: trunk/lib/FlairSensorActuator/src/Imu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Imu.cpp	(revision 180)
+++ trunk/lib/FlairSensorActuator/src/Imu.cpp	(revision 186)
@@ -42,4 +42,5 @@
   setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
   rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation");
+  AddDataToLog(imuData);
 }
 
Index: trunk/lib/FlairSensorActuator/src/Mb800.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Mb800.cpp	(revision 180)
+++ trunk/lib/FlairSensorActuator/src/Mb800.cpp	(revision 186)
@@ -108,5 +108,5 @@
 
   while (!ToBeStopped()) {
-    SleepMS(10);
+    //SleepMS(10);??
     size = 0;
     while (!ToBeStopped()) {
Index: trunk/lib/FlairSensorActuator/src/SimuImu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 180)
+++ trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 186)
@@ -79,4 +79,10 @@
     state.wy = input->ValueNoMutex(8, 0);
     state.wz = input->ValueNoMutex(9, 0);
+    state.ax = input->ValueNoMutex(13, 0);
+    state.ay = input->ValueNoMutex(14, 0);
+    state.az = input->ValueNoMutex(15, 0);
+    state.mx = input->ValueNoMutex(16, 0);
+    state.my = input->ValueNoMutex(17, 0);
+    state.mz = input->ValueNoMutex(18, 0);
     input->ReleaseMutex();
 
@@ -108,8 +114,14 @@
     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);
+    Vector3Df rawMag(state.mx, state.my, state.mz);
+    Vector3Df rawGyr(state.wx, state.wy, state.wz);
     ApplyRotation(angRate);
     ApplyRotation(quaternion);
+    ApplyRotation(rawAcc);
+    ApplyRotation(rawMag);
+    ApplyRotation(rawGyr);
     ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
-
+    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
     imuData->SetDataTime(GetTime());
     ahrsData->SetDataTime(GetTime());
Index: trunk/lib/FlairSensorActuator/src/SimuImu.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuImu.h	(revision 180)
+++ trunk/lib/FlairSensorActuator/src/SimuImu.h	(revision 186)
@@ -94,4 +94,10 @@
     float wy;
     float wz;
+    float ax;
+    float ay;
+    float az;
+    float mx;
+    float my;
+    float mz;
   } imu_states_t;
   
Index: trunk/lib/FlairSensorActuator/src/TargetController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 180)
+++ trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 186)
@@ -118,5 +118,6 @@
   Message *message;
 
-  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
+  //if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
+  if (!ControllerInitialization()) {
     SafeStop();
     Thread::Err("An error occured, we don't proceed with the loop.\n");
