Index: trunk/lib/FlairCore/src/RotationMatrix.cpp
===================================================================
--- trunk/lib/FlairCore/src/RotationMatrix.cpp	(revision 100)
+++ trunk/lib/FlairCore/src/RotationMatrix.cpp	(revision 101)
@@ -51,4 +51,38 @@
 }
 
+//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,6 +90,8 @@
 		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;
Index: trunk/lib/FlairMeta/src/HdsX8.cpp
===================================================================
--- trunk/lib/FlairMeta/src/HdsX8.cpp	(revision 100)
+++ trunk/lib/FlairMeta/src/HdsX8.cpp	(revision 101)
@@ -62,5 +62,5 @@
           }
   */
-  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
+  //SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
 }
 
@@ -70,5 +70,5 @@
   ((Gx3_25_ahrs *)GetAhrs())->Start();
   ((Srf08 *)GetUsRangeFinder())->Start();
-  ((Ps3Eye *)GetVerticalCamera())->Start();
+  //((Ps3Eye *)GetVerticalCamera())->Start();
   Uav::StartSensors();
 }
Index: trunk/lib/FlairMeta/src/UavStateMachine.cpp
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 100)
+++ trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 101)
@@ -182,20 +182,25 @@
 
     //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,10 +226,10 @@
   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;
   }
 }
Index: trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp	(revision 100)
+++ trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp	(revision 101)
@@ -37,5 +37,5 @@
 
   } 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);
