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

Timestamp:

Jul 5, 2016, 8:50:58 AM (8 years ago)

Author:

Sanahuja Guillaume

Message:

m

Location:

trunk/lib/FlairMeta/src

Files:

• UavStateMachine.cpp (modified) (6 diffs)
• UavStateMachine.h (modified) (1 diff)

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -42 +42 @@
 #include <stdio.h>
 #include <TrajectoryGenerator1D.h>
+#include <math.h>
 
 using namespace std;
@@ -180 +181 @@
     ComputeThrust(); // logs are added to uz, so it must be updated at last
 
+    //check nan problems
+    if(CheckIsNan(currentTorques.roll,"roll torque")
+       || CheckIsNan(currentTorques.pitch,"pitch torque")
+       || CheckIsNan(currentTorques.yaw,"yaw torque")
+       || CheckIsNan(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();
+      }
+    }
+
     // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
     // thrust (value between 0 and 1)
@@ -198 +218 @@
 }
 
+bool UavStateMachine::CheckIsNan(float value,std::string desc) {
+  if(isnan(value)) {
+    Warn("%s is not an number\n",desc.c_str());
+    return true;
+  } else {
+    return false;
+  }
+}
+
+
 void UavStateMachine::ComputeOrientation(void) {
   if (failSafeMode) {
@@ -254 +284 @@
       // The Uav target altitude has reached its landing value (typically 0)
       // but the real Uav altitude may not have reach this value yet because of
-      // command delay. Moreover, it may never exactly reach this value if the 
+      // command delay. Moreover, it may never exactly reach this value if the
       // ground is not perfectly leveled (critical case: there's a
       // deep and narrow hole right in the sensor line of sight). That's why we
@@ -547 +577 @@
         EnterFailSafeMode();
         Land();
-    }
+    }/*
     Time now=GetTime();
     if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
@@ -553 +583 @@
         Thread::Err("Critical sensor lost\n");
         EnterFailSafeMode();
-        EmergencyLand(); 
-    }
+        EmergencyLand();
+    }*/
 }
 

trunk/lib/FlairMeta/src/UavStateMachine.h

@@ -261 +261 @@
   void Run(void);
   void StopMotors(void);
+  bool CheckIsNan(float value,std::string desc);
 
   meta::Uav *uav;