Changeset 42 in flair-src for trunk/lib

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

m

Location:

trunk/lib

Files:

• FlairCore/src/GpsData.cpp (added)
• FlairCore/src/GpsData.h (added)
• FlairMeta/src/UavStateMachine.cpp (modified) (6 diffs)
• FlairMeta/src/UavStateMachine.h (modified) (1 diff)
• FlairSensorActuator/src/Gps.cpp (modified) (1 diff)
• FlairSensorActuator/src/Mb800.cpp (modified) (5 diffs)
• FlairSensorActuator/src/TargetController.cpp (modified) (2 diffs)

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;

trunk/lib/FlairSensorActuator/src/Gps.cpp

@@ -216 +216 @@
   result = nmea_parse(&parser, frame, frame_size, &info);
   if (result != 1) {
-    Warn("unrecognized nmea sentence\n");
-    Warn("%s\n", frame);
+    Warn("unrecognized nmea sentence: %s\n",frame);
   }
 

trunk/lib/FlairSensorActuator/src/Mb800.cpp

@@ -53 +53 @@
       Thread::Err("erreur Write (%s)\n", strerror(-written));
   }
-/*
+
   {
     char to_send[] = "$PASHS,CPD,AFP,95.0\r\n";
@@ -68 +68 @@
     }
   }
-*/
+
   if ((NMEAFlags & GGA) != 0) {
     char to_send[] = "$PASHS,NME,GGA,A,ON,0.05\r\n";
@@ -99 +99 @@
 
   while (!ToBeStopped()) {
-    //Printf("loop\n");
     SleepMS(10);
-    size = 0;/*
+    size = 0;
     while (!ToBeStopped()) {
       ssize_t read = serialport->Read(&response[size], 1);
@@ -111 +110 @@
         break;
       size++;
-    }*/
+    }
     size++;
-    //parseFrame(response, size);
+    parseFrame(response, size);
   }
   /** fin running loop **/
@@ -122 +121 @@
   char data = 0;
   ssize_t read = 0;
-Printf("sync\n");
+
   // attente fin trame
   while (data != 0x0a && !ToBeStopped()) {
     read = serialport->Read(&data, 1);
     SleepMS(10);
-    Printf("%i %x\n",read,data);
+
     if (read < 0) {
       Thread::Err("erreur Read (%s)\n", strerror(-read));

trunk/lib/FlairSensorActuator/src/TargetController.cpp

@@ -39 +39 @@
     TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
     setup_tab=new Tab(tab,"Reglages");
-    SetPeriodMS(20);; //50Hz
 }
 
@@ -115 +114 @@
 
 void TargetController::Run() {
+  SetPeriodMS(20);; //50Hz
+
   Message *message;