Changeset 101 in flair-src


Ignore:
Timestamp:
Oct 5, 2016, 5:20:41 PM (4 years ago)
Author:
Sanahuja Guillaume
Message:

maj quaternion 3dmgx3

Location:
trunk
Files:
5 edited

Legend:

Unmodified
Added
Removed
  • trunk/demos/Gps/uav/src/DemoGps.cpp

    r89 r101  
    4949    gps->UseDefaultPlot();
    5050    getFrameworkManager()->AddDeviceToLog(gps);
    51     gps->Start();
     51    //gps->Start();
    5252
    5353    circle=new TrajectoryGenerator2DCircle(gps->GetLayout()->NewRow(),"circle");
  • trunk/lib/FlairCore/src/RotationMatrix.cpp

    r100 r101  
    5151}
    5252
     53//from
     54//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
     55void 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    }
     83  }
     84  quaternion.Normalize();
     85}
     86/*
    5387void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
    5488                quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
     
    5690                quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
    5791                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]);
    5894}
    59 
     95*/
    6096Quaternion RotationMatrix::ToQuaternion(void) const {
    6197  Quaternion quaternion;
  • trunk/lib/FlairMeta/src/HdsX8.cpp

    r100 r101  
    6262          }
    6363  */
    64   SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
     64  //SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
    6565}
    6666
     
    7070  ((Gx3_25_ahrs *)GetAhrs())->Start();
    7171  ((Srf08 *)GetUsRangeFinder())->Start();
    72   ((Ps3Eye *)GetVerticalCamera())->Start();
     72  //((Ps3Eye *)GetVerticalCamera())->Start();
    7373  Uav::StartSensors();
    7474}
  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r45 r101  
    182182
    183183    //check nan/inf problems
    184     if(IsValuePossible(currentTorques.roll,"roll torque")
    185        || IsValuePossible(currentTorques.pitch,"pitch torque")
    186        || IsValuePossible(currentTorques.yaw,"yaw torque")
    187        || IsValuePossible(currentThrust,"thrust")) {
    188 
    189       if(failSafeMode) {
    190         Warn("We are already in safe mode, the uav is going to crash!\n");
    191       } else {
    192         Thread::Warn("switching back to safe mode\n");
    193         EnterFailSafeMode();
    194         needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
    195         needToComputeDefaultThrust = true;
    196 
    197         ComputeTorques();
    198         ComputeThrust();
    199       }
     184    if(!IsValuePossible(currentTorques.roll,"roll torque")
     185       || !IsValuePossible(currentTorques.pitch,"pitch torque")
     186       || !IsValuePossible(currentTorques.yaw,"yaw torque")
     187       || !IsValuePossible(currentThrust,"thrust")) {
     188
     189        if(altitudeState==AltitudeState_t::Stopped) {
     190            SafeStop();
     191        } else {
     192
     193          if(failSafeMode) {
     194            Warn("We are already in safe mode, the uav is going to crash!\n");
     195          } else {
     196            Thread::Warn("switching back to safe mode\n");
     197            EnterFailSafeMode();
     198            needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
     199            needToComputeDefaultThrust = true;
     200
     201            ComputeTorques();
     202            ComputeThrust();
     203          }
     204        }
    200205    }
    201206
     
    221226  if(isnan(value)) {
    222227    Warn("%s is not an number\n",desc.c_str());
    223     return true;
     228    return false;
    224229  } else if(isinf(value)) {
    225230    Warn("%s is infinite\n",desc.c_str());
     231    return false;
     232  } else {
    226233    return true;
    227   } else {
    228     return false;
    229234  }
    230235}
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp

    r15 r101  
    3737
    3838  } else if (command == AccelerationAngularRateAndOrientationMatrix) {
    39     Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
     39    //Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
    4040  } else {
    4141    Thread::Err("command not supported (%i)\n", command);
Note: See TracChangeset for help on using the changeset viewer.