Changeset 103 in flair-src
- Timestamp:
- Oct 11, 2016, 12:07:16 PM (8 years ago)
- Location:
- trunk/lib
- Files:
-
- 4 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairCore/src/OneAxisRotation_impl.cpp
r15 r103 49 49 50 50 void OneAxisRotation_impl::ComputeRotation(RotationMatrix &matrix) const { 51 printf("not yet implemented\n");51 Printf("not yet implemented\n"); 52 52 } 53 53 -
trunk/lib/FlairCore/src/RotationMatrix.cpp
r101 r103 21 21 #include "Quaternion.h" 22 22 #include "math.h" 23 24 using std::max; 23 25 24 26 namespace flair { … … 51 53 } 52 54 53 //from 54 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ 55 //from 3dmgx3-25 manual 55 56 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const { 56 float trace = m[0][0] + m[1][1] + m[2][2]; 57 if( trace > 0 ) { 58 float s = 0.5f / sqrtf(trace+ 1.0f); 59 quaternion.q0 = 0.25f / s; 60 quaternion.q1 = ( m[2][1] - m[1][2] ) * s; 61 quaternion.q2 = ( m[0][2] - m[2][0] ) * s; 62 quaternion.q3 = ( m[1][0] - m[0][1] ) * s; 63 } else { 64 if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) { 65 float s = 2.0f * sqrtf( 1.0f + m[0][0] - m[1][1] - m[2][2]); 66 quaternion.q0 = (m[2][1] - m[1][2] ) / s; 67 quaternion.q1 = 0.25f * s; 68 quaternion.q2 = (m[0][1] + m[1][0] ) / s; 69 quaternion.q3 = (m[0][2] + m[2][0] ) / s; 70 } else if (m[1][1] > m[2][2]) { 71 float s = 2.0f * sqrtf( 1.0f + m[1][1] - m[0][0] - m[2][2]); 72 quaternion.q0 = (m[0][2] - m[2][0] ) / s; 73 quaternion.q1 = (m[0][1] + m[1][0] ) / s; 74 quaternion.q2 = 0.25f * s; 75 quaternion.q3 = (m[1][2] + m[2][1] ) / s; 76 } else { 77 float s = 2.0f * sqrtf( 1.0f + m[2][2] - m[0][0] - m[1][1] ); 78 quaternion.q0 = (m[1][0] - m[0][1] ) / s; 79 quaternion.q1 = (m[0][2] + m[2][0] ) / s; 80 quaternion.q2 = (m[1][2] + m[2][1] ) / s; 81 quaternion.q3 = 0.25f * s; 82 } 57 float test1 = m[0][0] + m[1][1] + m[2][2]; 58 float test2 = m[0][0] - m[1][1] - m[2][2]; 59 float test3 = -m[0][0] + m[1][1] - m[2][2]; 60 float test4 = -m[0][0] - m[1][1] + m[2][2]; 61 62 if(test1==max(max(test1,test2),max(test3,test4))) { 63 float s=2*sqrtf(1+test1); 64 quaternion.q0 = s/4.; 65 quaternion.q1 = ( m[1][2] - m[2][1] ) / s; 66 quaternion.q2 = ( m[2][0] - m[0][2] ) / s; 67 quaternion.q3 = ( m[0][1] - m[1][0] ) / s; 68 } 69 70 if(test2==max(max(test1,test2),max(test3,test4))) { 71 float s=2*sqrtf(1+test2); 72 quaternion.q0 = ( m[2][1] - m[1][2] ) / s; 73 quaternion.q1 = -s/4.; 74 quaternion.q2 = -( m[1][0] + m[0][1] ) / s; 75 quaternion.q3 = -( m[0][2] + m[2][0] ) / s; 76 } 77 78 if(test3==max(max(test1,test2),max(test3,test4))) { 79 float s=2*sqrtf(1+test3); 80 quaternion.q0 = (m[0][2] - m[2][0] ) / s; 81 quaternion.q1 = -( m[1][0] + m[0][1] ) / s; 82 quaternion.q2 = -s/4.; 83 quaternion.q3 = -( m[2][1] + m[1][2] ) / s; 84 } 85 86 if(test4==max(max(test1,test2),max(test3,test4))) { 87 float s=2*sqrtf(1+test4); 88 quaternion.q0 = (m[1][0] - m[0][1] ) / s; 89 quaternion.q1 = -( m[0][2] + m[2][0] ) / s; 90 quaternion.q2 = -( m[2][1] + m[1][2] ) / s; 91 quaternion.q3 = -s/4.; 83 92 } 84 93 quaternion.Normalize(); 85 94 } 86 /* 87 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const { 88 quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]); 89 quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]); 90 quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]); 91 quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]); 92 //Printf("%f %f %f\n", m[0][0] , m[1][1] , m[2][2]); 93 //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]); 94 } 95 */ 95 96 96 Quaternion RotationMatrix::ToQuaternion(void) const { 97 97 Quaternion quaternion; -
trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp
r102 r103 35 35 36 36 if (command == EulerAnglesAndAngularRates) { 37 37 Printf("Gx3_25_imu enabling EulerAnglesAndAngularRates\n"); 38 38 } else if (command == AccelerationAngularRateAndOrientationMatrix) { 39 //Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");39 Printf("Gx3_25_imu enabling AccelerationAngularRateAndOrientationMatrix\n"); 40 40 } else if (command == Quaternion) { 41 41 Printf("Gx3_25_imu enabling Quaternion\n"); 42 42 } else { 43 43 Thread::Err("command not supported (%i)\n", command); -
trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
r102 r103 118 118 SetContinuousMode(command); 119 119 120 //_printf("firmware version: %i\n",get_firmware_number());121 122 120 while (!self->ToBeStopped()) { 123 121 if (command == Gx3_25_imu::EulerAnglesAndAngularRates) { … … 352 350 command[5] = rate & 0xff; // data rate LSB 353 351 result = 0; 354 if (disable_magn->IsChecked() == true) 355 result |= 0x01; 356 if (disable_north_comp->IsChecked() == true) 357 result |= 0x04; 358 if (disable_grav_comp->IsChecked() == true) 359 result |= 0x08; 360 352 if (disable_magn->IsChecked() == true) result |= 0x01; 353 if (disable_north_comp->IsChecked() == true) result |= 0x04; 354 if (disable_grav_comp->IsChecked() == true) result |= 0x08; 355 if (this->command == Gx3_25_imu::Quaternion) result |= 0x10;//enable quaternion 361 356 command[6] = result; 357 362 358 result = 0x01; // Calculate orientation 363 359 if (coning->IsChecked() == true)
Note:
See TracChangeset
for help on using the changeset viewer.