Changeset 103 in flair-src


Ignore:
Timestamp:
Oct 11, 2016, 12:07:16 PM (4 years ago)
Author:
Sanahuja Guillaume
Message:

maj quaternion 3dmgx3

Location:
trunk/lib
Files:
4 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairCore/src/OneAxisRotation_impl.cpp

    r15 r103  
    4949
    5050void OneAxisRotation_impl::ComputeRotation(RotationMatrix &matrix) const {
    51   printf("not yet implemented\n");
     51  Printf("not yet implemented\n");
    5252}
    5353
  • trunk/lib/FlairCore/src/RotationMatrix.cpp

    r101 r103  
    2121#include "Quaternion.h"
    2222#include "math.h"
     23
     24using std::max;
    2325
    2426namespace flair {
     
    5153}
    5254
    53 //from
    54 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
     55//from 3dmgx3-25 manual
    5556void 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.;
    8392  }
    8493  quaternion.Normalize();
    8594}
    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
    9696Quaternion RotationMatrix::ToQuaternion(void) const {
    9797  Quaternion quaternion;
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp

    r102 r103  
    3535
    3636  if (command == EulerAnglesAndAngularRates) {
    37 
     37    Printf("Gx3_25_imu enabling EulerAnglesAndAngularRates\n");
    3838  } 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");
    4040  } else if (command == Quaternion) {
    41 
     41    Printf("Gx3_25_imu enabling Quaternion\n");
    4242  } else {
    4343    Thread::Err("command not supported (%i)\n", command);
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

    r102 r103  
    118118  SetContinuousMode(command);
    119119
    120   //_printf("firmware version: %i\n",get_firmware_number());
    121 
    122120  while (!self->ToBeStopped()) {
    123121    if (command == Gx3_25_imu::EulerAnglesAndAngularRates) {
     
    352350  command[5] = rate & 0xff;        // data rate LSB
    353351  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
    361356  command[6] = result;
     357
    362358  result = 0x01; // Calculate orientation
    363359  if (coning->IsChecked() == true)
Note: See TracChangeset for help on using the changeset viewer.