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

Timestamp:

Apr 26, 2017, 11:12:59 AM (8 years ago)

Author:

Sanahuja Guillaume

Message:

modif imu

Location:

trunk/lib/FlairSensorActuator/src

Files:

• Gx3_25_imu_impl.cpp (modified) (5 diffs)
• Imu.cpp (modified) (1 diff)
• Imu.h (modified) (2 diffs)
• SimuImu.cpp (modified) (1 diff)

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

trunk/lib/FlairSensorActuator/src/Imu.cpp

@@ -64 +64 @@
 }
 
-void Imu::UpdateImu() {
+void Imu::ApplyRotation(Vector3Df& vector) {
   if (rotation == NULL) {
     Err("not applicable for simulation part.\n");
     return;
   }
-  Vector3Df rawAcc, rawMag, rawGyr;
-  imuData->GetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
-  rotation->ComputeRotation(rawAcc);
-  rotation->ComputeRotation(rawGyr);
-  rotation->ComputeRotation(rawMag);
-  imuData->SetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
+  rotation->ComputeRotation(vector);
+}
+
+void Imu::ApplyRotation(Quaternion& quaternion) {
+  if (rotation == NULL) {
+    Err("not applicable for simulation part.\n");
+    return;
+  }
+  rotation->ComputeRotation(quaternion);
 }
 

trunk/lib/FlairSensorActuator/src/Imu.h

@@ -15 +15 @@
 
 #include <IODevice.h>
+#include <Vector3D.h>
 
 namespace flair {
@@ -121 +122 @@
 
   /*!
-  * \brief UpdateImu
+  * \brief ApplyRotation
   *
-  * The reimplemented class must call this function as soon as IMU datas are
-  *available. \n
+  * The reimplemented class must call this function to rotate IMU datas, before filling the ImuData. \n
   * It handles the data rotation if it was defined.
   *
+  * \param vector vector to apply rotation to
   */
-  void UpdateImu();
+  void ApplyRotation(core::Vector3Df& vector);
+  
+  /*!
+  * \brief ApplyRotation
+  *
+  * The reimplemented class must call this function to rotate IMU datas, before filling the ImuData. \n
+  * It handles the data rotation if it was defined.
+  *
+  * \param quaternion quaternion to apply rotation to
+  */
+  void ApplyRotation(core::Quaternion& quaternion);
 
   /*!

trunk/lib/FlairSensorActuator/src/SimuImu.cpp

@@ -105 +105 @@
 
     shmem->Read((char *)&state, sizeof(imu_states_t));
-    ahrsData->SetQuaternionAndAngularRates(
-        Quaternion(state.q0, state.q1, state.q2, state.q3),
-        Vector3Df(state.wx, state.wy, state.wz));
+    
+    Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
+    Vector3Df angRate(state.wx, state.wy, state.wz);
+    ApplyRotation(angRate);
+    ApplyRotation(quaternion);
+    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
 
     imuData->SetDataTime(GetTime());
     ahrsData->SetDataTime(GetTime());
 
-    UpdateImu();
     ProcessUpdate(ahrsData);
   }