Index: trunk/lib/FlairMeta/src/MetaDualShock3.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaDualShock3.cpp	(revision 22)
+++ trunk/lib/FlairMeta/src/MetaDualShock3.cpp	(revision 38)
@@ -18,4 +18,5 @@
 #include "MetaDualShock3.h"
 #include "MetaDualShock3_impl.h"
+#include "TargetController.h"
 #include "JoyReference.h"
 #include <Tab.h>
@@ -30,4 +31,5 @@
 using namespace flair::filter;
 using namespace flair::gui;
+using namespace flair::sensor;
 
 namespace flair {
@@ -35,9 +37,9 @@
 
 MetaDualShock3::MetaDualShock3(FrameworkManager *parent, string name,
-                               uint16_t port, uint8_t priority)
-    : TargetEthController(parent, name, port, priority) {
+                               TargetController *controller)
+    : controller(controller),IODevice((IODevice*)controller, name) {
   pimpl_ = new MetaDualShock3_impl(this, name);
   parent->AddDeviceToLog(pimpl_->joy_ref);
-  Start();
+  controller->Start();
 }
 
@@ -49,25 +51,22 @@
 
 void MetaDualShock3::ErrorNotify(void) {
-  TargetEthController::FlashLed(4, 10, 0);
-  TargetEthController::Rumble(0xff, 20, 0, 0);
+    controller->FlashLed(4,10,0);
+    controller->Rumble(0xff,20,0,0);
 }
 
-void MetaDualShock3::Rumble(uint8_t left_force, uint8_t left_timeout,
-                            uint8_t right_force, uint8_t right_timeout) {
-  TargetEthController::Rumble(left_force, left_timeout, right_force,
-                              right_timeout);
+void MetaDualShock3::Rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout) {
+    controller->Rumble(left_force,left_timeout,right_force,right_timeout);
 }
 
 void MetaDualShock3::SetLedON(unsigned int ledId) {
-  TargetEthController::SetLedOn(ledId);
+    controller->SetLedOn(ledId);
 }
 
 void MetaDualShock3::SetLedOFF(unsigned int ledId) {
-  TargetEthController::SetLedOff(ledId);
+    controller->SetLedOff(ledId);
 }
 
-void MetaDualShock3::FlashLed(unsigned int ledId, uint8_t on_timeout,
-                              uint8_t off_timeout) {
-  TargetEthController::FlashLed(ledId, on_timeout, off_timeout);
+void MetaDualShock3::FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout) {
+    controller->FlashLed(ledId,on_timeout,off_timeout);
 }
 
@@ -91,5 +90,9 @@
 
 float MetaDualShock3::PitchTrim(void) const {
-  return pimpl_->joy_ref->PitchTrim();
+    return pimpl_->joy_ref->PitchTrim();
+}
+
+void MetaDualShock3::UpdateFrom(const flair::core::io_data *data) {
+    pimpl_->UpdateFrom(data);
 }
 
Index: trunk/lib/FlairMeta/src/MetaDualShock3.h
===================================================================
--- trunk/lib/FlairMeta/src/MetaDualShock3.h	(revision 22)
+++ trunk/lib/FlairMeta/src/MetaDualShock3.h	(revision 38)
@@ -14,5 +14,6 @@
 #define METADUALSHOCK3_H
 
-#include <TargetEthController.h>
+#include <TargetController.h>
+#include <IODevice.h>
 
 namespace flair {
@@ -35,10 +36,10 @@
 * \brief Classe intégrant la manette MetaDualShock3
 */
-class MetaDualShock3 : public sensor::TargetEthController {
+class MetaDualShock3 : public core::IODevice {
   friend class ::MetaDualShock3_impl;
 
 public:
   MetaDualShock3(core::FrameworkManager *parent, std::string name,
-                 uint16_t port, uint8_t priority);
+                 sensor::TargetController *controller);
   ~MetaDualShock3();
   core::AhrsData *GetReferenceOrientation(void) const;
@@ -66,7 +67,9 @@
   void FlashLed(unsigned int ledId, uint8_t on_timeout, uint8_t off_timeout);
 
-private:
-  class MetaDualShock3_impl *pimpl_;
-};
+        private:
+            class MetaDualShock3_impl* pimpl_;
+            void UpdateFrom(const flair::core::io_data *data);
+            sensor::TargetController *controller;
+    };
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp	(revision 22)
+++ trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp	(revision 38)
@@ -29,7 +29,6 @@
 using namespace flair::meta;
 
-MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name)
-    : IODevice((IODevice *)self, name) {
-  joy_ref = new JoyReference(self->GetTab()->NewRow(), "consignes joy");
+MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name) {
+  joy_ref=new JoyReference(self->controller->GetTab()->NewRow(),"consignes joy");
   this->self = self;
   joy_init = false;
@@ -50,5 +49,5 @@
 
   // up
-  if (self->IsButtonPressed(12)) {
+  if (self->controller->IsButtonPressed(12)) {
     if (!wasPitchTrimDownButtonPressed) {
       joy_ref->PitchTrimDown();
@@ -60,5 +59,5 @@
 
   // down
-  if (self->IsButtonPressed(13)) {
+  if (self->controller->IsButtonPressed(13)) {
     if (!wasPitchTrimUpButtonPressed) {
       joy_ref->PitchTrimUp();
@@ -70,5 +69,5 @@
 
   // right
-  if (self->IsButtonPressed(15)) {
+  if (self->controller->IsButtonPressed(15)) {
     if (!wasRollTrimUpButtonPressed) {
       joy_ref->RollTrimUp();
@@ -80,5 +79,5 @@
 
   // left
-  if (self->IsButtonPressed(14)) {
+  if (self->controller->IsButtonPressed(14)) {
     if (!wasRollTrimDownButtonPressed) {
       joy_ref->RollTrimDown();
@@ -107,5 +106,5 @@
 
   if (!joy_init) {
-    self->TargetEthController::FlashLed(1, 10, 10);
+    self->controller->FlashLed(1, 10, 10);
     joy_init = true;
   }
Index: trunk/lib/FlairMeta/src/UavStateMachine.cpp
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 22)
+++ trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 38)
@@ -51,10 +51,9 @@
 using namespace flair::meta;
 
-UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
-    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
-      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
-      flagZTrajectoryFinished(false) {
-  altitudeState = AltitudeState_t::Stopped;
-  uav->UseDefaultPlot();
+UavStateMachine::UavStateMachine(Uav* inUav,TargetController *controller):
+        Thread(getFrameworkManager(),"UavStateMachine",50),
+        uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
+    altitudeState=AltitudeState_t::Stopped;
+    uav->UseDefaultPlot();
 
   Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
@@ -90,6 +89,6 @@
   uZ->AddDeviceToLog(uYaw);
 
-  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
-  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
+    joy=new MetaDualShock3(getFrameworkManager(),"uav high level controller",controller);
+    uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
 
   altitudeMode = AltitudeMode_t::Manual;
@@ -124,5 +123,7 @@
 }
 
-const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
+const TargetController *UavStateMachine::GetJoystick(void) const {
+    return controller;
+}
 
 const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
@@ -253,7 +254,6 @@
       // 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 ground is not perfectly
-      // leveled (critical case: there's a
+      // 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
       // have a 2 phases landing strategy.
@@ -424,12 +424,11 @@
   flagZTrajectoryFinished = false;
 
-  if (altitudeState == AltitudeState_t::Stopped) { // &&
-    // GetBatteryMonitor()->IsBatteryLow()==false)
-    // The uav always takes off in fail safe mode
-    EnterFailSafeMode();
-    joy->SetLedOFF(4); // DualShock3::led4
-    joy->SetLedOFF(1); // DualShock3::led1
-    joy->Rumble(0x70);
-    joy->SetZRef(0);
+    if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
+        //The uav always takes off in fail safe mode
+        EnterFailSafeMode();
+        joy->SetLedOFF(4);//DualShock3::led4
+        joy->SetLedOFF(1);//DualShock3::led1
+        joy->Rumble(0x70);
+        joy->SetZRef(0);
 
     uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
@@ -458,14 +457,22 @@
     joy->Rumble(0x70);
 
-    altitudeTrajectory->StopTraj();
-    joy->SetZRef(0);
-    altitudeTrajectory->StartTraj(
-        currentAltitude,
-        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
-    SignalEvent(Event_t::StartLanding);
-    altitudeState = AltitudeState_t::StartLanding;
-  } else {
-    joy->ErrorNotify();
-  }
+        altitudeTrajectory->StopTraj();
+        joy->SetZRef(0);
+        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
+        SignalEvent(Event_t::StartLanding);
+        altitudeState=AltitudeState_t::StartLanding;
+    } else if (altitudeState==AltitudeState_t::TakingOff) {
+        EmergencyLand();
+    } else {
+        joy->ErrorNotify();
+    }
+}
+
+void UavStateMachine::EmergencyLand(void) {
+    //Gradually decrease motor speed
+    //Called if landing is required during take off (motors are accelerating but Uav did not actually left the ground yet), or if critical sensors have been lost (attitude is lost)
+    altitudeState=AltitudeState_t::FinishLanding;
+    safeToFly=false;
+Printf("Emergency landing!\n");
 }
 
@@ -494,10 +501,11 @@
 
 void UavStateMachine::EmergencyStop(void) {
-  if (altitudeState != AltitudeState_t::Stopped) {
-    StopMotors();
-    EnterFailSafeMode();
-    joy->Rumble(0x70);
-    SignalEvent(Event_t::EmergencyStop);
-  }
+    if(altitudeState!=AltitudeState_t::Stopped) {
+        StopMotors();
+        EnterFailSafeMode();
+        joy->Rumble(0x70);
+        SignalEvent(Event_t::EmergencyStop);
+    }
+    safeToFly=false;
 }
 
@@ -534,12 +542,17 @@
     }
   }
-  if ((altitudeState == AltitudeState_t::TakingOff ||
-       altitudeState == AltitudeState_t::Stabilized) &&
-      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
-    flagBatteryLow = true;
-    Printf("Battery Low\n");
-    EnterFailSafeMode();
-    Land();
-  }
+    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
+        flagBatteryLow=true;
+        Thread::Err("Low Battery\n");
+        EnterFailSafeMode();
+        Land();
+    }
+    Time now=GetTime();
+    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
+        flagCriticalSensorLost=true;
+        Thread::Err("Critical sensor lost\n");
+        EnterFailSafeMode();
+        EmergencyLand(); 
+    }
 }
 
@@ -554,5 +567,5 @@
   static bool isSafeModeButtonPressed = false;
 
-  if (joy->IsButtonPressed(1)) { // select
+  if (controller->IsButtonPressed(1)) { // select
     if (!isEmergencyStopButtonPressed) {
       isEmergencyStopButtonPressed = true;
@@ -563,5 +576,5 @@
     isEmergencyStopButtonPressed = false;
 
-  if (joy->IsButtonPressed(0)) { // start
+  if (controller->IsButtonPressed(0)) { // start
     if (!isTakeOffButtonPressed) {
       isTakeOffButtonPressed = true;
@@ -585,7 +598,7 @@
   // check if l1,l2,r1 and r2 are not pressed
   // to allow a combination in user program
-  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
-      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
-      !joy->IsButtonPressed(10)) {
+  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
+      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
+      !controller->IsButtonPressed(10)) {
     if (!isSafeModeButtonPressed) {
       isSafeModeButtonPressed = true;
Index: trunk/lib/FlairMeta/src/UavStateMachine.h
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 22)
+++ trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 38)
@@ -101,6 +101,6 @@
   };
 
-  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
-  ~UavStateMachine();
+        UavStateMachine(meta::Uav* uav, sensor::TargetController* controller);
+        ~UavStateMachine();
 
   const core::Quaternion &GetCurrentQuaternion(void) const;
@@ -110,12 +110,13 @@
   const meta::Uav *GetUav(void) const;
 
-  void Land(void);
-  void TakeOff(void);
-  void EmergencyStop(void);
-  //! Used to signal an event
-  /*!
-      \param event the event which occured
-  */
-  virtual void SignalEvent(Event_t event);
+        void Land(void);
+        void EmergencyLand(void);
+        void TakeOff(void);
+        void EmergencyStop(void);
+        //! Used to signal an event
+        /*!
+            \param event the event which occured
+        */
+        virtual void SignalEvent(Event_t event);
 
   virtual const core::AhrsData *GetOrientation(void) const;
@@ -262,4 +263,5 @@
 
   meta::Uav *uav;
+  sensor::TargetController *controller;
 
   core::Quaternion currentQuaternion;
@@ -289,5 +291,7 @@
   bool flagBatteryLow;
   bool flagConnectionLost;
+  bool flagCriticalSensorLost;
   bool flagZTrajectoryFinished;
+  bool safeToFly;
   filter::NestedSat *uRoll, *uPitch;
   filter::Pid *uYaw;
Index: trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h
===================================================================
--- trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h	(revision 22)
+++ trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h	(revision 38)
@@ -14,5 +14,5 @@
 #define METADUALSHOCK3_IMPL_H
 
-#include <IODevice.h>
+#include <MetaDualShock3.h>
 
 namespace flair {
@@ -29,12 +29,12 @@
 * \brief Classe intégrant la manette DualShock3 et les consignes joystick
 */
-class MetaDualShock3_impl : public flair::core::IODevice {
+class MetaDualShock3_impl {
 public:
   MetaDualShock3_impl(flair::meta::MetaDualShock3 *self, std::string name);
   ~MetaDualShock3_impl();
   flair::filter::JoyReference *joy_ref;
+  void UpdateFrom(const flair::core::io_data *data);
 
 private:
-  void UpdateFrom(const flair::core::io_data *data);
   flair::meta::MetaDualShock3 *self;
   bool joy_init;
