Index: /trunk/demos/CircleFollower/simulator/CMakeLists.txt
===================================================================
--- /trunk/demos/CircleFollower/simulator/CMakeLists.txt	(revision 41)
+++ /trunk/demos/CircleFollower/simulator/CMakeLists.txt	(revision 42)
@@ -11,8 +11,4 @@
 SET(SRC_FILES
 	${CMAKE_SOURCE_DIR}/src/main.cpp
-)
-
-LINK_DIRECTORIES (
-	${FLAIR_LIBRARY_DIR}
 )
 
Index: /trunk/demos/CircleFollower/uav/CMakeLists.txt
===================================================================
--- /trunk/demos/CircleFollower/uav/CMakeLists.txt	(revision 41)
+++ /trunk/demos/CircleFollower/uav/CMakeLists.txt	(revision 42)
@@ -14,8 +14,4 @@
 	${CMAKE_SOURCE_DIR}/src/main.cpp
 	${CMAKE_SOURCE_DIR}/src/CircleFollower.cpp
-)
-
-LINK_DIRECTORIES (
-	${FLAIR_LIBRARY_DIR}
 )
 
Index: /trunk/demos/SimpleFleet/simulator/CMakeLists.txt
===================================================================
--- /trunk/demos/SimpleFleet/simulator/CMakeLists.txt	(revision 41)
+++ /trunk/demos/SimpleFleet/simulator/CMakeLists.txt	(revision 42)
@@ -11,8 +11,4 @@
 SET(SRC_FILES
 	${CMAKE_SOURCE_DIR}/src/main.cpp
-)
-
-LINK_DIRECTORIES (
-	${FLAIR_LIBRARY_DIR}
 )
 
Index: /trunk/demos/SimpleFleet/uav/CMakeLists.txt
===================================================================
--- /trunk/demos/SimpleFleet/uav/CMakeLists.txt	(revision 41)
+++ /trunk/demos/SimpleFleet/uav/CMakeLists.txt	(revision 42)
@@ -14,8 +14,4 @@
 	${CMAKE_SOURCE_DIR}/src/main.cpp
 	${CMAKE_SOURCE_DIR}/src/SimpleFleet.cpp
-)
-
-LINK_DIRECTORIES (
-	${FLAIR_LIBRARY_DIR}
 )
 
Index: /trunk/lib/FlairCore/src/GpsData.cpp
===================================================================
--- /trunk/lib/FlairCore/src/GpsData.cpp	(revision 42)
+++ /trunk/lib/FlairCore/src/GpsData.cpp	(revision 42)
@@ -0,0 +1,97 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2016/07/01
+//  filename:   GpsData.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    class defining gps datas
+//
+//
+/*********************************************************************/
+
+#include "GpsData.h"
+#include "Euler.h"
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+
+using std::string;
+
+namespace flair {
+namespace core {
+
+/*! \class GpsDataElement
+    */
+class GpsDataElement : public IODataElement {
+public:
+  GpsDataElement(const GpsData *inGpsdata, string name,
+                 GpsData::PlotableData_t inPlotableData)
+      : IODataElement(inGpsdata, name) {
+    gpsdata = inGpsdata;
+    plotableData = inPlotableData;
+
+    size = 0;
+  }
+
+  ~GpsDataElement() {}
+
+  void CopyData(char *dst) const {
+
+
+  }
+
+  FloatType const &GetDataType(void) const { return dataType; }
+
+private:
+  const GpsData *gpsdata;
+  GpsData::PlotableData_t plotableData;
+  FloatType dataType;
+};
+
+GpsData::GpsData(const Object *parent, std::string name, int n)
+    : io_data(parent, name, n), dataType(floatType) {
+  if (n > 1)
+    Warn("n>1 not supported\n");
+
+  AppendLogDescription("raw_ax", floatType);
+  AppendLogDescription("raw_ay", floatType);
+  AppendLogDescription("raw_az", floatType);
+
+  AppendLogDescription("raw_gx", floatType);
+  AppendLogDescription("raw_gy", floatType);
+  AppendLogDescription("raw_gz", floatType);
+
+  AppendLogDescription("raw_mx", floatType);
+  AppendLogDescription("raw_my", floatType);
+  AppendLogDescription("raw_mz", floatType);
+}
+
+GpsData::~GpsData() {}
+
+
+IODataElement *GpsData::Element(PlotableData_t data_type) const {
+
+  //return new GpsDataElement(this, name, data_type);
+}
+
+void GpsData::CopyDatas(char *dst) const {
+  GetMutex();
+
+  //Queue(&dst, &rawAcc.x, sizeof(rawAcc.x));
+
+  ReleaseMutex();
+}
+
+void GpsData::Queue(char **dst, const void *src, size_t size) const {
+  memcpy(*dst, src, size);
+  *dst += size;
+}
+
+} // end namespace core
+} // end namespace flair
Index: /trunk/lib/FlairCore/src/GpsData.h
===================================================================
--- /trunk/lib/FlairCore/src/GpsData.h	(revision 42)
+++ /trunk/lib/FlairCore/src/GpsData.h	(revision 42)
@@ -0,0 +1,105 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file GpsData.h
+ * \brief Class defining gps datas
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2016/07/01
+ * \version 4.0
+ */
+#ifndef GPSDATA_H
+#define GPSDATA_H
+
+#include <io_data.h>
+#include <IODataElement.h>
+#include <Vector3D.h>
+
+namespace flair {
+namespace core {
+
+/*! \class GpsData
+*
+* \brief Class defining gps datas
+*
+* GPS datas consist of
+*
+*
+*/
+class GpsData : public io_data {
+public:
+  class Type : public DataType {
+  public:
+    Type(ScalarType const &_elementDataType)
+        : elementDataType(_elementDataType) {}
+    ScalarType const &GetElementDataType() const { return elementDataType; }
+    std::string GetDescription() const { return "gps data"; }
+    size_t GetSize() const {
+      return 0;
+    }
+
+  private:
+    ScalarType const &elementDataType;
+  };
+
+  /*!
+  \enum PlotableData_t
+  \brief Datas wich can be plotted in a DataPlot1D
+  */
+  typedef enum {
+
+  } PlotableData_t;
+
+  /*!
+  * \brief Constructor
+  *
+  * Construct an io_data representing GPS datas. \n
+  *
+  * \param parent parent
+  * \param name name
+  * \param n number of samples
+  */
+  GpsData(const Object *parent, std::string name = "", int n = 1);
+
+  /*!
+  * \brief Destructor
+  *
+  */
+  ~GpsData();
+
+  /*!
+  * \brief Element
+  *
+  * Get a pointer to a specific element. This pointer can be used for plotting.
+  *
+  * \param data_type data type
+  *
+  * \return pointer to the element
+  */
+  IODataElement *Element(PlotableData_t data_type) const;
+
+
+  Type const &GetDataType() const { return dataType; }
+
+private:
+  /*!
+  * \brief Copy datas
+  *
+  * Reimplemented from io_data. \n
+  * See io_data::CopyDatas.
+  *
+  * \param dst destination buffer
+  */
+  void CopyDatas(char *dst) const;
+
+
+
+  void Queue(char **dst, const void *src, size_t size) const;
+  Type dataType;
+};
+
+} // end namespace core
+} // end namespace flair
+
+#endif // GPSDATA_H
Index: /trunk/lib/FlairMeta/src/UavStateMachine.cpp
===================================================================
--- /trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 41)
+++ /trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 42)
@@ -42,4 +42,5 @@
 #include <stdio.h>
 #include <TrajectoryGenerator1D.h>
+#include <math.h>
 
 using namespace std;
@@ -180,4 +181,23 @@
     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,4 +218,14 @@
 }
 
+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,5 +284,5 @@
       // 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,5 +577,5 @@
         EnterFailSafeMode();
         Land();
-    }
+    }/*
     Time now=GetTime();
     if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
@@ -553,6 +583,6 @@
         Thread::Err("Critical sensor lost\n");
         EnterFailSafeMode();
-        EmergencyLand(); 
-    }
+        EmergencyLand();
+    }*/
 }
 
Index: /trunk/lib/FlairMeta/src/UavStateMachine.h
===================================================================
--- /trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 41)
+++ /trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 42)
@@ -261,4 +261,5 @@
   void Run(void);
   void StopMotors(void);
+  bool CheckIsNan(float value,std::string desc);
 
   meta::Uav *uav;
Index: /trunk/lib/FlairSensorActuator/src/Gps.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Gps.cpp	(revision 41)
+++ /trunk/lib/FlairSensorActuator/src/Gps.cpp	(revision 42)
@@ -216,6 +216,5 @@
   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);
   }
 
Index: /trunk/lib/FlairSensorActuator/src/Mb800.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Mb800.cpp	(revision 41)
+++ /trunk/lib/FlairSensorActuator/src/Mb800.cpp	(revision 42)
@@ -53,5 +53,5 @@
       Thread::Err("erreur Write (%s)\n", strerror(-written));
   }
-/*
+
   {
     char to_send[] = "$PASHS,CPD,AFP,95.0\r\n";
@@ -68,5 +68,5 @@
     }
   }
-*/
+
   if ((NMEAFlags & GGA) != 0) {
     char to_send[] = "$PASHS,NME,GGA,A,ON,0.05\r\n";
@@ -99,7 +99,6 @@
 
   while (!ToBeStopped()) {
-    //Printf("loop\n");
     SleepMS(10);
-    size = 0;/*
+    size = 0;
     while (!ToBeStopped()) {
       ssize_t read = serialport->Read(&response[size], 1);
@@ -111,7 +110,7 @@
         break;
       size++;
-    }*/
+    }
     size++;
-    //parseFrame(response, size);
+    parseFrame(response, size);
   }
   /** fin running loop **/
@@ -122,10 +121,10 @@
   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));
Index: /trunk/lib/FlairSensorActuator/src/TargetController.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 41)
+++ /trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 42)
@@ -39,5 +39,4 @@
     TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
     setup_tab=new Tab(tab,"Reglages");
-    SetPeriodMS(20);; //50Hz
 }
 
@@ -115,4 +114,6 @@
 
 void TargetController::Run() {
+  SetPeriodMS(20);; //50Hz
+
   Message *message;
 
