Changeset 101 in flair-src
- Timestamp:
- Oct 5, 2016, 5:20:41 PM (8 years ago)
- Location:
- trunk
- Files:
-
- 5 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/demos/Gps/uav/src/DemoGps.cpp
r89 r101 49 49 gps->UseDefaultPlot(); 50 50 getFrameworkManager()->AddDeviceToLog(gps); 51 gps->Start();51 //gps->Start(); 52 52 53 53 circle=new TrajectoryGenerator2DCircle(gps->GetLayout()->NewRow(),"circle"); -
trunk/lib/FlairCore/src/RotationMatrix.cpp
r100 r101 51 51 } 52 52 53 //from 54 //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ 55 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 } 83 } 84 quaternion.Normalize(); 85 } 86 /* 53 87 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const { 54 88 quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]); … … 56 90 quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]); 57 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]); 58 94 } 59 95 */ 60 96 Quaternion RotationMatrix::ToQuaternion(void) const { 61 97 Quaternion quaternion; -
trunk/lib/FlairMeta/src/HdsX8.cpp
r100 r101 62 62 } 63 63 */ 64 SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));64 //SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50)); 65 65 } 66 66 … … 70 70 ((Gx3_25_ahrs *)GetAhrs())->Start(); 71 71 ((Srf08 *)GetUsRangeFinder())->Start(); 72 ((Ps3Eye *)GetVerticalCamera())->Start();72 //((Ps3Eye *)GetVerticalCamera())->Start(); 73 73 Uav::StartSensors(); 74 74 } -
trunk/lib/FlairMeta/src/UavStateMachine.cpp
r45 r101 182 182 183 183 //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 } 200 205 } 201 206 … … 221 226 if(isnan(value)) { 222 227 Warn("%s is not an number\n",desc.c_str()); 223 return true;228 return false; 224 229 } else if(isinf(value)) { 225 230 Warn("%s is infinite\n",desc.c_str()); 231 return false; 232 } else { 226 233 return true; 227 } else {228 return false;229 234 } 230 235 } -
trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp
r15 r101 37 37 38 38 } 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"); 40 40 } else { 41 41 Thread::Err("command not supported (%i)\n", command);
Note:
See TracChangeset
for help on using the changeset viewer.