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

Timestamp:

04/26/17 11:12:59 (6 years ago)

Author:

Sanahuja Guillaume

Message:

modif imu

File:

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

trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

@@ -29 +29 @@
 #include <Vector3D.h>
 #include <RotationMatrix.h>
+#include <OneAxisRotation.h>
 
 #include <math.h>
@@ -135 +136 @@
       filteredAngRates.z = Dequeue(&buf);
 
-      ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),
+      Quaternion quaternion=eulerAngles.ToQuaternion();
+      self->ApplyRotation(filteredAngRates);
+      self->ApplyRotation(quaternion);
+      ahrsData->SetQuaternionAndAngularRates(quaternion,
                                              filteredAngRates);
     } else if (command ==
@@ -164 +168 @@
       matrix(2, 2) = Dequeue(&buf);
 
-      ahrsData->SetQuaternionAndAngularRates(matrix.ToQuaternion(),
+      Quaternion quaternion=matrix.ToQuaternion();
+      self->ApplyRotation(filteredAngRates);
+      self->ApplyRotation(quaternion);
+      ahrsData->SetQuaternionAndAngularRates(quaternion,
                                              filteredAngRates);
+                                             
+      self->ApplyRotation(rawAcc);
       imuData->SetRawAcc(rawAcc);
     } else if (command == Gx3_25_imu::Quaternion) {
@@ -183 +192 @@
       filteredAngRates.z = 0;
 
+      self->ApplyRotation(filteredAngRates);
+      self->ApplyRotation(quaternion);
       ahrsData->SetQuaternionAndAngularRates(quaternion,
                                              filteredAngRates);
@@ -207 +218 @@
     imuData->SetDataTime(imuTime);
     ahrsData->SetDataTime(imuTime);
-    self->UpdateImu();
     self->ProcessUpdate(ahrsData);
   }