Changeset 100 in flair-src

Timestamp:

Oct 5, 2016, 4:11:18 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

passage quaternoin 3dmgx3

Location:

trunk/lib

Files:

• FlairCore/src/RotationMatrix.cpp (modified) (2 diffs)
• FlairCore/src/RotationMatrix.h (modified) (2 diffs)
• FlairMeta/src/HdsX8.cpp (modified) (1 diff)
• FlairMeta/src/XAir.cpp (modified) (1 diff)
• FlairSensorActuator/src/Gx3_25_imu_impl.cpp (modified) (1 diff)

trunk/lib/FlairCore/src/RotationMatrix.cpp

@@ -19 +19 @@
 #include "Object.h"
 #include "Euler.h"
+#include "Quaternion.h"
 #include "math.h"
 
@@ -50 +51 @@
 }
 
+void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
+                quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
+                quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
+                quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
+                quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
+}
+
+Quaternion RotationMatrix::ToQuaternion(void) const {
+  Quaternion quaternion;
+  ToQuaternion(quaternion);
+  return quaternion;
+}
+
 float &RotationMatrix::operator()(size_t row, size_t col) {
   if (row < 3 && col < 3) {

trunk/lib/FlairCore/src/RotationMatrix.h

@@ -18 +18 @@
 namespace core {
 class Euler;
+class Quaternion;
 
 /*! \class RotationMatrix
@@ -52 +53 @@
   */
   Euler ToEuler(void) const;
+
+  /*!
+  * \brief Convert to quaternion
+  *
+  * \param quaternion output quaternion
+  */
+  void ToQuaternion(Quaternion &quaternion) const;
+
+  /*!
+  * \brief Convert to quaternion
+  *
+  * \return quaternion
+  */
+  Quaternion ToQuaternion(void) const;
+
   /*!
   * \brief matrix

trunk/lib/FlairMeta/src/HdsX8.cpp

@@ -48 +48 @@
   SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
   SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
-                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
+                          Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix, 70));
   SetBatteryMonitor(((BlCtrlV2 *)GetBldc())->GetBatteryMonitor());
 

trunk/lib/FlairMeta/src/XAir.cpp

@@ -51 +51 @@
   SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
   SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
-                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
+                          Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix, 70));
   Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
   SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));

trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

@@ -165 +165 @@
       matrix(2, 2) = Dequeue(&buf);
 
-      ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),
+      ahrsData->SetQuaternionAndAngularRates(matrix.ToQuaternion(),
                                              filteredAngRates);
       imuData->SetRawAcc(rawAcc);