Index: trunk/lib/FlairMeta/src/HdsX8.cpp
===================================================================
--- trunk/lib/FlairMeta/src/HdsX8.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/HdsX8.cpp	(revision 15)
@@ -32,38 +32,44 @@
 using namespace flair::actuator;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-HdsX8::HdsX8(FrameworkManager* parent,string uav_name,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
-    RTDM_I2cPort* i2cport=new RTDM_I2cPort(parent,"rtdm_i2c","rti2c3");
-    RTDM_SerialPort* imu_port=new RTDM_SerialPort(parent,"imu_port","rtser1");
+HdsX8::HdsX8(FrameworkManager *parent, string uav_name,
+             filter::UavMultiplex *multiplex)
+    : Uav(parent, uav_name, multiplex) {
+  RTDM_I2cPort *i2cport = new RTDM_I2cPort(parent, "rtdm_i2c", "rti2c3");
+  RTDM_SerialPort *imu_port = new RTDM_SerialPort(parent, "imu_port", "rtser1");
 
-    if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
+  if (multiplex == NULL)
+    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
 
-    SetBldc(new BlCtrlV2(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),i2cport));
-    SetUsRangeFinder(new Srf08(parent,"SRF08",i2cport,0x70,60));
-    SetAhrs(new Gx3_25_ahrs(parent,"imu",imu_port,Gx3_25_imu::EulerAnglesAndAngularRates,70));
-    SetBatteryMonitor(((BlCtrlV2*)GetBldc())->GetBatteryMonitor());
+  SetBldc(new BlCtrlV2(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
+                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
+  SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
+  SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
+                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
+  SetBatteryMonitor(((BlCtrlV2 *)GetBldc())->GetBatteryMonitor());
 
-/*
-        if(VRPNType==Auto || VRPNType==AutoSerialPort)
-        {
-            RTDM_SerialPort* vrpn_port=new RTDM_SerialPort(parent,"vrpn_port","rtser3");
+  /*
+          if(VRPNType==Auto || VRPNType==AutoSerialPort)
+          {
+              RTDM_SerialPort* vrpn_port=new
+     RTDM_SerialPort(parent,"vrpn_port","rtser3");
 
-            vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
-            uav_vrpn=new MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
-        }
-*/
-    SetVerticalCamera(new Ps3Eye(parent,"camv",0,50));
+              vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
+              uav_vrpn=new
+     MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
+          }
+  */
+  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
 }
 
-HdsX8::~HdsX8() {
-
-}
+HdsX8::~HdsX8() {}
 
 void HdsX8::StartSensors(void) {
-    ((Gx3_25_ahrs*)GetAhrs())->Start();
-    ((Srf08*)GetUsRangeFinder())->Start();
-    ((Ps3Eye *)GetVerticalCamera())->Start();
-    Uav::StartSensors();
+  ((Gx3_25_ahrs *)GetAhrs())->Start();
+  ((Srf08 *)GetUsRangeFinder())->Start();
+  ((Ps3Eye *)GetVerticalCamera())->Start();
+  Uav::StartSensors();
 }
 
Index: trunk/lib/FlairMeta/src/HdsX8.h
===================================================================
--- trunk/lib/FlairMeta/src/HdsX8.h	(revision 10)
+++ trunk/lib/FlairMeta/src/HdsX8.h	(revision 15)
@@ -16,21 +16,19 @@
 #include "Uav.h"
 
-namespace flair
-{
-namespace meta
-{
-    /*! \class HdsX8
-    *
-    * \brief Class defining an ardrone2 uav
-    */
-    class HdsX8 : public Uav {
-        public:
-            HdsX8(core::FrameworkManager* parent,std::string uav_name,filter::UavMultiplex *multiplex=NULL);
-            ~HdsX8();
-            void StartSensors(void);
+namespace flair {
+namespace meta {
+/*! \class HdsX8
+*
+* \brief Class defining an ardrone2 uav
+*/
+class HdsX8 : public Uav {
+public:
+  HdsX8(core::FrameworkManager *parent, std::string uav_name,
+        filter::UavMultiplex *multiplex = NULL);
+  ~HdsX8();
+  void StartSensors(void);
 
-        private:
-
-    };
+private:
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/MetaDualShock3.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaDualShock3.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaDualShock3.cpp	(revision 15)
@@ -31,67 +31,65 @@
 using namespace flair::gui;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-MetaDualShock3::MetaDualShock3(FrameworkManager* parent,string name,uint16_t port,uint8_t priority) : TargetEthController(parent,name,port,priority) {
-    pimpl_=new MetaDualShock3_impl(this,name);
-    parent->AddDeviceToLog(pimpl_->joy_ref);
-    Start();
+MetaDualShock3::MetaDualShock3(FrameworkManager *parent, string name,
+                               uint16_t port, uint8_t priority)
+    : TargetEthController(parent, name, port, priority) {
+  pimpl_ = new MetaDualShock3_impl(this, name);
+  parent->AddDeviceToLog(pimpl_->joy_ref);
+  Start();
 }
 
-MetaDualShock3::~MetaDualShock3() {
-    delete pimpl_;
-}
+MetaDualShock3::~MetaDualShock3() { delete pimpl_; }
 
-AhrsData* MetaDualShock3::GetReferenceOrientation(void) const {
-    return pimpl_->joy_ref->GetReferenceOrientation();
+AhrsData *MetaDualShock3::GetReferenceOrientation(void) const {
+  return pimpl_->joy_ref->GetReferenceOrientation();
 }
 
 void MetaDualShock3::ErrorNotify(void) {
-    TargetEthController::FlashLed(4,10,0);
-    TargetEthController::Rumble(0xff,20,0,0);
+  TargetEthController::FlashLed(4, 10, 0);
+  TargetEthController::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) {
+  TargetEthController::Rumble(left_force, left_timeout, right_force,
+                              right_timeout);
 }
 
 void MetaDualShock3::SetLedON(unsigned int ledId) {
-    TargetEthController::SetLedOn(ledId);
+  TargetEthController::SetLedOn(ledId);
 }
 
 void MetaDualShock3::SetLedOFF(unsigned int ledId) {
-    TargetEthController::SetLedOff(ledId);
+  TargetEthController::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) {
+  TargetEthController::FlashLed(ledId, on_timeout, off_timeout);
 }
 
-float MetaDualShock3::ZRef(void) const {
-    return pimpl_->joy_ref->ZRef();
-}
+float MetaDualShock3::ZRef(void) const { return pimpl_->joy_ref->ZRef(); }
 
-float MetaDualShock3::DzRef(void) const {
-    return pimpl_->joy_ref->DzRef();
-}
+float MetaDualShock3::DzRef(void) const { return pimpl_->joy_ref->DzRef(); }
 
 void MetaDualShock3::SetYawRef(float value) {
-    pimpl_->joy_ref->SetYawRef(value);
+  pimpl_->joy_ref->SetYawRef(value);
 }
 
 void MetaDualShock3::SetYawRef(Quaternion const &value) {
-    pimpl_->joy_ref->SetYawRef(value);
+  pimpl_->joy_ref->SetYawRef(value);
 }
 
-void MetaDualShock3::SetZRef(float value) {
-    pimpl_->joy_ref->SetZRef(value);
-}
+void MetaDualShock3::SetZRef(float value) { pimpl_->joy_ref->SetZRef(value); }
 
 float MetaDualShock3::RollTrim(void) const {
-    return pimpl_->joy_ref->RollTrim();
+  return pimpl_->joy_ref->RollTrim();
 }
 
 float MetaDualShock3::PitchTrim(void) const {
-    return pimpl_->joy_ref->PitchTrim();
+  return pimpl_->joy_ref->PitchTrim();
 }
 
Index: trunk/lib/FlairMeta/src/MetaDualShock3.h
===================================================================
--- trunk/lib/FlairMeta/src/MetaDualShock3.h	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaDualShock3.h	(revision 15)
@@ -17,11 +17,11 @@
 
 namespace flair {
-    namespace core {
-        class AhrsData;
-        class Quaternion;
-    }
-    namespace filter {
-        class Ahrs;
-    }
+namespace core {
+class AhrsData;
+class Quaternion;
+}
+namespace filter {
+class Ahrs;
+}
 }
 
@@ -31,40 +31,42 @@
 namespace meta {
 
-    /*! \class MetaDualShock3
-    *
-    * \brief Classe intégrant la manette MetaDualShock3
-    */
-    class MetaDualShock3 : public sensor::TargetEthController {
-        friend class ::MetaDualShock3_impl;
+/*! \class MetaDualShock3
+*
+* \brief Classe intégrant la manette MetaDualShock3
+*/
+class MetaDualShock3 : public sensor::TargetEthController {
+  friend class ::MetaDualShock3_impl;
 
-        public:
-            MetaDualShock3(core::FrameworkManager* parent,std::string name,uint16_t port,uint8_t priority);
-            ~MetaDualShock3();
-            core::AhrsData* GetReferenceOrientation(void) const;
-            float ZRef(void) const;
-            float DzRef(void) const;
-            void SetYawRef(float value);
-            /*!
-            * \brief Set yaw reference
-            *
-            * Yaw part of the output quaternion is obtained by integrating the wz desired angular speed.\n
-            * This method reset the yaw.
-            *
-            * \param value value, only the yaw part of the quaternion is used
-            */
-            void SetYawRef(core::Quaternion const &value);
-            void SetZRef(float value);
-            float RollTrim(void) const;
-            float PitchTrim(void) const;
-            void ErrorNotify(void);
-            void Rumble(uint8_t left_force,uint8_t left_timeout=20,uint8_t right_force=0,uint8_t right_timeout=0);
-            void SetLedON(unsigned int ledId);
-            void SetLedOFF(unsigned int ledId);
-            void FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout);
+public:
+  MetaDualShock3(core::FrameworkManager *parent, std::string name,
+                 uint16_t port, uint8_t priority);
+  ~MetaDualShock3();
+  core::AhrsData *GetReferenceOrientation(void) const;
+  float ZRef(void) const;
+  float DzRef(void) const;
+  void SetYawRef(float value);
+  /*!
+  * \brief Set yaw reference
+  *
+  * Yaw part of the output quaternion is obtained by integrating the wz desired
+  *angular speed.\n
+  * This method reset the yaw.
+  *
+  * \param value value, only the yaw part of the quaternion is used
+  */
+  void SetYawRef(core::Quaternion const &value);
+  void SetZRef(float value);
+  float RollTrim(void) const;
+  float PitchTrim(void) const;
+  void ErrorNotify(void);
+  void Rumble(uint8_t left_force, uint8_t left_timeout = 20,
+              uint8_t right_force = 0, uint8_t right_timeout = 0);
+  void SetLedON(unsigned int ledId);
+  void SetLedOFF(unsigned int ledId);
+  void FlashLed(unsigned int ledId, uint8_t on_timeout, uint8_t off_timeout);
 
-        private:
-            class MetaDualShock3_impl* pimpl_;
-
-    };
+private:
+  class MetaDualShock3_impl *pimpl_;
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp	(revision 15)
@@ -29,85 +29,84 @@
 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");
-    this->self=self;
-    joy_init=false;
-    wasRollTrimUpButtonPressed=false;
-    wasRollTrimDownButtonPressed=false;
-    wasPitchTrimUpButtonPressed=false;
-    wasPitchTrimDownButtonPressed=false;
+MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name)
+    : IODevice((IODevice *)self, name) {
+  joy_ref = new JoyReference(self->GetTab()->NewRow(), "consignes joy");
+  this->self = self;
+  joy_init = false;
+  wasRollTrimUpButtonPressed = false;
+  wasRollTrimDownButtonPressed = false;
+  wasPitchTrimUpButtonPressed = false;
+  wasPitchTrimDownButtonPressed = false;
 }
 
-MetaDualShock3_impl::~MetaDualShock3_impl() {
+MetaDualShock3_impl::~MetaDualShock3_impl() {}
 
+// receives updates from the controler
+void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
+  cvmatrix *input = (cvmatrix *)data;
+
+  // on prend une fois pour toute le mutex et on fait des accès directs
+  input->GetMutex();
+
+  // up
+  if (self->IsButtonPressed(12)) {
+    if (!wasPitchTrimDownButtonPressed) {
+      joy_ref->PitchTrimDown();
+      wasPitchTrimDownButtonPressed = true;
+    }
+  } else {
+    wasPitchTrimDownButtonPressed = false;
+  }
+
+  // down
+  if (self->IsButtonPressed(13)) {
+    if (!wasPitchTrimUpButtonPressed) {
+      joy_ref->PitchTrimUp();
+      wasPitchTrimUpButtonPressed = true;
+    }
+  } else {
+    wasPitchTrimUpButtonPressed = false;
+  }
+
+  // right
+  if (self->IsButtonPressed(15)) {
+    if (!wasRollTrimUpButtonPressed) {
+      joy_ref->RollTrimUp();
+      wasRollTrimUpButtonPressed = true;
+    }
+  } else {
+    wasRollTrimUpButtonPressed = false;
+  }
+
+  // left
+  if (self->IsButtonPressed(14)) {
+    if (!wasRollTrimDownButtonPressed) {
+      joy_ref->RollTrimDown();
+      wasRollTrimDownButtonPressed = true;
+    }
+  } else {
+    wasRollTrimDownButtonPressed = false;
+  }
+
+  if (!getFrameworkManager()->ConnectionLost()) {
+    input->GetMutex();
+    joy_ref->SetRollAxis(input->ValueNoMutex(0, 0));
+    joy_ref->SetPitchAxis(input->ValueNoMutex(1, 0));
+    joy_ref->SetYawAxis(input->ValueNoMutex(2, 0));
+    joy_ref->SetAltitudeAxis(input->ValueNoMutex(3, 0));
+    input->ReleaseMutex();
+  } else {
+    joy_ref->SetRollAxis(0);
+    joy_ref->SetPitchAxis(0);
+    joy_ref->SetYawAxis(0);
+    joy_ref->SetAltitudeAxis(0);
+  }
+  input->ReleaseMutex();
+
+  joy_ref->Update(data->DataTime());
+
+  if (!joy_init) {
+    self->TargetEthController::FlashLed(1, 10, 10);
+    joy_init = true;
+  }
 }
-
-//receives updates from the controler
-void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
-    cvmatrix *input=(cvmatrix*)data;
-
-    //on prend une fois pour toute le mutex et on fait des accès directs
-    input->GetMutex();
-
-    //up
-    if(self->IsButtonPressed(12)) {
-        if(!wasPitchTrimDownButtonPressed) {
-            joy_ref->PitchTrimDown();
-            wasPitchTrimDownButtonPressed=true;
-        }
-    } else {
-        wasPitchTrimDownButtonPressed=false;
-    }
-
-    //down
-    if(self->IsButtonPressed(13)) {
-        if(!wasPitchTrimUpButtonPressed) {
-            joy_ref->PitchTrimUp();
-            wasPitchTrimUpButtonPressed=true;
-        }
-    } else {
-        wasPitchTrimUpButtonPressed=false;
-    }
-
-    //right
-    if(self->IsButtonPressed(15)) {
-        if(!wasRollTrimUpButtonPressed) {
-            joy_ref->RollTrimUp();
-            wasRollTrimUpButtonPressed=true;
-        }
-    } else {
-        wasRollTrimUpButtonPressed=false;
-    }
-
-    //left
-    if(self->IsButtonPressed(14)) {
-        if(!wasRollTrimDownButtonPressed) {
-            joy_ref->RollTrimDown();
-            wasRollTrimDownButtonPressed=true;
-        }
-    } else {
-        wasRollTrimDownButtonPressed=false;
-    }
-
-    if(!getFrameworkManager()->ConnectionLost()) {
-        input->GetMutex();
-        joy_ref->SetRollAxis(input->ValueNoMutex(0,0));
-        joy_ref->SetPitchAxis(input->ValueNoMutex(1,0));
-        joy_ref->SetYawAxis(input->ValueNoMutex(2,0));
-        joy_ref->SetAltitudeAxis(input->ValueNoMutex(3,0));
-        input->ReleaseMutex();
-    } else {
-        joy_ref->SetRollAxis(0);
-        joy_ref->SetPitchAxis(0);
-        joy_ref->SetYawAxis(0);
-        joy_ref->SetAltitudeAxis(0);
-    }
-    input->ReleaseMutex();
-
-    joy_ref->Update(data->DataTime());
-
-    if(!joy_init) {
-        self->TargetEthController::FlashLed(1,10,10);
-        joy_init=true;
-    }
-}
Index: trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp	(revision 15)
@@ -35,44 +35,36 @@
 using namespace flair::sensor;
 
-namespace flair { namespace meta
-{
+namespace flair {
+namespace meta {
 
-MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder* us): Object(us,us->ObjectName()) {
-    this->us=us;
-    pbas_z=new ButterworthLowPass(us,us->GetLayout()->NewRow(),"Passe bas",3);
-    vz_euler=new EulerDerivative(pbas_z,us->GetLayout()->NewRow(),"Vz");
-    pbas_vz=new ButterworthLowPass(vz_euler,us->GetLayout()->NewRow(),"Passe bas v",3);
-
+MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
+    : Object(us, us->ObjectName()) {
+  this->us = us;
+  pbas_z =
+      new ButterworthLowPass(us, us->GetLayout()->NewRow(), "Passe bas", 3);
+  vz_euler = new EulerDerivative(pbas_z, us->GetLayout()->NewRow(), "Vz");
+  pbas_vz = new ButterworthLowPass(vz_euler, us->GetLayout()->NewRow(),
+                                   "Passe bas v", 3);
 }
 
-MetaUsRangeFinder::~MetaUsRangeFinder() {
+MetaUsRangeFinder::~MetaUsRangeFinder() {}
 
+void MetaUsRangeFinder::UseDefaultPlot(void) {
+  us->UseDefaultPlot();
+
+  us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0), DataPlot::Blue);
+
+  vz_plot = new DataPlot1D(us->GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
+  vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
+  vz_plot->AddCurve(pbas_vz->Matrix()->Element(0), DataPlot::Blue);
 }
 
-void MetaUsRangeFinder::UseDefaultPlot(void) {
-    us->UseDefaultPlot();
+gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() { return us->GetPlot(); }
 
-    us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0),DataPlot::Blue);
+gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() { return vz_plot; }
 
-    vz_plot=new DataPlot1D(us->GetPlotTab()->LastRowLastCol(),"vz",-2,2);
-    vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
-    vz_plot->AddCurve(pbas_vz->Matrix()->Element(0),DataPlot::Blue);
-}
+float MetaUsRangeFinder::z(void) const { return pbas_z->Output(); }
 
-gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() {
-    return us->GetPlot();
-}
-
-gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() {
-    return vz_plot;
-}
-
-float MetaUsRangeFinder::z(void) const {
-    return pbas_z->Output();
-}
-
-float MetaUsRangeFinder::Vz(void) const {
-    return pbas_vz->Output();
-}
+float MetaUsRangeFinder::Vz(void) const { return pbas_vz->Output(); }
 
 } // end namespace sensor
Index: trunk/lib/FlairMeta/src/MetaUsRangeFinder.h
===================================================================
--- trunk/lib/FlairMeta/src/MetaUsRangeFinder.h	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaUsRangeFinder.h	(revision 15)
@@ -17,43 +17,41 @@
 
 namespace flair {
-    namespace filter {
-        class ButterworthLowPass;
-        class EulerDerivative;
-    }
-    namespace sensor {
-        class UsRangeFinder;
-    }
-    namespace gui {
-        class DataPlot1D;
-    }
+namespace filter {
+class ButterworthLowPass;
+class EulerDerivative;
+}
+namespace sensor {
+class UsRangeFinder;
+}
+namespace gui {
+class DataPlot1D;
+}
 }
 
-namespace flair
-{
-namespace meta
-{
-    /*! \class MetaUsRangeFinder
-    *
-    * \brief Classe haut niveau pour capteur à ultra son
-    *
-    * Contient une dérivée d'euler et un passe bas.
-    * Cette classe est adaptée pour un capteur d'altitude.
-    */
-    class MetaUsRangeFinder: public core::Object {
-        public:
-            MetaUsRangeFinder(sensor::UsRangeFinder* us);
-            ~MetaUsRangeFinder();
-            void UseDefaultPlot(void);
-            float z(void) const;
-            float Vz(void) const;
-            gui::DataPlot1D *GetZPlot();
-            gui::DataPlot1D *GetVzPlot();
+namespace flair {
+namespace meta {
+/*! \class MetaUsRangeFinder
+*
+* \brief Classe haut niveau pour capteur à ultra son
+*
+* Contient une dérivée d'euler et un passe bas.
+* Cette classe est adaptée pour un capteur d'altitude.
+*/
+class MetaUsRangeFinder : public core::Object {
+public:
+  MetaUsRangeFinder(sensor::UsRangeFinder *us);
+  ~MetaUsRangeFinder();
+  void UseDefaultPlot(void);
+  float z(void) const;
+  float Vz(void) const;
+  gui::DataPlot1D *GetZPlot();
+  gui::DataPlot1D *GetVzPlot();
 
-        private:
-            sensor::UsRangeFinder* us;
-            filter::ButterworthLowPass *pbas_z,*pbas_vz;
-            filter::EulerDerivative *vz_euler;
-            gui::DataPlot1D* vz_plot;
-    };
+private:
+  sensor::UsRangeFinder *us;
+  filter::ButterworthLowPass *pbas_z, *pbas_vz;
+  filter::EulerDerivative *vz_euler;
+  gui::DataPlot1D *vz_plot;
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/MetaVrpnObject.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaVrpnObject.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaVrpnObject.cpp	(revision 15)
@@ -36,79 +36,69 @@
 using namespace flair::filter;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent,string name): VrpnObject(parent,name,parent->GetTabWidget())
-{
-    ConstructorCommon(parent,name);
+MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent, string name)
+    : VrpnObject(parent, name, parent->GetTabWidget()) {
+  ConstructorCommon(parent, name);
 }
 
-MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent,std::string name,uint8_t id): VrpnObject(parent,name,id,parent->GetTabWidget())
-{
-    ConstructorCommon(parent,name);
+MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent, std::string name,
+                               uint8_t id)
+    : VrpnObject(parent, name, id, parent->GetTabWidget()) {
+  ConstructorCommon(parent, name);
 }
 
-void MetaVrpnObject::ConstructorCommon(const VrpnClient *parent,string name) {
-    cvmatrix_descriptor* desc=new cvmatrix_descriptor(6,1);
-    for(int i=0;i<6;i++) {
-        desc->SetElementName(i,0,Output()->Name(i,0));
-    }
-    cvmatrix* prev_value=new cvmatrix(this,desc,elementDataType,name);
-    for(int i=0;i<6;i++) {
-        prev_value->SetValue(i,0,0);
-    }
+void MetaVrpnObject::ConstructorCommon(const VrpnClient *parent, string name) {
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
+  for (int i = 0; i < 6; i++) {
+    desc->SetElementName(i, 0, Output()->Name(i, 0));
+  }
+  cvmatrix *prev_value = new cvmatrix(this, desc, elementDataType, name);
+  for (int i = 0; i < 6; i++) {
+    prev_value->SetValue(i, 0, 0);
+  }
 
-    pbas=new LowPassFilter(this,parent->GetLayout()->NewRow(),name + " Passe bas",prev_value);
+  pbas = new LowPassFilter(this, parent->GetLayout()->NewRow(),
+                           name + " Passe bas", prev_value);
 
-    desc=new cvmatrix_descriptor(6,1);
-    for(int i=0;i<6;i++) {
-        desc->SetElementName(i,0,"d" + Output()->Name(i,0));
-    }
-    prev_value=new cvmatrix(this,desc,elementDataType,name);
-    for(int i=0;i<6;i++) {
-        prev_value->SetValue(i,0,0);
-    }
+  desc = new cvmatrix_descriptor(6, 1);
+  for (int i = 0; i < 6; i++) {
+    desc->SetElementName(i, 0, "d" + Output()->Name(i, 0));
+  }
+  prev_value = new cvmatrix(this, desc, elementDataType, name);
+  for (int i = 0; i < 6; i++) {
+    prev_value->SetValue(i, 0, 0);
+  }
 
-    euler=new EulerDerivative(pbas,parent->GetLayout()->NewRow(),name + "_euler",prev_value);
+  euler = new EulerDerivative(pbas, parent->GetLayout()->NewRow(),
+                              name + "_euler", prev_value);
 
-    vx_opti_plot=new DataPlot1D(GetPlotTab()->NewRow(),"vx",-3,3);
-    vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
-    vy_opti_plot=new DataPlot1D(GetPlotTab()->LastRowLastCol(),"vy",-3,3);
-    vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
-    vz_opti_plot=new DataPlot1D(GetPlotTab()->LastRowLastCol(),"vz",-2,2);
-    vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
+  vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);
+  vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
+  vy_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vy", -3, 3);
+  vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
+  vz_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
+  vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
 
-    plot_tab=new Tab(parent->GetTabWidget(),"Mesures (xy) "+ name);
-    xy_plot=new DataPlot2D(plot_tab->NewRow(),"xy","y",-5,5,"x",-5,5);
-        xy_plot->AddCurve(Output()->Element(4,0),Output()->Element(3,0));
+  plot_tab = new Tab(parent->GetTabWidget(), "Mesures (xy) " + name);
+  xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
+  xy_plot->AddCurve(Output()->Element(4, 0), Output()->Element(3, 0));
 }
 
-MetaVrpnObject::~MetaVrpnObject() {
-    delete plot_tab;
-}
+MetaVrpnObject::~MetaVrpnObject() { delete plot_tab; }
 
-DataPlot1D* MetaVrpnObject::VxPlot(void) const
-{
-    return vx_opti_plot;
-}
+DataPlot1D *MetaVrpnObject::VxPlot(void) const { return vx_opti_plot; }
 
-DataPlot1D* MetaVrpnObject::VyPlot(void) const
-{
-    return vy_opti_plot;
-}
+DataPlot1D *MetaVrpnObject::VyPlot(void) const { return vy_opti_plot; }
 
-DataPlot1D* MetaVrpnObject::VzPlot(void) const
-{
-    return vz_opti_plot;
-}
+DataPlot1D *MetaVrpnObject::VzPlot(void) const { return vz_opti_plot; }
 
-DataPlot2D* MetaVrpnObject::XyPlot(void) const {
-    return xy_plot;
-}
+DataPlot2D *MetaVrpnObject::XyPlot(void) const { return xy_plot; }
 
-void MetaVrpnObject::GetSpeed(Vector3D &speed) const
-{
-    speed.x=euler->Output(3,0);
-    speed.y=euler->Output(4,0);
-    speed.z=euler->Output(5,0);
+void MetaVrpnObject::GetSpeed(Vector3D &speed) const {
+  speed.x = euler->Output(3, 0);
+  speed.y = euler->Output(4, 0);
+  speed.z = euler->Output(5, 0);
 }
 
Index: trunk/lib/FlairMeta/src/MetaVrpnObject.h
===================================================================
--- trunk/lib/FlairMeta/src/MetaVrpnObject.h	(revision 10)
+++ trunk/lib/FlairMeta/src/MetaVrpnObject.h	(revision 15)
@@ -17,60 +17,53 @@
 #include "io_data.h"
 
-namespace flair
-{
-    namespace core
-    {
-        class Vector3D;
-        class FloatType;
-    }
-    namespace gui
-    {
-        class DataPlot1D;
-        class DataPlot2D;
-        class Tab;
-    }
-    namespace filter
-    {
-        class EulerDerivative;
-        class LowPassFilter;
-    }
-    namespace sensor
-    {
-        class VrpnClient;
-    }
+namespace flair {
+namespace core {
+class Vector3D;
+class FloatType;
+}
+namespace gui {
+class DataPlot1D;
+class DataPlot2D;
+class Tab;
+}
+namespace filter {
+class EulerDerivative;
+class LowPassFilter;
+}
+namespace sensor {
+class VrpnClient;
+}
 }
 
-namespace flair
-{
-namespace meta
-{
+namespace flair {
+namespace meta {
 
-    /*! \class MetaVrpnObject
-    *
-    * \brief Classe haut niveau intégrant un objet VRPN
-    *
-    * Contient un objet VRPN et une dérivée, d'euler.
-    */
-    class MetaVrpnObject: public sensor::VrpnObject {
-        public:
-            MetaVrpnObject(const sensor::VrpnClient *parent,std::string name);
-            MetaVrpnObject(const sensor::VrpnClient *parent,std::string name,uint8_t id);
-            ~MetaVrpnObject();
-            gui::DataPlot1D* VxPlot(void) const;//1,0
-            gui::DataPlot1D* VyPlot(void) const;//1,1
-            gui::DataPlot1D* VzPlot(void) const;//1,2
-            gui::DataPlot2D* XyPlot(void) const;
-            void GetSpeed(core::Vector3D &speed) const;
+/*! \class MetaVrpnObject
+*
+* \brief Classe haut niveau intégrant un objet VRPN
+*
+* Contient un objet VRPN et une dérivée, d'euler.
+*/
+class MetaVrpnObject : public sensor::VrpnObject {
+public:
+  MetaVrpnObject(const sensor::VrpnClient *parent, std::string name);
+  MetaVrpnObject(const sensor::VrpnClient *parent, std::string name,
+                 uint8_t id);
+  ~MetaVrpnObject();
+  gui::DataPlot1D *VxPlot(void) const; // 1,0
+  gui::DataPlot1D *VyPlot(void) const; // 1,1
+  gui::DataPlot1D *VzPlot(void) const; // 1,2
+  gui::DataPlot2D *XyPlot(void) const;
+  void GetSpeed(core::Vector3D &speed) const;
 
-        private:
-            void ConstructorCommon(const sensor::VrpnClient *parent,std::string name);
-            filter::LowPassFilter *pbas;
-            filter::EulerDerivative *euler;
-            gui::DataPlot2D *xy_plot;
-            gui::DataPlot1D *vx_opti_plot,*vy_opti_plot,*vz_opti_plot;
-            gui::Tab* plot_tab;
-            core::FloatType elementDataType;
-
-    };
+private:
+  void ConstructorCommon(const sensor::VrpnClient *parent, std::string name);
+  filter::LowPassFilter *pbas;
+  filter::EulerDerivative *euler;
+  gui::DataPlot2D *xy_plot;
+  gui::DataPlot1D *vx_opti_plot, *vy_opti_plot, *vz_opti_plot;
+  gui::Tab *plot_tab;
+  core::FloatType elementDataType;
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/SimuX4.cpp
===================================================================
--- trunk/lib/FlairMeta/src/SimuX4.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/SimuX4.cpp	(revision 15)
@@ -34,33 +34,35 @@
 using namespace flair::actuator;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-SimuX4::SimuX4(FrameworkManager* parent,string uav_name,int simu_id,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
+SimuX4::SimuX4(FrameworkManager *parent, string uav_name, int simu_id,
+               filter::UavMultiplex *multiplex)
+    : Uav(parent, uav_name, multiplex) {
 
-    if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X4));
+  if (multiplex == NULL)
+    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X4));
 
-    SetBldc(new SimuBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),simu_id));
-    SetUsRangeFinder(new SimuUs(parent,"us",simu_id,60));
-    SetAhrs(new SimuAhrs(parent,"imu",simu_id,70));
-    Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
-    SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
-    GetBatteryMonitor()->SetBatteryValue(12);
-    SetVerticalCamera(new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
+  SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
+                       "motors", GetUavMultiplex()->MotorsCount(), simu_id));
+  SetUsRangeFinder(new SimuUs(parent, "us", simu_id, 60));
+  SetAhrs(new SimuAhrs(parent, "imu", simu_id, 70));
+  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
+  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
+  GetBatteryMonitor()->SetBatteryValue(12);
+  SetVerticalCamera(
+      new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
 }
 
-SimuX4::~SimuX4() {
+SimuX4::~SimuX4() {}
 
+void SimuX4::StartSensors(void) {
+  ((SimuAhrs *)GetAhrs())->Start();
+  ((SimuUs *)GetUsRangeFinder())->Start();
+  ((SimuCamera *)GetVerticalCamera())->Start();
+  Uav::StartSensors();
 }
 
-void SimuX4::StartSensors(void) {
-    ((SimuAhrs*)GetAhrs())->Start();
-    ((SimuUs*)GetUsRangeFinder())->Start();
-    ((SimuCamera *)GetVerticalCamera())->Start();
-    Uav::StartSensors();
-}
-
-void SimuX4::SetupVRPNAutoIP(string name) {
-    SetupVRPN("127.0.0.1:3883",name);
-}
+void SimuX4::SetupVRPNAutoIP(string name) { SetupVRPN("127.0.0.1:3883", name); }
 
 } // end namespace meta
Index: trunk/lib/FlairMeta/src/SimuX4.h
===================================================================
--- trunk/lib/FlairMeta/src/SimuX4.h	(revision 10)
+++ trunk/lib/FlairMeta/src/SimuX4.h	(revision 15)
@@ -16,21 +16,20 @@
 #include "Uav.h"
 
-namespace flair
-{
-namespace meta
-{
-    /*! \class SimuX4
-    *
-    * \brief Class defining a simultation x4 uav
-    */
-    class SimuX4 : public Uav {
-        public:
-            //simu_id: 0 if simulating only one UAV
-            //>0 otherwise
-            SimuX4(core::FrameworkManager* parent,std::string uav_name,int simu_id=0,filter::UavMultiplex *multiplex=NULL);
-            ~SimuX4();
-            void StartSensors(void);
-            void SetupVRPNAutoIP(std::string name);
-    };
+namespace flair {
+namespace meta {
+/*! \class SimuX4
+*
+* \brief Class defining a simultation x4 uav
+*/
+class SimuX4 : public Uav {
+public:
+  // simu_id: 0 if simulating only one UAV
+  //>0 otherwise
+  SimuX4(core::FrameworkManager *parent, std::string uav_name, int simu_id = 0,
+         filter::UavMultiplex *multiplex = NULL);
+  ~SimuX4();
+  void StartSensors(void);
+  void SetupVRPNAutoIP(std::string name);
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/SimuX8.cpp
===================================================================
--- trunk/lib/FlairMeta/src/SimuX8.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/SimuX8.cpp	(revision 15)
@@ -34,33 +34,35 @@
 using namespace flair::actuator;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-SimuX8::SimuX8(FrameworkManager* parent,string uav_name,int simu_id,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
+SimuX8::SimuX8(FrameworkManager *parent, string uav_name, int simu_id,
+               filter::UavMultiplex *multiplex)
+    : Uav(parent, uav_name, multiplex) {
 
-    if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
+  if (multiplex == NULL)
+    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
 
-    SetBldc(new SimuBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),simu_id));
-    SetUsRangeFinder(new SimuUs(parent,"us",simu_id,60));
-    SetAhrs(new SimuAhrs(parent,"imu",simu_id,70));
-    Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
-    SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
-    GetBatteryMonitor()->SetBatteryValue(12);
-    SetVerticalCamera(new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
+  SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
+                       "motors", GetUavMultiplex()->MotorsCount(), simu_id));
+  SetUsRangeFinder(new SimuUs(parent, "us", simu_id, 60));
+  SetAhrs(new SimuAhrs(parent, "imu", simu_id, 70));
+  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
+  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
+  GetBatteryMonitor()->SetBatteryValue(12);
+  SetVerticalCamera(
+      new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
 }
 
-SimuX8::~SimuX8() {
+SimuX8::~SimuX8() {}
 
+void SimuX8::StartSensors(void) {
+  ((SimuAhrs *)GetAhrs())->Start();
+  ((SimuUs *)GetUsRangeFinder())->Start();
+  ((SimuCamera *)GetVerticalCamera())->Start();
+  Uav::StartSensors();
 }
 
-void SimuX8::StartSensors(void) {
-    ((SimuAhrs*)GetAhrs())->Start();
-    ((SimuUs*)GetUsRangeFinder())->Start();
-    ((SimuCamera *)GetVerticalCamera())->Start();
-    Uav::StartSensors();
-}
-
-void SimuX8::SetupVRPNAutoIP(string name) {
-    SetupVRPN("127.0.0.1:3883",name);
-}
+void SimuX8::SetupVRPNAutoIP(string name) { SetupVRPN("127.0.0.1:3883", name); }
 
 } // end namespace meta
Index: trunk/lib/FlairMeta/src/SimuX8.h
===================================================================
--- trunk/lib/FlairMeta/src/SimuX8.h	(revision 10)
+++ trunk/lib/FlairMeta/src/SimuX8.h	(revision 15)
@@ -16,21 +16,20 @@
 #include "Uav.h"
 
-namespace flair
-{
-namespace meta
-{
-    /*! \class SimuX8
-    *
-    * \brief Class defining a simultation x8 uav
-    */
-    class SimuX8 : public Uav {
-        public:
-            //simu_id: 0 if simulating only one UAV
-            //>0 otherwise
-            SimuX8(core::FrameworkManager* parent,std::string uav_name,int simu_id=0,filter::UavMultiplex *multiplex=NULL);
-            ~SimuX8();
-            void StartSensors(void);
-            void SetupVRPNAutoIP(std::string name);
-    };
+namespace flair {
+namespace meta {
+/*! \class SimuX8
+*
+* \brief Class defining a simultation x8 uav
+*/
+class SimuX8 : public Uav {
+public:
+  // simu_id: 0 if simulating only one UAV
+  //>0 otherwise
+  SimuX8(core::FrameworkManager *parent, std::string uav_name, int simu_id = 0,
+         filter::UavMultiplex *multiplex = NULL);
+  ~SimuX8();
+  void StartSensors(void);
+  void SetupVRPNAutoIP(std::string name);
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/Uav.cpp
===================================================================
--- trunk/lib/FlairMeta/src/Uav.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/Uav.cpp	(revision 15)
@@ -38,46 +38,46 @@
 using namespace flair::actuator;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-Uav::Uav(FrameworkManager* parent,string name,UavMultiplex *multiplex): Object(parent,name) {
-    vrpnclient=NULL;
-    uav_vrpn=NULL;
-    verticalCamera=NULL;
-    this->multiplex=multiplex;
+Uav::Uav(FrameworkManager *parent, string name, UavMultiplex *multiplex)
+    : Object(parent, name) {
+  vrpnclient = NULL;
+  uav_vrpn = NULL;
+  verticalCamera = NULL;
+  this->multiplex = multiplex;
 }
 
-Uav::~Uav() {
-}
+Uav::~Uav() {}
 
 void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) {
-    us=(UsRangeFinder*)inUs;
-    meta_us=new MetaUsRangeFinder(us);
-    getFrameworkManager()->AddDeviceToLog(us);
+  us = (UsRangeFinder *)inUs;
+  meta_us = new MetaUsRangeFinder(us);
+  getFrameworkManager()->AddDeviceToLog(us);
 }
 
 void Uav::SetAhrs(const Ahrs *inAhrs) {
-    ahrs=(Ahrs*)inAhrs;
-    imu=(Imu*)ahrs->GetImu();
-    getFrameworkManager()->AddDeviceToLog(imu);
+  ahrs = (Ahrs *)inAhrs;
+  imu = (Imu *)ahrs->GetImu();
+  getFrameworkManager()->AddDeviceToLog(imu);
 }
 
-void Uav::SetBldc(const Bldc* inBldc) {
-    bldc=(Bldc*)inBldc;
-}
+void Uav::SetBldc(const Bldc *inBldc) { bldc = (Bldc *)inBldc; }
 
-void Uav::SetBatteryMonitor(const BatteryMonitor* inBattery) {
-    battery=(BatteryMonitor*)inBattery;
+void Uav::SetBatteryMonitor(const BatteryMonitor *inBattery) {
+  battery = (BatteryMonitor *)inBattery;
 }
 
 void Uav::SetMultiplex(const UavMultiplex *inMultiplex) {
-    multiplex=(UavMultiplex*)inMultiplex;
-    getFrameworkManager()->AddDeviceToLog(multiplex);
+  multiplex = (UavMultiplex *)inMultiplex;
+  getFrameworkManager()->AddDeviceToLog(multiplex);
 }
 
-void Uav::SetVerticalCamera(const Camera* inVerticalCamera) {
-    verticalCamera=(Camera*)inVerticalCamera;
+void Uav::SetVerticalCamera(const Camera *inVerticalCamera) {
+  verticalCamera = (Camera *)inVerticalCamera;
 }
 /*
-void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int VRPNSerialObjectId) {
+void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int
+VRPNSerialObjectId) {
     vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",vrpn_port,10000,80);
     uav_vrpn=new MetaVrpnObject(vrpnclient,name,VRPNSerialObjectId);
@@ -87,102 +87,105 @@
 */
 void Uav::SetupVRPNAutoIP(string name) {
-    SetupVRPN("192.168.147.197:3883",name);
+  SetupVRPN("192.168.147.197:3883", name);
 }
 
-void Uav::SetupVRPN(string optitrack_address,string name) {
-    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",optitrack_address,10000,80);
-    uav_vrpn=new MetaVrpnObject(vrpnclient,name);
+void Uav::SetupVRPN(string optitrack_address, string name) {
+  vrpnclient = new VrpnClient(getFrameworkManager(), "vrpn", optitrack_address,
+                              10000, 80);
+  uav_vrpn = new MetaVrpnObject(vrpnclient, name);
 
-    getFrameworkManager()->AddDeviceToLog(uav_vrpn);
+  getFrameworkManager()->AddDeviceToLog(uav_vrpn);
 
-    GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),DataPlot::Green);
-    //GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
-    //GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
+  GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),
+                                 DataPlot::Green);
+  // GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
+  // GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
 }
 
-void Uav::StartSensors(void)  {
-    if(vrpnclient!=NULL) {
-        vrpnclient->Start();
-    }
+void Uav::StartSensors(void) {
+  if (vrpnclient != NULL) {
+    vrpnclient->Start();
+  }
 }
 void Uav::UseDefaultPlot(void) {
-    multiplex->UseDefaultPlot();
+  multiplex->UseDefaultPlot();
 
-    if(bldc->HasSpeedMeasurement()) {
-        Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Speeds");
-        DataPlot1D* plots[4];
-        plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,7000);
-        plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,7000);
-        plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,7000);
-        plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,7000);
+  if (bldc->HasSpeedMeasurement()) {
+    Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Speeds");
+    DataPlot1D *plots[4];
+    plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 7000);
+    plots[1] =
+        new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 7000);
+    plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 7000);
+    plots[3] =
+        new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 7000);
 
-        if(bldc->MotorsCount()==8) {
-            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0),DataPlot::Red,"top");
-            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),0),DataPlot::Blue,"bottom");
-        } else {
-            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0));
-        }
+    if (bldc->MotorsCount() == 8) {
+      for (int i = 0; i < 4; i++)
+        plots[i]->AddCurve(
+            bldc->Output()->Element(multiplex->MultiplexValue(i), 0),
+            DataPlot::Red, "top");
+      for (int i = 0; i < 4; i++)
+        plots[i]->AddCurve(
+            bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 0),
+            DataPlot::Blue, "bottom");
+    } else {
+      for (int i = 0; i < 4; i++)
+        plots[i]->AddCurve(
+            bldc->Output()->Element(multiplex->MultiplexValue(i), 0));
     }
+  }
 
-    if(bldc->HasCurrentMeasurement()) {
-        Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Currents");
-        DataPlot1D* plots[4];
-        plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,10);
-        plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,10);
-        plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,10);
-        plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,10);
+  if (bldc->HasCurrentMeasurement()) {
+    Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Currents");
+    DataPlot1D *plots[4];
+    plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 10);
+    plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 10);
+    plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 10);
+    plots[3] = new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 10);
 
-        if(bldc->MotorsCount()==8) {
-            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1),DataPlot::Red,"top");
-            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),1),DataPlot::Blue,"bottom");
-        } else {
-            for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1));
-        }
+    if (bldc->MotorsCount() == 8) {
+      for (int i = 0; i < 4; i++)
+        plots[i]->AddCurve(
+            bldc->Output()->Element(multiplex->MultiplexValue(i), 1),
+            DataPlot::Red, "top");
+      for (int i = 0; i < 4; i++)
+        plots[i]->AddCurve(
+            bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 1),
+            DataPlot::Blue, "bottom");
+    } else {
+      for (int i = 0; i < 4; i++)
+        plots[i]->AddCurve(
+            bldc->Output()->Element(multiplex->MultiplexValue(i), 1));
     }
+  }
 
-    meta_us->UseDefaultPlot();
-    ahrs->UseDefaultPlot();
+  meta_us->UseDefaultPlot();
+  ahrs->UseDefaultPlot();
 }
 
-UavMultiplex* Uav::GetUavMultiplex(void) const {
-    return multiplex;
+UavMultiplex *Uav::GetUavMultiplex(void) const { return multiplex; }
+
+Bldc *Uav::GetBldc(void) const { return bldc; }
+
+Ahrs *Uav::GetAhrs(void) const { return ahrs; }
+
+Imu *Uav::GetImu(void) const { return imu; }
+
+MetaUsRangeFinder *Uav::GetMetaUsRangeFinder(void) const { return meta_us; }
+
+UsRangeFinder *Uav::GetUsRangeFinder(void) const { return us; }
+
+BatteryMonitor *Uav::GetBatteryMonitor(void) const { return battery; }
+
+VrpnClient *Uav::GetVrpnClient(void) const {
+  if (vrpnclient == NULL)
+    Err("vrpn is not setup, call SetupVRPN before\n");
+  return vrpnclient;
 }
 
-Bldc* Uav::GetBldc(void) const {
-    return bldc;
-}
+MetaVrpnObject *Uav::GetVrpnObject(void) const { return uav_vrpn; }
 
-Ahrs* Uav::GetAhrs(void) const {
-    return ahrs;
-}
-
-Imu* Uav::GetImu(void) const {
-    return imu;
-}
-
-MetaUsRangeFinder* Uav::GetMetaUsRangeFinder(void) const {
-    return meta_us;
-}
-
-UsRangeFinder* Uav::GetUsRangeFinder(void) const {
-    return us;
-}
-
-BatteryMonitor* Uav::GetBatteryMonitor(void) const {
-    return battery;
-}
-
-VrpnClient* Uav::GetVrpnClient(void) const {
-    if(vrpnclient==NULL) Err("vrpn is not setup, call SetupVRPN before\n");
-    return vrpnclient;
-}
-
-MetaVrpnObject* Uav::GetVrpnObject(void) const {
-    return uav_vrpn;
-}
-
-Camera* Uav::GetVerticalCamera(void) const {
-    return verticalCamera;
-}
+Camera *Uav::GetVerticalCamera(void) const { return verticalCamera; }
 
 } // end namespace meta
Index: trunk/lib/FlairMeta/src/Uav.h
===================================================================
--- trunk/lib/FlairMeta/src/Uav.h	(revision 10)
+++ trunk/lib/FlairMeta/src/Uav.h	(revision 15)
@@ -17,84 +17,86 @@
 
 namespace flair {
-    namespace core {
-        class FrameworkManager;
-    }
-    namespace filter {
-        class Ahrs;
-        class UavMultiplex;
-    }
-    namespace actuator {
-        class Bldc;
-    }
-    namespace sensor {
-        class UsRangeFinder;
-        class BatteryMonitor;
-        class VrpnClient;
-        class Imu;
-        class Camera;
-    }
+namespace core {
+class FrameworkManager;
+}
+namespace filter {
+class Ahrs;
+class UavMultiplex;
+}
+namespace actuator {
+class Bldc;
+}
+namespace sensor {
+class UsRangeFinder;
+class BatteryMonitor;
+class VrpnClient;
+class Imu;
+class Camera;
+}
 }
 
 namespace flair {
 namespace meta {
-    class MetaUsRangeFinder;
-    class MetaVrpnObject;
+class MetaUsRangeFinder;
+class MetaVrpnObject;
 
-    /*! \class Uav
-    *
-    * \brief Base class to construct sensors/actuators depending on uav type
-    */
-    class Uav: public core::Object {
-        public:
-        /*
-            typedef enum {
-                None,
-                Auto,//rt serial if hds x4 ou x8, auto ip sinon
-                AutoIP,
-                UserDefinedIP,
-                AutoSerialPort,
-                } VRPNType_t;
+/*! \class Uav
+*
+* \brief Base class to construct sensors/actuators depending on uav type
 */
-            //uav_name: for optitrack
-            Uav(core::FrameworkManager* parent,std::string name,filter::UavMultiplex *multiplex=NULL);
-            ~Uav();
-            void SetupVRPN(std::string optitrack_address,std::string name);
-            //vrpn serial: broken, need to add serial port in uav specific code
-            //void SetupVRPNSerial(core::SerialPort *vrpn_port,std::string name,int VRPNSerialObjectId);
-            virtual void SetupVRPNAutoIP(std::string name);
+class Uav : public core::Object {
+public:
+  /*
+      typedef enum {
+          None,
+          Auto,//rt serial if hds x4 ou x8, auto ip sinon
+          AutoIP,
+          UserDefinedIP,
+          AutoSerialPort,
+          } VRPNType_t;
+*/
+  // uav_name: for optitrack
+  Uav(core::FrameworkManager *parent, std::string name,
+      filter::UavMultiplex *multiplex = NULL);
+  ~Uav();
+  void SetupVRPN(std::string optitrack_address, std::string name);
+  // vrpn serial: broken, need to add serial port in uav specific code
+  // void SetupVRPNSerial(core::SerialPort *vrpn_port,std::string name,int
+  // VRPNSerialObjectId);
+  virtual void SetupVRPNAutoIP(std::string name);
 
-            virtual void StartSensors(void);
-            void UseDefaultPlot(void);
-            actuator::Bldc* GetBldc(void) const;
-            filter::UavMultiplex* GetUavMultiplex(void) const;
-            sensor::Imu* GetImu(void) const;
-            filter::Ahrs* GetAhrs(void) const;
-            MetaUsRangeFinder* GetMetaUsRangeFinder(void) const;
-            sensor::UsRangeFinder* GetUsRangeFinder(void) const;
-            sensor::BatteryMonitor* GetBatteryMonitor(void) const;
-            sensor::VrpnClient* GetVrpnClient(void) const;
-            meta::MetaVrpnObject* GetVrpnObject(void) const;
-            sensor::Camera* GetVerticalCamera(void) const;
+  virtual void StartSensors(void);
+  void UseDefaultPlot(void);
+  actuator::Bldc *GetBldc(void) const;
+  filter::UavMultiplex *GetUavMultiplex(void) const;
+  sensor::Imu *GetImu(void) const;
+  filter::Ahrs *GetAhrs(void) const;
+  MetaUsRangeFinder *GetMetaUsRangeFinder(void) const;
+  sensor::UsRangeFinder *GetUsRangeFinder(void) const;
+  sensor::BatteryMonitor *GetBatteryMonitor(void) const;
+  sensor::VrpnClient *GetVrpnClient(void) const;
+  meta::MetaVrpnObject *GetVrpnObject(void) const;
+  sensor::Camera *GetVerticalCamera(void) const;
 
-        protected:
-            void SetBldc(const actuator::Bldc *bldc);
-            void SetMultiplex(const filter::UavMultiplex *multiplex);
-            void SetAhrs(const filter::Ahrs *ahrs);
-            void SetUsRangeFinder(const sensor::UsRangeFinder *us);
-            void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
-            void SetVerticalCamera(const sensor::Camera *verticalCamera);
+protected:
+  void SetBldc(const actuator::Bldc *bldc);
+  void SetMultiplex(const filter::UavMultiplex *multiplex);
+  void SetAhrs(const filter::Ahrs *ahrs);
+  void SetUsRangeFinder(const sensor::UsRangeFinder *us);
+  void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
+  void SetVerticalCamera(const sensor::Camera *verticalCamera);
 
-        private:
-            sensor::Imu* imu;
-            filter::Ahrs* ahrs;
-            actuator::Bldc* bldc;
-            filter::UavMultiplex *multiplex;
-            sensor::UsRangeFinder* us;
-            MetaUsRangeFinder* meta_us;
-            sensor::VrpnClient *vrpnclient;
-            MetaVrpnObject* uav_vrpn;
-            sensor::BatteryMonitor* battery;
-            sensor::Camera* verticalCamera;
-    };
+private:
+  sensor::Imu *imu;
+  filter::Ahrs *ahrs;
+  actuator::Bldc *bldc;
+  filter::UavMultiplex *multiplex;
+  sensor::UsRangeFinder *us;
+  MetaUsRangeFinder *meta_us;
+  sensor::VrpnClient *vrpnclient;
+  MetaVrpnObject *uav_vrpn;
+  sensor::BatteryMonitor *battery;
+  sensor::Camera *verticalCamera;
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/UavFactory.cpp
===================================================================
--- trunk/lib/FlairMeta/src/UavFactory.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/UavFactory.cpp	(revision 15)
@@ -29,32 +29,33 @@
 using namespace flair::meta;
 
-Uav* CreateUav(FrameworkManager* parent,string uav_name,string uav_type,UavMultiplex *multiplex) {
-    /*if(uav_type=="ardrone2") {
+Uav *CreateUav(FrameworkManager *parent, string uav_name, string uav_type,
+               UavMultiplex *multiplex) {
+  /*if(uav_type=="ardrone2") {
         return new ArDrone2(parent,uav_name,multiplex);
-    } else */if(uav_type=="hds_x4") {
-        parent->Err("UAV type %s not yet implemented\n",uav_type.c_str());
-        return NULL;
-    } else if(uav_type=="hds_x8") {
-        return new HdsX8(parent,uav_name,multiplex);
-    } else if(uav_type=="xair") {
-        return new XAir(parent,uav_name,multiplex);
-    } else if(uav_type=="hds_xufo") {
-        parent->Err("UAV type %s not yet implemented\n",uav_type.c_str());
-        return NULL;
-    } else if(uav_type.compare(0,7,"x4_simu")==0) {
-        int simu_id=0;
-        if(uav_type.size()>7) {
-            simu_id=atoi(uav_type.substr (7,uav_type.size()-7).c_str());
-        }
-        return new SimuX4(parent,uav_name,simu_id,multiplex);
-    } else if(uav_type.compare(0,7,"x8_simu") == 0) {
-        int simu_id=0;
-        if(uav_type.size()>7) {
-            simu_id=atoi(uav_type.substr (7,uav_type.size()-7).c_str());
-        }
-        return new SimuX8(parent,uav_name,simu_id,multiplex);
-    } else {
-        parent->Err("UAV type %s unknown\n",uav_type.c_str());
-        return NULL;
+    } else */ if (uav_type == "hds_x4") {
+    parent->Err("UAV type %s not yet implemented\n", uav_type.c_str());
+    return NULL;
+  } else if (uav_type == "hds_x8") {
+    return new HdsX8(parent, uav_name, multiplex);
+  } else if (uav_type == "xair") {
+    return new XAir(parent, uav_name, multiplex);
+  } else if (uav_type == "hds_xufo") {
+    parent->Err("UAV type %s not yet implemented\n", uav_type.c_str());
+    return NULL;
+  } else if (uav_type.compare(0, 7, "x4_simu") == 0) {
+    int simu_id = 0;
+    if (uav_type.size() > 7) {
+      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
     }
+    return new SimuX4(parent, uav_name, simu_id, multiplex);
+  } else if (uav_type.compare(0, 7, "x8_simu") == 0) {
+    int simu_id = 0;
+    if (uav_type.size() > 7) {
+      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
+    }
+    return new SimuX8(parent, uav_name, simu_id, multiplex);
+  } else {
+    parent->Err("UAV type %s unknown\n", uav_type.c_str());
+    return NULL;
+  }
 }
Index: trunk/lib/FlairMeta/src/UavFactory.h
===================================================================
--- trunk/lib/FlairMeta/src/UavFactory.h	(revision 10)
+++ trunk/lib/FlairMeta/src/UavFactory.h	(revision 15)
@@ -21,5 +21,7 @@
 #include <Uav.h>
 
-flair::meta::Uav* CreateUav(flair::core::FrameworkManager* parent,std::string uav_name,std::string uav_type,flair::filter::UavMultiplex *multiplex=NULL);
+flair::meta::Uav *CreateUav(flair::core::FrameworkManager *parent,
+                            std::string uav_name, std::string uav_type,
+                            flair::filter::UavMultiplex *multiplex = NULL);
 
 #endif // UAVFACTORY
Index: trunk/lib/FlairMeta/src/UavStateMachine.cpp
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 15)
@@ -51,598 +51,658 @@
 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();
-
-    Tab *uavTab=new Tab(getFrameworkManager()->GetTabWidget(),"uav",0);
-    buttonslayout=new GridLayout(uavTab->NewRow(),"buttons");
-    button_kill=new PushButton(buttonslayout->NewRow(),"kill");
-    button_start_log=new PushButton(buttonslayout->NewRow(),"start_log");
-    button_stop_log=new PushButton(buttonslayout->LastRowLastCol(),"stop_log");
-    button_take_off=new PushButton(buttonslayout->NewRow(),"take_off");
-    button_land=new PushButton(buttonslayout->LastRowLastCol(),"land");
-
-    Tab *lawTab=new Tab(getFrameworkManager()->GetTabWidget(),"control laws");
-    TabWidget *tabWidget=new TabWidget(lawTab->NewRow(),"laws");
-    setupLawTab=new Tab(tabWidget,"Setup");
-    graphLawTab=new Tab(tabWidget,"Graphes");
-
-    uRoll=new NestedSat(setupLawTab->At(0,0),"u_roll");
-    uRoll->ConvertSatFromDegToRad();
-    uRoll->UseDefaultPlot(graphLawTab->NewRow());
-
-    uPitch=new NestedSat(setupLawTab->At(0,1),"u_pitch");
-    uPitch->ConvertSatFromDegToRad();
-    uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
-
-    uYaw=new Pid(setupLawTab->At(0,2),"u_yaw");
-    uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
-
-    uZ=new PidThrust(setupLawTab->At(1,2),"u_z");
-    uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
-
-    getFrameworkManager()->AddDeviceToLog(uZ);
-    uZ->AddDeviceToLog(uRoll);
-    uZ->AddDeviceToLog(uPitch);
-    uZ->AddDeviceToLog(uYaw);
-
-    joy=new MetaDualShock3(getFrameworkManager(),"dualshock3",ds3Port,30);
-    uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
-
-    altitudeMode=AltitudeMode_t::Manual;
-    orientationMode=OrientationMode_t::Manual;
-    thrustMode=ThrustMode_t::Default;
-    torqueMode=TorqueMode_t::Default;
-
-    GroupBox* reglagesGroupbox=new GroupBox(uavTab->NewRow(),"takeoff/landing");
-    desiredTakeoffAltitude=new DoubleSpinBox(reglagesGroupbox->NewRow(),"desired takeoff altitude"," m",0,5,0.1,2,1);
-    desiredLandingAltitude=new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),"desired landing altitude"," m",0,1,0.1,1);
-    altitudeTrajectory=new TrajectoryGenerator1D(uavTab->NewRow(),"alt cons","m");
-    uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(0),DataPlot::Green);
-    uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(1),DataPlot::Green);
-}
-
-UavStateMachine::~UavStateMachine() {
-}
-
-void UavStateMachine::AddDeviceToControlLawLog(const IODevice* device) {
-    uZ->AddDeviceToLog(device);
-}
-
-void UavStateMachine::AddDataToControlLawLog(const core::io_data* data) {
-    uZ->AddDataToLog(data);
-}
-
-const TargetController *UavStateMachine::GetJoystick(void) const {
-    return joy;
-}
+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();
+
+  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
+  buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
+  button_kill = new PushButton(buttonslayout->NewRow(), "kill");
+  button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
+  button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
+  button_take_off = new PushButton(buttonslayout->NewRow(), "take_off");
+  button_land = new PushButton(buttonslayout->LastRowLastCol(), "land");
+
+  Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
+  TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
+  setupLawTab = new Tab(tabWidget, "Setup");
+  graphLawTab = new Tab(tabWidget, "Graphes");
+
+  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
+  uRoll->ConvertSatFromDegToRad();
+  uRoll->UseDefaultPlot(graphLawTab->NewRow());
+
+  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
+  uPitch->ConvertSatFromDegToRad();
+  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
+
+  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
+  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
+
+  uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
+  uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
+
+  getFrameworkManager()->AddDeviceToLog(uZ);
+  uZ->AddDeviceToLog(uRoll);
+  uZ->AddDeviceToLog(uPitch);
+  uZ->AddDeviceToLog(uYaw);
+
+  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
+  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
+
+  altitudeMode = AltitudeMode_t::Manual;
+  orientationMode = OrientationMode_t::Manual;
+  thrustMode = ThrustMode_t::Default;
+  torqueMode = TorqueMode_t::Default;
+
+  GroupBox *reglagesGroupbox =
+      new GroupBox(uavTab->NewRow(), "takeoff/landing");
+  desiredTakeoffAltitude =
+      new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
+                        " m", 0, 5, 0.1, 2, 1);
+  desiredLandingAltitude =
+      new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
+                        "desired landing altitude", " m", 0, 1, 0.1, 1);
+  altitudeTrajectory =
+      new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
+  uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
+      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
+  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
+      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
+}
+
+UavStateMachine::~UavStateMachine() {}
+
+void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
+  uZ->AddDeviceToLog(device);
+}
+
+void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
+  uZ->AddDataToLog(data);
+}
+
+const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
 
 const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
-            return currentQuaternion;
+  return currentQuaternion;
 }
 
 const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
-    return currentAngularSpeed;
-}
-
-const Uav *UavStateMachine::GetUav(void) const {
-    return uav;
-}
-
-void UavStateMachine::AltitudeValues(float &altitude,float &verticalSpeed) const{
-    FailSafeAltitudeValues(altitude, verticalSpeed);
-}
-
-void UavStateMachine::FailSafeAltitudeValues(float &altitude,float &verticalSpeed) const {
-    altitude=uav->GetMetaUsRangeFinder()->z();
-    verticalSpeed=uav->GetMetaUsRangeFinder()->Vz();
+  return currentAngularSpeed;
+}
+
+const Uav *UavStateMachine::GetUav(void) const { return uav; }
+
+void UavStateMachine::AltitudeValues(float &altitude,
+                                     float &verticalSpeed) const {
+  FailSafeAltitudeValues(altitude, verticalSpeed);
+}
+
+void UavStateMachine::FailSafeAltitudeValues(float &altitude,
+                                             float &verticalSpeed) const {
+  altitude = uav->GetMetaUsRangeFinder()->z();
+  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
 }
 
 void UavStateMachine::Run() {
-    WarnUponSwitches(true);
-    uav->StartSensors();
-
-    if(getFrameworkManager()->ErrorOccured()==true) {
-        SafeStop();
-    }
-
-    while(!ToBeStopped()) {
-        SecurityCheck();
-
-        //get controller inputs
-        CheckJoystick();
-        CheckPushButton();
-
-        if(IsPeriodSet()) {
-            WaitPeriod();
-        } else {
-            WaitUpdate(uav->GetAhrs());
-        }
-        needToComputeDefaultTorques=true;
-        needToComputeDefaultThrust=true;
-
-        SignalEvent(Event_t::EnteringControlLoop);
-
-        ComputeOrientation();
-        ComputeAltitude();
-
-        //compute thrust and torques to apply
-        ComputeTorques();
-        ComputeThrust();//logs are added to uz, so it must be updated at last
-
-        // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set thrust (value between 0 and 1)
-        uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
-        uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
-        uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
-        uav->GetUavMultiplex()->SetThrust(-currentThrust);//on raisonne en negatif sur l'altitude, a revoir avec les equations
-        uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
-        uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
-        uav->GetUavMultiplex()->SetYawTrim(0);
-        uav->GetUavMultiplex()->Update(GetTime());
-    }
-
-    WarnUponSwitches(false);
+  WarnUponSwitches(true);
+  uav->StartSensors();
+
+  if (getFrameworkManager()->ErrorOccured() == true) {
+    SafeStop();
+  }
+
+  while (!ToBeStopped()) {
+    SecurityCheck();
+
+    // get controller inputs
+    CheckJoystick();
+    CheckPushButton();
+
+    if (IsPeriodSet()) {
+      WaitPeriod();
+    } else {
+      WaitUpdate(uav->GetAhrs());
+    }
+    needToComputeDefaultTorques = true;
+    needToComputeDefaultThrust = true;
+
+    SignalEvent(Event_t::EnteringControlLoop);
+
+    ComputeOrientation();
+    ComputeAltitude();
+
+    // compute thrust and torques to apply
+    ComputeTorques();
+    ComputeThrust(); // logs are added to uz, so it must be updated at last
+
+    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
+    // thrust (value between 0 and 1)
+    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
+    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
+    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
+    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
+                                                       // sur l'altitude, a
+                                                       // revoir avec les
+                                                       // equations
+    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
+    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
+    uav->GetUavMultiplex()->SetYawTrim(0);
+    uav->GetUavMultiplex()->Update(GetTime());
+  }
+
+  WarnUponSwitches(false);
 }
 
 void UavStateMachine::ComputeOrientation(void) {
-    if (failSafeMode) {
-        GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
+  if (failSafeMode) {
+    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
+                                                          currentAngularSpeed);
+  } else {
+    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
+                                                   currentAngularSpeed);
+  }
+}
+
+const AhrsData *UavStateMachine::GetOrientation(void) const {
+  return GetDefaultOrientation();
+}
+
+const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
+  return uav->GetAhrs()->GetDatas();
+}
+
+void UavStateMachine::ComputeAltitude(void) {
+  if (failSafeMode) {
+    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
+  } else {
+    AltitudeValues(currentAltitude, currentVerticalSpeed);
+  }
+}
+
+void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
+                                               float &refVerticalVelocity) {
+  if (altitudeMode == AltitudeMode_t::Manual) {
+    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
+  } else {
+    GetReferenceAltitude(refAltitude, refVerticalVelocity);
+  }
+}
+
+void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
+                                                  float &refVerticalVelocity) {
+  float zc, dzc;
+
+  switch (altitudeState) {
+  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
+  // until we detect a take off of 0.03m (hard coded value) above the ground.
+  case AltitudeState_t::TakingOff: {
+    if (currentAltitude > groundAltitude + 0.03) {
+      altitudeTrajectory->StartTraj(currentAltitude,
+                                    desiredTakeoffAltitude->Value());
+      altitudeState = AltitudeState_t::Stabilized;
+      SignalEvent(Event_t::Stabilized);
+    }
+    break;
+  }
+  // landing, only check if we reach desired landing altitude
+  case AltitudeState_t::StartLanding: {
+    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
+      // 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
+      // deep and narrow hole right in the sensor line of sight). That's why we
+      // have a 2 phases landing strategy.
+      altitudeState = AltitudeState_t::FinishLanding;
+      SignalEvent(Event_t::FinishLanding);
+      joy->SetLedOFF(1); // DualShock3::led1
+    }
+  }
+  // stabilized: check if z trajectory is finished
+  case AltitudeState_t::Stabilized: {
+    if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
+      SignalEvent(Event_t::ZTrajectoryFinished);
+      flagZTrajectoryFinished = true;
+    }
+    if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
+      flagZTrajectoryFinished = false;
+      altitudeTrajectory->StartTraj(currentAltitude,
+                                    desiredTakeoffAltitude->Value());
+      joy->SetZRef(0);
+    }
+  }
+  }
+
+  // Récupère les consignes (du joystick dans l'implémentation par défaut). La
+  // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
+  // la manette
+  zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
+                    // generator
+  dzc = joy->DzRef();
+
+  // z control law
+  altitudeTrajectory->SetPositionOffset(zc);
+  altitudeTrajectory->SetSpeedOffset(dzc);
+
+  altitudeTrajectory->Update(GetTime());
+  refAltitude = altitudeTrajectory->Position();
+  refVerticalVelocity = altitudeTrajectory->Speed();
+}
+
+void UavStateMachine::GetReferenceAltitude(float &refAltitude,
+                                           float &refVerticalVelocity) {
+  Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
+               "switching back to safe mode\n");
+  EnterFailSafeMode();
+};
+
+void UavStateMachine::ComputeThrust(void) {
+  if (altitudeMode == AltitudeMode_t::Manual) {
+    currentThrust = ComputeDefaultThrust();
+  } else {
+    currentThrust = ComputeCustomThrust();
+  }
+}
+
+float UavStateMachine::ComputeDefaultThrust(void) {
+  if (needToComputeDefaultThrust) {
+    // compute desired altitude
+    float refAltitude, refVerticalVelocity;
+    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
+
+    switch (altitudeState) {
+    case AltitudeState_t::TakingOff: {
+      // The progressive increase in motor speed is used to evaluate the motor
+      // speed that compensate the uav weight. This value
+      // will be used as an offset for altitude control afterwards
+      uZ->OffsetStepUp();
+      break;
+    }
+    case AltitudeState_t::StartLanding:
+    case AltitudeState_t::Stabilized: {
+      float p_error = currentAltitude - refAltitude;
+      float d_error = currentVerticalSpeed - refVerticalVelocity;
+      uZ->SetValues(p_error, d_error);
+      break;
+    }
+    // decrease motor speed in open loop until value offset_g , uav should have
+    // already landed or be very close to at this point
+    case AltitudeState_t::FinishLanding: {
+      if (uZ->OffsetStepDown() == false) {
+        StopMotors();
+      }
+      break;
+    }
+    }
+    uZ->Update(GetTime());
+
+    savedDefaultThrust = uZ->Output();
+    needToComputeDefaultThrust = false;
+  }
+
+  return savedDefaultThrust;
+}
+
+float UavStateMachine::ComputeCustomThrust(void) {
+  Thread::Warn("Default GetThrust method is not overloaded => switching back "
+               "to safe mode\n");
+  EnterFailSafeMode();
+  return ComputeDefaultThrust();
+}
+
+const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
+  if (orientationMode == OrientationMode_t::Manual) {
+    return GetDefaultReferenceOrientation();
+  } else {
+    return GetReferenceOrientation();
+  }
+}
+
+const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
+  // We directly control yaw, pitch, roll angles
+  return joy->GetReferenceOrientation();
+}
+
+const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
+  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
+               "switching back to safe mode\n");
+  EnterFailSafeMode();
+  return GetDefaultReferenceOrientation();
+}
+
+void UavStateMachine::ComputeTorques(void) {
+  if (torqueMode == TorqueMode_t::Default) {
+    ComputeDefaultTorques(currentTorques);
+  } else {
+    ComputeCustomTorques(currentTorques);
+  }
+}
+
+void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
+  if (needToComputeDefaultTorques) {
+    const AhrsData *refOrientation = ComputeReferenceOrientation();
+    Quaternion refQuaternion;
+    Vector3D refAngularRates;
+    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
+                                                 refAngularRates);
+    Euler refAngles = refQuaternion.ToEuler();
+    Euler currentAngles = currentQuaternion.ToEuler();
+
+    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
+                    currentAngularSpeed.z - refAngularRates.z);
+    uYaw->Update(GetTime());
+    torques.yaw = uYaw->Output();
+
+    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
+                      currentAngularSpeed.y);
+    uPitch->Update(GetTime());
+    torques.pitch = uPitch->Output();
+
+    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
+    uRoll->Update(GetTime());
+    torques.roll = uRoll->Output();
+
+    savedDefaultTorques = torques;
+    needToComputeDefaultTorques = false;
+  } else {
+    torques = savedDefaultTorques;
+  }
+}
+
+void UavStateMachine::ComputeCustomTorques(Euler &torques) {
+  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
+               "switching back to safe mode\n");
+  EnterFailSafeMode();
+  ComputeDefaultTorques(torques);
+}
+
+void UavStateMachine::TakeOff(void) {
+  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);
+
+    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
+                     // valeur initiale (station sol)
+
+    uav->GetUavMultiplex()->LockUserInterface();
+    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
+    // consignes moteurs
+    // sans les faire tourner effectivement (en déplaçant à la main le drone)
+    uav->GetBldc()->SetEnabled(true);
+    groundAltitude = currentAltitude;
+    altitudeState = AltitudeState_t::TakingOff;
+
+    SignalEvent(Event_t::TakingOff);
+  } else {
+    joy->ErrorNotify();
+  }
+}
+
+void UavStateMachine::Land(void) {
+  if (altitudeMode != AltitudeMode_t::Manual) {
+    SetAltitudeMode(AltitudeMode_t::Manual);
+  }
+  if (altitudeState == AltitudeState_t::Stabilized) {
+    joy->SetLedOFF(4); // DualShock3::led4
+    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();
+  }
+}
+
+void UavStateMachine::SignalEvent(Event_t event) {
+  switch (event) {
+  case Event_t::StartLanding:
+    Thread::Info("Altitude: entering 'StartLanding' state\n");
+    break;
+  case Event_t::Stopped:
+    Thread::Info("Altitude: entering 'Stopped' state\n");
+    break;
+  case Event_t::TakingOff:
+    Thread::Info("Altitude: taking off\n");
+    break;
+  case Event_t::Stabilized:
+    Thread::Info("Altitude: entering 'Stabilized' state\n");
+    break;
+  case Event_t::FinishLanding:
+    Thread::Info("Altitude: entering 'FinishLanding' state\n");
+    break;
+  case Event_t::EmergencyStop:
+    Thread::Info("Emergency stop!\n");
+    break;
+  }
+}
+
+void UavStateMachine::EmergencyStop(void) {
+  if (altitudeState != AltitudeState_t::Stopped) {
+    StopMotors();
+    EnterFailSafeMode();
+    joy->Rumble(0x70);
+    SignalEvent(Event_t::EmergencyStop);
+  }
+}
+
+void UavStateMachine::StopMotors(void) {
+  joy->FlashLed(1, 10, 10); // DualShock3::led1
+  uav->GetBldc()->SetEnabled(false);
+  uav->GetUavMultiplex()->UnlockUserInterface();
+  SignalEvent(Event_t::Stopped);
+  altitudeState = AltitudeState_t::Stopped;
+  uav->GetAhrs()->UnlockUserInterface();
+
+  uZ->SetValues(0, 0);
+  uZ->Reset();
+}
+
+GridLayout *UavStateMachine::GetButtonsLayout(void) const {
+  return buttonslayout;
+}
+
+void UavStateMachine::SecurityCheck(void) {
+  MandatorySecurityCheck();
+  ExtraSecurityCheck();
+}
+
+void UavStateMachine::MandatorySecurityCheck(void) {
+  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
+    flagConnectionLost = true;
+    Thread::Err("Connection lost\n");
+    EnterFailSafeMode();
+    if (altitudeState == AltitudeState_t::Stopped) {
+      SafeStop();
     } else {
-        GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
-    }
-}
-
-const AhrsData *UavStateMachine::GetOrientation(void) const {
-    return GetDefaultOrientation();
-}
-
-const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
-    return uav->GetAhrs()->GetDatas();
-}
-
-void UavStateMachine::ComputeAltitude(void) {
-    if (failSafeMode) {
-        FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
-    } else {
-        AltitudeValues(currentAltitude,currentVerticalSpeed);
-    }
-}
-
-void UavStateMachine::ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
-    if (altitudeMode==AltitudeMode_t::Manual) {
-        GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
-    } else {
-        GetReferenceAltitude(refAltitude, refVerticalVelocity);
-    }
-}
-
-void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
-    float zc,dzc;
-
-    switch(altitudeState) {
-        //initiate a takeoff: increase motor speed in open loop (see ComputeThrust) until we detect a take off of 0.03m (hard coded value) above the ground.
-        case AltitudeState_t::TakingOff: {
-            if(currentAltitude>groundAltitude+0.03) {
-                altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
-                altitudeState=AltitudeState_t::Stabilized;
-                SignalEvent(Event_t::Stabilized);
-            }
-            break;
-        }
-        //landing, only check if we reach desired landing altitude
-        case AltitudeState_t::StartLanding: {
-            if(altitudeTrajectory->Position()==desiredLandingAltitude->Value()) {
-                //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
-                // deep and narrow hole right in the sensor line of sight). That's why we have a 2 phases landing strategy.
-                altitudeState=AltitudeState_t::FinishLanding;
-                SignalEvent(Event_t::FinishLanding);
-                joy->SetLedOFF(1);//DualShock3::led1
-            }
-        }
-        //stabilized: check if z trajectory is finished
-        case AltitudeState_t::Stabilized: {
-            if(!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
-                SignalEvent(Event_t::ZTrajectoryFinished);
-                flagZTrajectoryFinished=true;
-            }
-            if(flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
-                flagZTrajectoryFinished=false;
-                altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
-                joy->SetZRef(0);
-            }
-        }
-    }
-
-    //Récupère les consignes (du joystick dans l'implémentation par défaut). La consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par la manette
-    zc=joy->ZRef();//a revoir, la position offset devrait se calculer dans le generator
-    dzc=joy->DzRef();
-
-    //z control law
-    altitudeTrajectory->SetPositionOffset(zc);
-    altitudeTrajectory->SetSpeedOffset(dzc);
-
-    altitudeTrajectory->Update(GetTime());
-    refAltitude=altitudeTrajectory->Position();
-    refVerticalVelocity=altitudeTrajectory->Speed();
-}
-
-void UavStateMachine::GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
-    Thread::Warn("Default GetReferenceAltitude method is not overloaded => switching back to safe mode\n");
+      Land();
+    }
+  }
+  if ((altitudeState == AltitudeState_t::TakingOff ||
+       altitudeState == AltitudeState_t::Stabilized) &&
+      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
+    flagBatteryLow = true;
+    Printf("Battery Low\n");
     EnterFailSafeMode();
-};
-
-void UavStateMachine::ComputeThrust(void) {
-    if (altitudeMode==AltitudeMode_t::Manual) {
-        currentThrust=ComputeDefaultThrust();
-    } else {
-        currentThrust=ComputeCustomThrust();
-    }
-}
-
-float UavStateMachine::ComputeDefaultThrust(void) {
-    if(needToComputeDefaultThrust) {
-        //compute desired altitude
-        float refAltitude, refVerticalVelocity;
-        ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
-
-        switch(altitudeState) {
-            case AltitudeState_t::TakingOff: {
-                //The progressive increase in motor speed is used to evaluate the motor speed that compensate the uav weight. This value
-                //will be used as an offset for altitude control afterwards
-                uZ->OffsetStepUp();
-                break;
-            }
-            case AltitudeState_t::StartLanding:
-            case AltitudeState_t::Stabilized: {
-                float p_error=currentAltitude-refAltitude;
-                float d_error=currentVerticalSpeed-refVerticalVelocity;
-                uZ->SetValues(p_error,d_error);
-                break;
-            }
-            //decrease motor speed in open loop until value offset_g , uav should have already landed or be very close to at this point
-            case AltitudeState_t::FinishLanding: {
-                if(uZ->OffsetStepDown()==false) {
-                    StopMotors();
-                }
-                break;
-            }
-        }
-        uZ->Update(GetTime());
-
-        savedDefaultThrust=uZ->Output();
-        needToComputeDefaultThrust=false;
-    }
-
-    return savedDefaultThrust;
-}
-
-float UavStateMachine::ComputeCustomThrust(void) {
-    Thread::Warn("Default GetThrust method is not overloaded => switching back to safe mode\n");
-    EnterFailSafeMode();
-    return ComputeDefaultThrust();
-}
-
-const AhrsData* UavStateMachine::ComputeReferenceOrientation(void) {
-    if(orientationMode==OrientationMode_t::Manual) {
-        return GetDefaultReferenceOrientation();
-    } else {
-        return GetReferenceOrientation();
-    }
-}
-
-const AhrsData* UavStateMachine::GetDefaultReferenceOrientation(void) const {
-    // We directly control yaw, pitch, roll angles
-    return joy->GetReferenceOrientation();
-}
-
-const AhrsData* UavStateMachine::GetReferenceOrientation(void) {
-    Thread::Warn("Default GetReferenceOrientation method is not overloaded => switching back to safe mode\n");
-    EnterFailSafeMode();
-    return GetDefaultReferenceOrientation();
-}
-
-void UavStateMachine::ComputeTorques(void) {
-    if(torqueMode==TorqueMode_t::Default) {
-        ComputeDefaultTorques(currentTorques);
-    } else {
-        ComputeCustomTorques(currentTorques);
-    }
-}
-
-void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
-    if(needToComputeDefaultTorques) {
-        const AhrsData *refOrientation=ComputeReferenceOrientation();
-        Quaternion refQuaternion;
-        Vector3D refAngularRates;
-        refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
-        Euler refAngles=refQuaternion.ToEuler();
-        Euler currentAngles=currentQuaternion.ToEuler();
-
-        uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),currentAngularSpeed.z-refAngularRates.z);
-        uYaw->Update(GetTime());
-        torques.yaw=uYaw->Output();
-
-        uPitch->SetValues(refAngles.pitch,currentAngles.pitch,currentAngularSpeed.y);
-        uPitch->Update(GetTime());
-        torques.pitch=uPitch->Output();
-
-        uRoll->SetValues(refAngles.roll,currentAngles.roll,currentAngularSpeed.x);
-        uRoll->Update(GetTime());
-        torques.roll=uRoll->Output();
-
-        savedDefaultTorques=torques;
-        needToComputeDefaultTorques=false;
-    } else {
-        torques=savedDefaultTorques;
-    }
-}
-
-void UavStateMachine::ComputeCustomTorques(Euler &torques) {
-    Thread::Warn("Default ComputeCustomTorques method is not overloaded => switching back to safe mode\n");
-    EnterFailSafeMode();
-    ComputeDefaultTorques(torques);
-}
-
-void UavStateMachine::TakeOff(void) {
-    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);
-
-        uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa valeur initiale (station sol)
-
-        uav->GetUavMultiplex()->LockUserInterface();
-        //Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les consignes moteurs
-        // sans les faire tourner effectivement (en déplaçant à la main le drone)
-        uav->GetBldc()->SetEnabled(true);
-        groundAltitude=currentAltitude;
-        altitudeState=AltitudeState_t::TakingOff;
-
-        SignalEvent(Event_t::TakingOff);
-    } else {
+    Land();
+  }
+}
+
+void UavStateMachine::CheckJoystick(void) {
+  GenericCheckJoystick();
+  ExtraCheckJoystick();
+}
+
+void UavStateMachine::GenericCheckJoystick(void) {
+  static bool isEmergencyStopButtonPressed = false;
+  static bool isTakeOffButtonPressed = false;
+  static bool isSafeModeButtonPressed = false;
+
+  if (joy->IsButtonPressed(1)) { // select
+    if (!isEmergencyStopButtonPressed) {
+      isEmergencyStopButtonPressed = true;
+      Thread::Info("Emergency stop from joystick\n");
+      EmergencyStop();
+    }
+  } else
+    isEmergencyStopButtonPressed = false;
+
+  if (joy->IsButtonPressed(0)) { // start
+    if (!isTakeOffButtonPressed) {
+      isTakeOffButtonPressed = true;
+      switch (altitudeState) {
+      case AltitudeState_t::Stopped:
+        TakeOff();
+        break;
+      case AltitudeState_t::Stabilized:
+        Land();
+        break;
+      default:
         joy->ErrorNotify();
-    }
-}
-
-void UavStateMachine::Land(void) {
-    if (altitudeMode!=AltitudeMode_t::Manual) {
-        SetAltitudeMode(AltitudeMode_t::Manual);
-    }
-    if(altitudeState==AltitudeState_t::Stabilized) {
-        joy->SetLedOFF(4);//DualShock3::led4
-        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();
-    }
-}
-
-void UavStateMachine::SignalEvent(Event_t event) {
-    switch(event) {
-    case Event_t::StartLanding:
-        Thread::Info("Altitude: entering 'StartLanding' state\n");
         break;
-    case Event_t::Stopped:
-        Thread::Info("Altitude: entering 'Stopped' state\n");
-        break;
-    case Event_t::TakingOff:
-        Thread::Info("Altitude: taking off\n");
-        break;
-    case Event_t::Stabilized:
-        Thread::Info("Altitude: entering 'Stabilized' state\n");
-        break;
-    case Event_t::FinishLanding:
-        Thread::Info("Altitude: entering 'FinishLanding' state\n");
-        break;
-    case Event_t::EmergencyStop:
-        Thread::Info("Emergency stop!\n");
-        break;
-    }
-}
-
-void UavStateMachine::EmergencyStop(void) {
-    if(altitudeState!=AltitudeState_t::Stopped) {
-        StopMotors();
-        EnterFailSafeMode();
-        joy->Rumble(0x70);
-        SignalEvent(Event_t::EmergencyStop);
-    }
-}
-
-void UavStateMachine::StopMotors(void)
-{
-    joy->FlashLed(1,10,10);//DualShock3::led1
-    uav->GetBldc()->SetEnabled(false);
-    uav->GetUavMultiplex()->UnlockUserInterface();
-    SignalEvent(Event_t::Stopped);
-    altitudeState=AltitudeState_t::Stopped;
-    uav->GetAhrs()->UnlockUserInterface();
-
-    uZ->SetValues(0,0);
-    uZ->Reset();
-}
-
-GridLayout* UavStateMachine::GetButtonsLayout(void) const {
-    return buttonslayout;
-}
-
-void UavStateMachine::SecurityCheck(void) {
-    MandatorySecurityCheck();
-    ExtraSecurityCheck();
-}
-
-void UavStateMachine::MandatorySecurityCheck(void) {
-    if(getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
-        flagConnectionLost=true;
-        Thread::Err("Connection lost\n");
-        EnterFailSafeMode();
-        if(altitudeState==AltitudeState_t::Stopped) {
-            SafeStop();
-        } else {
-            Land();
-        }
-    }
-    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
-        flagBatteryLow=true;
-        Printf("Battery Low\n");
-        EnterFailSafeMode();
-        Land();
-    }
-}
-
-void UavStateMachine::CheckJoystick(void) {
-    GenericCheckJoystick();
-    ExtraCheckJoystick();
-}
-
-void UavStateMachine::GenericCheckJoystick(void) {
-    static bool isEmergencyStopButtonPressed=false;
-    static bool isTakeOffButtonPressed=false;
-    static bool isSafeModeButtonPressed=false;
-
-   if(joy->IsButtonPressed(1)) { //select
-        if (!isEmergencyStopButtonPressed) {
-            isEmergencyStopButtonPressed=true;
-            Thread::Info("Emergency stop from joystick\n");
-            EmergencyStop();
-        }
-    } else isEmergencyStopButtonPressed=false;
-
-    if(joy->IsButtonPressed(0)) { //start
-        if (!isTakeOffButtonPressed) {
-            isTakeOffButtonPressed=true;
-            switch(altitudeState) {
-                case AltitudeState_t::Stopped:
-                    TakeOff();
-                    break;
-                case AltitudeState_t::Stabilized:
-                    Land();
-                    break;
-                default:
-                    joy->ErrorNotify();
-                    break;
-            }
-        }
-    } else isTakeOffButtonPressed=false;
-
-    //cross
-    //gsanahuj:conflict with Majd programs.
-    //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 (!isSafeModeButtonPressed) {
-            isSafeModeButtonPressed=true;
-            Thread::Info("Entering fail safe mode\n");
-            EnterFailSafeMode();
-        }
-    } else isSafeModeButtonPressed=false;
+      }
+    }
+  } else
+    isTakeOffButtonPressed = false;
+
+  // cross
+  // gsanahuj:conflict with Majd programs.
+  // 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 (!isSafeModeButtonPressed) {
+      isSafeModeButtonPressed = true;
+      Thread::Info("Entering fail safe mode\n");
+      EnterFailSafeMode();
+    }
+  } else
+    isSafeModeButtonPressed = false;
 }
 
 void UavStateMachine::CheckPushButton(void) {
-    GenericCheckPushButton();
-    ExtraCheckPushButton();
+  GenericCheckPushButton();
+  ExtraCheckPushButton();
 }
 
 void UavStateMachine::GenericCheckPushButton(void) {
-    if(button_kill->Clicked()==true) SafeStop();
-    if(button_take_off->Clicked()==true) TakeOff();
-    if(button_land->Clicked()==true) Land();
-    if(button_start_log->Clicked()==true) getFrameworkManager()->StartLog();
-    if(button_stop_log->Clicked()==true) getFrameworkManager()->StopLog();
+  if (button_kill->Clicked() == true)
+    SafeStop();
+  if (button_take_off->Clicked() == true)
+    TakeOff();
+  if (button_land->Clicked() == true)
+    Land();
+  if (button_start_log->Clicked() == true)
+    getFrameworkManager()->StartLog();
+  if (button_stop_log->Clicked() == true)
+    getFrameworkManager()->StopLog();
 }
 
 void UavStateMachine::EnterFailSafeMode(void) {
-    SetAltitudeMode(AltitudeMode_t::Manual);
-    SetOrientationMode(OrientationMode_t::Manual);
-    SetThrustMode(ThrustMode_t::Default);
-    SetTorqueMode(TorqueMode_t::Default);
-
-    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
-    joy->SetYawRef(currentQuaternion);
+  SetAltitudeMode(AltitudeMode_t::Manual);
+  SetOrientationMode(OrientationMode_t::Manual);
+  SetThrustMode(ThrustMode_t::Default);
+  SetTorqueMode(TorqueMode_t::Default);
+
+  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
+                                                        currentAngularSpeed);
+  joy->SetYawRef(currentQuaternion);
+  uYaw->Reset();
+  uPitch->Reset();
+  uRoll->Reset();
+
+  failSafeMode = true;
+  SignalEvent(Event_t::EnteringFailSafeMode);
+}
+
+bool UavStateMachine::ExitFailSafeMode(void) {
+  // only exit fail safe mode if in Stabilized altitude state
+  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
+  // le ruban perturbe l'us
+  /*
+    if (altitudeState!=AltitudeState_t::Stabilized) {
+        return false;
+    } else*/ {
+    failSafeMode = false;
+    return true;
+  }
+}
+
+bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
+  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
+    if (!ExitFailSafeMode())
+      return false;
+  }
+  // When transitionning from Custom to Default torque mode, we should reset the
+  // default control laws
+  if ((torqueMode == TorqueMode_t::Custom) &&
+      (newTorqueMode == TorqueMode_t::Default)) {
     uYaw->Reset();
     uPitch->Reset();
     uRoll->Reset();
-
-    failSafeMode=true;
-    SignalEvent(Event_t::EnteringFailSafeMode);
-}
-
-bool UavStateMachine::ExitFailSafeMode(void) {
-    // only exit fail safe mode if in Stabilized altitude state
-    //gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
-    //le ruban perturbe l'us
-    /*
-    if (altitudeState!=AltitudeState_t::Stabilized) {
-        return false;
-    } else*/ {
-        failSafeMode=false;
-        return true;
-    }
-}
-
-bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
-    if ((newTorqueMode==TorqueMode_t::Custom) && (failSafeMode)) {
-        if (!ExitFailSafeMode()) return false;
-    }
-    //When transitionning from Custom to Default torque mode, we should reset the default control laws
-    if ((torqueMode==TorqueMode_t::Custom) && (newTorqueMode==TorqueMode_t::Default)) {
-        uYaw->Reset();
-        uPitch->Reset();
-        uRoll->Reset();
-    }
-    torqueMode=newTorqueMode;
-    return true;
+  }
+  torqueMode = newTorqueMode;
+  return true;
 }
 
 bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
-    if ((newAltitudeMode==AltitudeMode_t::Custom) && (failSafeMode)) {
-        if (!ExitFailSafeMode()) return false;
-    }
-    altitudeMode=newAltitudeMode;
-    GotoAltitude(desiredTakeoffAltitude->Value());
-
-    return true;
+  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
+    if (!ExitFailSafeMode())
+      return false;
+  }
+  altitudeMode = newAltitudeMode;
+  GotoAltitude(desiredTakeoffAltitude->Value());
+
+  return true;
 }
 
 bool UavStateMachine::GotoAltitude(float desiredAltitude) {
-    if (altitudeMode!=AltitudeMode_t::Manual) {
-        return false;
-    }
-    altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),desiredAltitude);
-    return true;
-}
-
-bool UavStateMachine::SetOrientationMode(OrientationMode_t const &newOrientationMode) {
-    if ((newOrientationMode==OrientationMode_t::Custom) && (failSafeMode)) {
-        if (!ExitFailSafeMode()) return false;
-    }
-    //When transitionning from Custom to Manual mode we must reset to yaw reference to the current absolute yaw angle,
-    // overwise the Uav will abruptly change orientation
-    if ((orientationMode==OrientationMode_t::Custom) && (newOrientationMode==OrientationMode_t::Manual)) {
-        joy->SetYawRef(currentQuaternion);
-    }
-    orientationMode=newOrientationMode;
-    return true;
+  if (altitudeMode != AltitudeMode_t::Manual) {
+    return false;
+  }
+  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
+                                desiredAltitude);
+  return true;
+}
+
+bool UavStateMachine::SetOrientationMode(
+    OrientationMode_t const &newOrientationMode) {
+  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
+    if (!ExitFailSafeMode())
+      return false;
+  }
+  // When transitionning from Custom to Manual mode we must reset to yaw
+  // reference to the current absolute yaw angle,
+  // overwise the Uav will abruptly change orientation
+  if ((orientationMode == OrientationMode_t::Custom) &&
+      (newOrientationMode == OrientationMode_t::Manual)) {
+    joy->SetYawRef(currentQuaternion);
+  }
+  orientationMode = newOrientationMode;
+  return true;
 }
 
 bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
-    if ((newThrustMode==ThrustMode_t::Custom) && (failSafeMode)) {
-        if (!ExitFailSafeMode()) return false;
-    }
-    thrustMode=newThrustMode;
-    return true;
-}
+  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
+    if (!ExitFailSafeMode())
+      return false;
+  }
+  thrustMode = newThrustMode;
+  return true;
+}
Index: trunk/lib/FlairMeta/src/UavStateMachine.h
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 10)
+++ trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 15)
@@ -26,281 +26,275 @@
 
 namespace flair {
-    namespace core {
-        class FrameworkManager;
-        class AhrsData;
-        class io_data;
-    }
-    namespace gui {
-        class PushButton;
-        class GridLayout;
-        class Tab;
-        class DoubleSpinBox;
-    }
-    namespace filter {
-        class ControlLaw;
-        class NestedSat;
-        class Pid;
-        class PidThrust;
-        class TrajectoryGenerator1D;
-    }
-    namespace sensor {
-        class TargetController;
-    }
-    namespace meta {
-        class MetaDualShock3;
-        class Uav;
-    }
-}
-
-namespace flair { namespace meta {
-
- /*! \class UavStateMachine
+namespace core {
+class FrameworkManager;
+class AhrsData;
+class io_data;
+}
+namespace gui {
+class PushButton;
+class GridLayout;
+class Tab;
+class DoubleSpinBox;
+}
+namespace filter {
+class ControlLaw;
+class NestedSat;
+class Pid;
+class PidThrust;
+class TrajectoryGenerator1D;
+}
+namespace sensor {
+class TargetController;
+}
+namespace meta {
+class MetaDualShock3;
+class Uav;
+}
+}
+
+namespace flair {
+namespace meta {
+
+/*! \class UavStateMachine
 *
 * \brief State machine for UAV
 *
-* thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
+* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
+*SetPeriodMS
 */
 
-class UavStateMachine : public core::Thread
-{
-    protected:
-        enum class AltitudeMode_t {
-            Manual,
-            Custom
-        };
-        const AltitudeMode_t &GetAltitudeMode(void) const {
-            return altitudeMode;
-        }
-        bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
-
-        //uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
-        //available in mode AltitudeMode_t::Manual
-        //return true if goto is possible
-        bool GotoAltitude(float desiredAltitude);
-
-        enum class OrientationMode_t {
-            Manual,
-            Custom
-        };
-        const OrientationMode_t &GetOrientationMode(void) const {
-            return orientationMode;
-        }
-        bool SetOrientationMode(const OrientationMode_t &orientationMode);
-
-        enum class ThrustMode_t {
-            Default,
-            Custom
-        };
-        const ThrustMode_t &GetThrustMode() const {
-            return thrustMode;
-        }
-        bool SetThrustMode(const ThrustMode_t &thrustMode);
-
-        enum class TorqueMode_t {
-            Default,
-            Custom
-        };
-        const TorqueMode_t &GetTorqueMode(void) const {
-            return torqueMode;
-        }
-        bool SetTorqueMode(const TorqueMode_t &torqueMode);
-
-        enum class Event_t {
-            EnteringFailSafeMode,
-            EnteringControlLoop,
-            StartLanding,
-            FinishLanding,
-            Stopped,
-            TakingOff,
-            EmergencyStop,
-            Stabilized, //as soon as uav is 3cm far from the ground
-            ZTrajectoryFinished,
-        };
-
-        UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
-        ~UavStateMachine();
-
-        const core::Quaternion &GetCurrentQuaternion(void) const;
-
-        const core::Vector3D &GetCurrentAngularSpeed(void) const;
-
-        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);
-
-        virtual const core::AhrsData *GetOrientation(void) const;
-        const core::AhrsData *GetDefaultOrientation(void) const;
-
-        virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
-        void EnterFailSafeMode(void);
-        bool ExitFailSafeMode(void);
-        void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
-
-        gui::GridLayout *GetButtonsLayout(void) const;
-        virtual void ExtraSecurityCheck(void){};
-        virtual void ExtraCheckJoystick(void){};
-        virtual void ExtraCheckPushButton(void){};
-
-        void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
-        virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
-        //float GetDefaultThrustOffset(void);
-        const core::AhrsData *GetDefaultReferenceOrientation(void) const;
-        virtual const core::AhrsData *GetReferenceOrientation(void);
-
-        /*!
-        * \brief Compute Custom Torques
-        *
-        * Reimplement this method to use TorqueMode_t::Custom. \n
-        * This method is called internally by UavStateMachine, do not call it by yourself. \n
-        * See GetTorques if you need torques values.
-        *
-        * \param torques custom torques
-        */
-        virtual void ComputeCustomTorques(core::Euler &torques);
-
-        /*!
-        * \brief Compute Default Torques
-        *
-        * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
-        * Torques are only computed once by loop, other calls to this method will use previously computed torques.
-        *
-        * \param torques default torques
-        */
-        void ComputeDefaultTorques(core::Euler &torques);
-
-        /*!
-        * \brief Get Torques
-        *
-        * \return torques current torques
-        */
-        //const core::Euler &GetTorques() const;
-
-        /*!
-        * \brief Compute Custom Thrust
-        *
-        * Reimplement this method to use ThrustMode_t::Custom. \n
-        * This method is called internally by UavStateMachine, do not call it by yourself. \n
-        * See GetThrust if you need thrust value.
-        *
-        * \return custom Thrust
-        */
-        virtual float ComputeCustomThrust(void);
-
-        /*!
-        * \brief Compute Default Thrust
-        *
-        * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
-        * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
-        *
-        * \return default thrust
-        */
-        float ComputeDefaultThrust(void);
-
-        /*!
-        * \brief Get Thrust
-        *
-        * \return current thrust
-        */
-        //float GetThrust() const;
-
-        /*!
-        * \brief Add an IODevice to the control law logs
-        *
-        * The IODevice will be automatically logged among the Uz logs,
-        * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
-        * and FrameworkManager::AddDeviceToLog). \n
-        *
-        * \param device IODevice to log
-        */
-        void AddDeviceToControlLawLog(const core::IODevice* device);
-
-        /*!
-        * \brief Add an io_data to the control law logs
-        *
-        * The io_data will be automatically logged among the Uz logs,
-        * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
-        * and FrameworkManager::AddDeviceToLog). \n
-        *
-        * \param data io_data to log
-        */
-        void AddDataToControlLawLog(const core::io_data* data);
-
-        const sensor::TargetController *GetJoystick(void) const;
-
-        gui::Tab *setupLawTab,*graphLawTab;
-
-    private:
-        /*!
-        \enum AltitudeState_t
-        \brief States of the altitude state machine
-        */
-        enum class AltitudeState_t {
-            Stopped,        /*!< the uav motors are stopped */
-            TakingOff,      /*!< take off initiated. Motors accelerate progressively until the UAV lift up */
-            Stabilized,     /*!< the uav is actively maintaining its altitude */
-            StartLanding,   /*!< landing initiated. Altitude is required to reach the landing altitude (0 by default) */
-            FinishLanding   /*!< motors are gradually stopped */
-        };
-        AltitudeState_t altitudeState;
-        void ProcessAltitudeFiniteStateMachine();
-        void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
-
-
-        float groundAltitude; // effective altitude when the uav leaves the ground
-        float currentAltitude,currentVerticalSpeed;
-
-        bool failSafeMode;
-        void SecurityCheck(void);
-        void MandatorySecurityCheck(void);
-        void CheckJoystick();
-        void GenericCheckJoystick();
-        void CheckPushButton(void);
-        void GenericCheckPushButton(void);
-        void Run(void);
-        void StopMotors(void);
-
-        meta::Uav *uav;
-
-        core::Quaternion currentQuaternion;
-        core::Vector3D currentAngularSpeed;
-
-        const core::AhrsData *ComputeReferenceOrientation(void);
-
-        void ComputeOrientation(void);
-        void ComputeAltitude(void);
-
-        void ComputeTorques(void);
-        core::Euler currentTorques,savedDefaultTorques;
-        bool needToComputeDefaultTorques;
-
-        void ComputeThrust(void);
-        float currentThrust,savedDefaultThrust;
-        bool needToComputeDefaultThrust;
-
-        gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
-        gui::GridLayout *buttonslayout;
-        gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
-        AltitudeMode_t altitudeMode;
-        OrientationMode_t orientationMode;
-        ThrustMode_t thrustMode;
-        TorqueMode_t torqueMode;
-        bool flagBatteryLow;
-        bool flagConnectionLost;
-        bool flagZTrajectoryFinished;
-        filter::NestedSat *uRoll,*uPitch;
-        filter::Pid *uYaw;
-        filter::PidThrust *uZ;
-
-        MetaDualShock3 *joy;
-        filter::TrajectoryGenerator1D *altitudeTrajectory;
+class UavStateMachine : public core::Thread {
+protected:
+  enum class AltitudeMode_t { Manual, Custom };
+  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
+  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
+
+  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
+  // available in mode AltitudeMode_t::Manual
+  // return true if goto is possible
+  bool GotoAltitude(float desiredAltitude);
+
+  enum class OrientationMode_t { Manual, Custom };
+  const OrientationMode_t &GetOrientationMode(void) const {
+    return orientationMode;
+  }
+  bool SetOrientationMode(const OrientationMode_t &orientationMode);
+
+  enum class ThrustMode_t { Default, Custom };
+  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
+  bool SetThrustMode(const ThrustMode_t &thrustMode);
+
+  enum class TorqueMode_t { Default, Custom };
+  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
+  bool SetTorqueMode(const TorqueMode_t &torqueMode);
+
+  enum class Event_t {
+    EnteringFailSafeMode,
+    EnteringControlLoop,
+    StartLanding,
+    FinishLanding,
+    Stopped,
+    TakingOff,
+    EmergencyStop,
+    Stabilized, // as soon as uav is 3cm far from the ground
+    ZTrajectoryFinished,
+  };
+
+  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
+  ~UavStateMachine();
+
+  const core::Quaternion &GetCurrentQuaternion(void) const;
+
+  const core::Vector3D &GetCurrentAngularSpeed(void) const;
+
+  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);
+
+  virtual const core::AhrsData *GetOrientation(void) const;
+  const core::AhrsData *GetDefaultOrientation(void) const;
+
+  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
+  void EnterFailSafeMode(void);
+  bool ExitFailSafeMode(void);
+  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
+
+  gui::GridLayout *GetButtonsLayout(void) const;
+  virtual void ExtraSecurityCheck(void){};
+  virtual void ExtraCheckJoystick(void){};
+  virtual void ExtraCheckPushButton(void){};
+
+  void GetDefaultReferenceAltitude(float &refAltitude,
+                                   float &refVerticalVelocity);
+  virtual void GetReferenceAltitude(float &refAltitude,
+                                    float &refVerticalVelocity);
+  // float GetDefaultThrustOffset(void);
+  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
+  virtual const core::AhrsData *GetReferenceOrientation(void);
+
+  /*!
+  * \brief Compute Custom Torques
+  *
+  * Reimplement this method to use TorqueMode_t::Custom. \n
+  * This method is called internally by UavStateMachine, do not call it by
+  *yourself. \n
+  * See GetTorques if you need torques values.
+  *
+  * \param torques custom torques
+  */
+  virtual void ComputeCustomTorques(core::Euler &torques);
+
+  /*!
+  * \brief Compute Default Torques
+  *
+  * This method is called internally by UavStateMachine when using
+  *TorqueMode_t::Default. \n
+  * Torques are only computed once by loop, other calls to this method will use
+  *previously computed torques.
+  *
+  * \param torques default torques
+  */
+  void ComputeDefaultTorques(core::Euler &torques);
+
+  /*!
+  * \brief Get Torques
+  *
+  * \return torques current torques
+  */
+  // const core::Euler &GetTorques() const;
+
+  /*!
+  * \brief Compute Custom Thrust
+  *
+  * Reimplement this method to use ThrustMode_t::Custom. \n
+  * This method is called internally by UavStateMachine, do not call it by
+  *yourself. \n
+  * See GetThrust if you need thrust value.
+  *
+  * \return custom Thrust
+  */
+  virtual float ComputeCustomThrust(void);
+
+  /*!
+  * \brief Compute Default Thrust
+  *
+  * This method is called internally by UavStateMachine when using
+  *ThrustMode_t::Default. \n
+  * Thrust is only computed once by loop, other calls to this method will use
+  *previously computed thrust.
+  *
+  * \return default thrust
+  */
+  float ComputeDefaultThrust(void);
+
+  /*!
+  * \brief Get Thrust
+  *
+  * \return current thrust
+  */
+  // float GetThrust() const;
+
+  /*!
+  * \brief Add an IODevice to the control law logs
+  *
+  * The IODevice will be automatically logged among the Uz logs,
+  * if logging is enabled (see IODevice::SetDataToLog,
+  *FrameworkManager::StartLog
+  * and FrameworkManager::AddDeviceToLog). \n
+  *
+  * \param device IODevice to log
+  */
+  void AddDeviceToControlLawLog(const core::IODevice *device);
+
+  /*!
+  * \brief Add an io_data to the control law logs
+  *
+  * The io_data will be automatically logged among the Uz logs,
+  * if logging is enabled (see IODevice::SetDataToLog,
+  *FrameworkManager::StartLog
+  * and FrameworkManager::AddDeviceToLog). \n
+  *
+  * \param data io_data to log
+  */
+  void AddDataToControlLawLog(const core::io_data *data);
+
+  const sensor::TargetController *GetJoystick(void) const;
+
+  gui::Tab *setupLawTab, *graphLawTab;
+
+private:
+  /*!
+  \enum AltitudeState_t
+  \brief States of the altitude state machine
+  */
+  enum class AltitudeState_t {
+    Stopped,      /*!< the uav motors are stopped */
+    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
+                     the UAV lift up */
+    Stabilized,   /*!< the uav is actively maintaining its altitude */
+    StartLanding, /*!< landing initiated. Altitude is required to reach the
+                     landing altitude (0 by default) */
+    FinishLanding /*!< motors are gradually stopped */
+  };
+  AltitudeState_t altitudeState;
+  void ProcessAltitudeFiniteStateMachine();
+  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
+
+  float groundAltitude; // effective altitude when the uav leaves the ground
+  float currentAltitude, currentVerticalSpeed;
+
+  bool failSafeMode;
+  void SecurityCheck(void);
+  void MandatorySecurityCheck(void);
+  void CheckJoystick();
+  void GenericCheckJoystick();
+  void CheckPushButton(void);
+  void GenericCheckPushButton(void);
+  void Run(void);
+  void StopMotors(void);
+
+  meta::Uav *uav;
+
+  core::Quaternion currentQuaternion;
+  core::Vector3D currentAngularSpeed;
+
+  const core::AhrsData *ComputeReferenceOrientation(void);
+
+  void ComputeOrientation(void);
+  void ComputeAltitude(void);
+
+  void ComputeTorques(void);
+  core::Euler currentTorques, savedDefaultTorques;
+  bool needToComputeDefaultTorques;
+
+  void ComputeThrust(void);
+  float currentThrust, savedDefaultThrust;
+  bool needToComputeDefaultThrust;
+
+  gui::PushButton *button_kill, *button_take_off, *button_land,
+      *button_start_log, *button_stop_log;
+  gui::GridLayout *buttonslayout;
+  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
+  AltitudeMode_t altitudeMode;
+  OrientationMode_t orientationMode;
+  ThrustMode_t thrustMode;
+  TorqueMode_t torqueMode;
+  bool flagBatteryLow;
+  bool flagConnectionLost;
+  bool flagZTrajectoryFinished;
+  filter::NestedSat *uRoll, *uPitch;
+  filter::Pid *uYaw;
+  filter::PidThrust *uZ;
+
+  MetaDualShock3 *joy;
+  filter::TrajectoryGenerator1D *altitudeTrajectory;
 };
-
 };
 };
Index: trunk/lib/FlairMeta/src/XAir.cpp
===================================================================
--- trunk/lib/FlairMeta/src/XAir.cpp	(revision 10)
+++ trunk/lib/FlairMeta/src/XAir.cpp	(revision 15)
@@ -35,40 +35,46 @@
 using namespace flair::actuator;
 
-namespace flair { namespace meta {
+namespace flair {
+namespace meta {
 
-XAir::XAir(FrameworkManager* parent,string uav_name,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
-    RTDM_I2cPort* i2cport=new RTDM_I2cPort(parent,"rtdm_i2c","rti2c3");
-    RTDM_SerialPort* imu_port=new RTDM_SerialPort(parent,"imu_port","rtser1");
+XAir::XAir(FrameworkManager *parent, string uav_name,
+           filter::UavMultiplex *multiplex)
+    : Uav(parent, uav_name, multiplex) {
+  RTDM_I2cPort *i2cport = new RTDM_I2cPort(parent, "rtdm_i2c", "rti2c3");
+  RTDM_SerialPort *imu_port = new RTDM_SerialPort(parent, "imu_port", "rtser1");
 
-    if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
+  if (multiplex == NULL)
+    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
 
-    SetBldc(new AfroBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),i2cport));
-    SetUsRangeFinder(new Srf08(parent,"SRF08",i2cport,0x70,60));
-    SetAhrs(new Gx3_25_ahrs(parent,"imu",imu_port,Gx3_25_imu::EulerAnglesAndAngularRates,70));
-    Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
-    SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
-    GetBatteryMonitor()->SetBatteryValue(12);
+  SetBldc(new AfroBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
+                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
+  SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
+  SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
+                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
+  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
+  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
+  GetBatteryMonitor()->SetBatteryValue(12);
 
-/*
-        if(VRPNType==Auto || VRPNType==AutoSerialPort)
-        {
-            RTDM_SerialPort* vrpn_port=new RTDM_SerialPort(parent,"vrpn_port","rtser3");
+  /*
+          if(VRPNType==Auto || VRPNType==AutoSerialPort)
+          {
+              RTDM_SerialPort* vrpn_port=new
+     RTDM_SerialPort(parent,"vrpn_port","rtser3");
 
-            vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
-            uav_vrpn=new MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
-        }
-*/
-    SetVerticalCamera(new Ps3Eye(parent,"camv",0,50));
+              vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
+              uav_vrpn=new
+     MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
+          }
+  */
+  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
 }
 
-XAir::~XAir() {
-
-}
+XAir::~XAir() {}
 
 void XAir::StartSensors(void) {
-    ((Gx3_25_ahrs*)GetAhrs())->Start();
-    ((Srf08*)GetUsRangeFinder())->Start();
-    ((Ps3Eye *)GetVerticalCamera())->Start();
-    Uav::StartSensors();
+  ((Gx3_25_ahrs *)GetAhrs())->Start();
+  ((Srf08 *)GetUsRangeFinder())->Start();
+  ((Ps3Eye *)GetVerticalCamera())->Start();
+  Uav::StartSensors();
 }
 
Index: trunk/lib/FlairMeta/src/XAir.h
===================================================================
--- trunk/lib/FlairMeta/src/XAir.h	(revision 10)
+++ trunk/lib/FlairMeta/src/XAir.h	(revision 15)
@@ -16,21 +16,19 @@
 #include "Uav.h"
 
-namespace flair
-{
-namespace meta
-{
-    /*! \class XAir
-    *
-    * \brief Class defining an ardrone2 uav
-    */
-    class XAir : public Uav {
-        public:
-            XAir(core::FrameworkManager* parent,std::string uav_name,filter::UavMultiplex *multiplex=NULL);
-            ~XAir();
-            void StartSensors(void);
+namespace flair {
+namespace meta {
+/*! \class XAir
+*
+* \brief Class defining an ardrone2 uav
+*/
+class XAir : public Uav {
+public:
+  XAir(core::FrameworkManager *parent, std::string uav_name,
+       filter::UavMultiplex *multiplex = NULL);
+  ~XAir();
+  void StartSensors(void);
 
-        private:
-
-    };
+private:
+};
 } // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h
===================================================================
--- trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h	(revision 10)
+++ trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h	(revision 15)
@@ -16,14 +16,11 @@
 #include <IODevice.h>
 
-namespace flair
-{
-    namespace meta
-    {
-        class MetaDualShock3;
-    }
-    namespace filter
-    {
-        class JoyReference;
-    }
+namespace flair {
+namespace meta {
+class MetaDualShock3;
+}
+namespace filter {
+class JoyReference;
+}
 }
 
@@ -32,16 +29,16 @@
 * \brief Classe intégrant la manette DualShock3 et les consignes joystick
 */
-class MetaDualShock3_impl: public flair::core::IODevice
-{
-    public:
-        MetaDualShock3_impl(flair::meta::MetaDualShock3* self,std::string name);
-        ~MetaDualShock3_impl();
-        flair::filter::JoyReference *joy_ref;
+class MetaDualShock3_impl : public flair::core::IODevice {
+public:
+  MetaDualShock3_impl(flair::meta::MetaDualShock3 *self, std::string name);
+  ~MetaDualShock3_impl();
+  flair::filter::JoyReference *joy_ref;
 
-    private:
-        void UpdateFrom(const flair::core::io_data *data);
-        flair::meta::MetaDualShock3* self;
-        bool joy_init;
-        bool wasRollTrimUpButtonPressed,wasRollTrimDownButtonPressed,wasPitchTrimUpButtonPressed,wasPitchTrimDownButtonPressed;
+private:
+  void UpdateFrom(const flair::core::io_data *data);
+  flair::meta::MetaDualShock3 *self;
+  bool joy_init;
+  bool wasRollTrimUpButtonPressed, wasRollTrimDownButtonPressed,
+      wasPitchTrimUpButtonPressed, wasPitchTrimDownButtonPressed;
 };
 
