Changeset 103 in flair-src for trunk

Timestamp:

Oct 11, 2016, 12:07:16 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

maj quaternion 3dmgx3

Location:

trunk/lib

Files:

• FlairCore/src/OneAxisRotation_impl.cpp (modified) (1 diff)
• FlairCore/src/RotationMatrix.cpp (modified) (2 diffs)
• FlairSensorActuator/src/Gx3_25_imu.cpp (modified) (1 diff)
• FlairSensorActuator/src/Gx3_25_imu_impl.cpp (modified) (2 diffs)

trunk/lib/FlairCore/src/OneAxisRotation_impl.cpp

@@ -49 +49 @@
 
 void OneAxisRotation_impl::ComputeRotation(RotationMatrix &matrix) const {
-  printf("not yet implemented\n");
+  Printf("not yet implemented\n");
 }
 

trunk/lib/FlairCore/src/RotationMatrix.cpp

@@ -21 +21 @@
 #include "Quaternion.h"
 #include "math.h"
+
+using std::max;
 
 namespace flair {
@@ -51 +53 @@
 }
 
-//from
-//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+//from 3dmgx3-25 manual
 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
-  float trace = m[0][0] + m[1][1] + m[2][2];
-  if( trace > 0 ) {
-    float s = 0.5f / sqrtf(trace+ 1.0f);
-    quaternion.q0 = 0.25f / s;
-    quaternion.q1 = ( m[2][1] - m[1][2] ) * s;
-    quaternion.q2 = ( m[0][2] - m[2][0] ) * s;
-    quaternion.q3 = ( m[1][0] - m[0][1] ) * s;
-  } else {
-    if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) {
-      float s = 2.0f * sqrtf( 1.0f + m[0][0] - m[1][1] - m[2][2]);
-      quaternion.q0 = (m[2][1] - m[1][2] ) / s;
-      quaternion.q1 = 0.25f * s;
-      quaternion.q2 = (m[0][1] + m[1][0] ) / s;
-      quaternion.q3 = (m[0][2] + m[2][0] ) / s;
-    } else if (m[1][1] > m[2][2]) {
-      float s = 2.0f * sqrtf( 1.0f + m[1][1] - m[0][0] - m[2][2]);
-      quaternion.q0 = (m[0][2] - m[2][0] ) / s;
-      quaternion.q1 = (m[0][1] + m[1][0] ) / s;
-      quaternion.q2 = 0.25f * s;
-      quaternion.q3 = (m[1][2] + m[2][1] ) / s;
-    } else {
-      float s = 2.0f * sqrtf( 1.0f + m[2][2] - m[0][0] - m[1][1] );
-      quaternion.q0 = (m[1][0] - m[0][1] ) / s;
-      quaternion.q1 = (m[0][2] + m[2][0] ) / s;
-      quaternion.q2 = (m[1][2] + m[2][1] ) / s;
-      quaternion.q3 = 0.25f * s;
-    }
+  float test1 = m[0][0] + m[1][1] + m[2][2];
+  float test2 = m[0][0] - m[1][1] - m[2][2];
+  float test3 = -m[0][0] + m[1][1] - m[2][2];
+  float test4 = -m[0][0] - m[1][1] + m[2][2];
+
+  if(test1==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test1);
+    quaternion.q0 = s/4.;
+    quaternion.q1 = ( m[1][2] - m[2][1] ) / s;
+    quaternion.q2 = ( m[2][0] - m[0][2] ) / s;
+    quaternion.q3 = ( m[0][1] - m[1][0] ) / s;
+  }
+
+  if(test2==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test2);
+    quaternion.q0 = ( m[2][1] - m[1][2] ) / s;
+    quaternion.q1 = -s/4.;
+    quaternion.q2 = -( m[1][0] + m[0][1] ) / s;
+    quaternion.q3 = -( m[0][2] + m[2][0] ) / s;
+  }
+
+  if(test3==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test3);
+    quaternion.q0 = (m[0][2] - m[2][0] ) / s;
+    quaternion.q1 = -( m[1][0] + m[0][1] ) / s;
+    quaternion.q2 = -s/4.;
+    quaternion.q3 = -( m[2][1] + m[1][2] ) / s;
+  }
+
+  if(test4==max(max(test1,test2),max(test3,test4))) {
+    float s=2*sqrtf(1+test4);
+    quaternion.q0 = (m[1][0] - m[0][1] ) / s;
+    quaternion.q1 = -( m[0][2] + m[2][0] ) / s;
+    quaternion.q2 = -( m[2][1] + m[1][2] ) / s;
+    quaternion.q3 = -s/4.;
   }
   quaternion.Normalize();
 }
-/*
-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]);
-                //Printf("%f %f %f\n", m[0][0] , m[1][1] , m[2][2]);
-                //Printf("%f %f %f\n",1.0f + m[0][0] - m[1][1] - m[2][2],1.0f - m[0][0] + m[1][1] - m[2][2],1.0f - m[0][0] - m[1][1] + m[2][2]);
-}
-*/
+
 Quaternion RotationMatrix::ToQuaternion(void) const {
   Quaternion quaternion;

trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp

@@ -35 +35 @@
 
   if (command == EulerAnglesAndAngularRates) {
-
+    Printf("Gx3_25_imu enabling EulerAnglesAndAngularRates\n");
   } else if (command == AccelerationAngularRateAndOrientationMatrix) {
-    //Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
+    Printf("Gx3_25_imu enabling AccelerationAngularRateAndOrientationMatrix\n");
   } else if (command == Quaternion) {
-
+    Printf("Gx3_25_imu enabling Quaternion\n");
   } else {
     Thread::Err("command not supported (%i)\n", command);

trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

@@ -118 +118 @@
   SetContinuousMode(command);
 
-  //_printf("firmware version: %i\n",get_firmware_number());
-
   while (!self->ToBeStopped()) {
     if (command == Gx3_25_imu::EulerAnglesAndAngularRates) {
@@ -352 +350 @@
   command[5] = rate & 0xff;        // data rate LSB
   result = 0;
-  if (disable_magn->IsChecked() == true)
-    result |= 0x01;
-  if (disable_north_comp->IsChecked() == true)
-    result |= 0x04;
-  if (disable_grav_comp->IsChecked() == true)
-    result |= 0x08;
-
+  if (disable_magn->IsChecked() == true) result |= 0x01;
+  if (disable_north_comp->IsChecked() == true) result |= 0x04;
+  if (disable_grav_comp->IsChecked() == true) result |= 0x08;
+  if (this->command == Gx3_25_imu::Quaternion) result |= 0x10;//enable quaternion
   command[6] = result;
+
   result = 0x01; // Calculate orientation
   if (coning->IsChecked() == true)