Changeset 186 in flair-src for trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

Timestamp:

06/23/17 17:33:03 (7 years ago)

Author:

Sanahuja Guillaume

Message:

maj imu

File:

• trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp (modified) (3 diffs)

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);