Changeset 101 in flair-src for trunk/lib

Timestamp:

Oct 5, 2016, 5:20:41 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

maj quaternion 3dmgx3

Location:

trunk/lib

Files:

• FlairCore/src/RotationMatrix.cpp (modified) (2 diffs)
• FlairMeta/src/HdsX8.cpp (modified) (2 diffs)
• FlairMeta/src/UavStateMachine.cpp (modified) (2 diffs)
• FlairSensorActuator/src/Gx3_25_imu.cpp (modified) (1 diff)

trunk/lib/FlairCore/src/RotationMatrix.cpp

@@ -51 +51 @@
 }
 
+//from
+//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+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;
+    }
+  }
+  quaternion.Normalize();
+}
+/*
 void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
                 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]);
                 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/FlairMeta/src/HdsX8.cpp

@@ -62 +62 @@
           }
   */
-  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
+  //SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
 }
 
@@ -70 +70 @@
   ((Gx3_25_ahrs *)GetAhrs())->Start();
   ((Srf08 *)GetUsRangeFinder())->Start();
-  ((Ps3Eye *)GetVerticalCamera())->Start();
+  //((Ps3Eye *)GetVerticalCamera())->Start();
   Uav::StartSensors();
 }

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -182 +182 @@
 
     //check nan/inf problems
-    if(IsValuePossible(currentTorques.roll,"roll torque")
-       || IsValuePossible(currentTorques.pitch,"pitch torque")
-       || IsValuePossible(currentTorques.yaw,"yaw torque")
-       || IsValuePossible(currentThrust,"thrust")) {
-
-      if(failSafeMode) {
-        Warn("We are already in safe mode, the uav is going to crash!\n");
-      } else {
-        Thread::Warn("switching back to safe mode\n");
-        EnterFailSafeMode();
-        needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
-        needToComputeDefaultThrust = true;
-
-        ComputeTorques();
-        ComputeThrust();
-      }
+    if(!IsValuePossible(currentTorques.roll,"roll torque")
+       || !IsValuePossible(currentTorques.pitch,"pitch torque")
+       || !IsValuePossible(currentTorques.yaw,"yaw torque")
+       || !IsValuePossible(currentThrust,"thrust")) {
+
+        if(altitudeState==AltitudeState_t::Stopped) {
+            SafeStop();
+        } else {
+
+          if(failSafeMode) {
+            Warn("We are already in safe mode, the uav is going to crash!\n");
+          } else {
+            Thread::Warn("switching back to safe mode\n");
+            EnterFailSafeMode();
+            needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
+            needToComputeDefaultThrust = true;
+
+            ComputeTorques();
+            ComputeThrust();
+          }
+        }
     }
 
@@ -221 +226 @@
   if(isnan(value)) {
     Warn("%s is not an number\n",desc.c_str());
-    return true;
+    return false;
   } else if(isinf(value)) {
     Warn("%s is infinite\n",desc.c_str());
+    return false;
+  } else {
     return true;
-  } else {
-    return false;
   }
 }

trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp

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