Changeset 101 in flair-src for trunk/lib/FlairMeta/src

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

maj quaternion 3dmgx3

Location:

trunk/lib/FlairMeta/src

Files:

• HdsX8.cpp (modified) (2 diffs)
• UavStateMachine.cpp (modified) (2 diffs)

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;
   }
 }