Changeset 186 in flair-src for trunk/lib/FlairSensorActuator

Timestamp:

Jun 23, 2017, 5:33:03 PM (7 years ago)

Author:

Sanahuja Guillaume

Message:

maj imu

Location:

trunk/lib/FlairSensorActuator/src

Files:

• Gx3_25_imu_impl.cpp (modified) (3 diffs)
• Imu.cpp (modified) (1 diff)
• Mb800.cpp (modified) (1 diff)
• SimuImu.cpp (modified) (2 diffs)
• SimuImu.h (modified) (1 diff)
• TargetController.cpp (modified) (1 diff)

trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

@@ -80 +80 @@
   Time imuTime;
   ImuData *imuData;
+  Quaternion prevQuat;prevQuat.q0=-2;//for init
   self->GetDatas(&imuData);
 
@@ -115 +116 @@
   }
 
-  // periode a passer en argument (reglable)
-  // ou plutot laisser la periode geree par le centrale (polling)
-  // self->SetPeriodMS(2);
   SetContinuousMode(command);
 
@@ -169 +167 @@
 
       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);

trunk/lib/FlairSensorActuator/src/Imu.cpp

@@ -42 +42 @@
   setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
   rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation");
+  AddDataToLog(imuData);
 }
 

trunk/lib/FlairSensorActuator/src/Mb800.cpp

@@ -108 +108 @@
 
   while (!ToBeStopped()) {
-    SleepMS(10);
+    //SleepMS(10);??
     size = 0;
     while (!ToBeStopped()) {

trunk/lib/FlairSensorActuator/src/SimuImu.cpp

@@ -79 +79 @@
     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 +114 @@
     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());

trunk/lib/FlairSensorActuator/src/SimuImu.h

@@ -94 +94 @@
     float wy;
     float wz;
+    float ax;
+    float ay;
+    float az;
+    float mx;
+    float my;
+    float mz;
   } imu_states_t;
   

trunk/lib/FlairSensorActuator/src/TargetController.cpp

@@ -118 +118 @@
   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");