Index: trunk/demos/CircleFollower/uav/src/CircleFollower.cpp
===================================================================
--- trunk/demos/CircleFollower/uav/src/CircleFollower.cpp	(revision 431)
+++ trunk/demos/CircleFollower/uav/src/CircleFollower.cpp	(revision 432)
@@ -53,4 +53,9 @@
     }
     
+    //set vrpn as failsafe altitude sensor for mamboedu as us in not working well for the moment
+    if(uav->GetType()=="mamboedu") {
+      SetFailSafeAltitudeSensor(uavVrpn->GetAltitudeSensor());
+    }
+    
     getFrameworkManager()->AddDeviceToLog(uavVrpn);
     getFrameworkManager()->AddDeviceToLog(targetVrpn);
Index: trunk/demos/MixedReality/real/uav/src/CircleFollower.cpp
===================================================================
--- trunk/demos/MixedReality/real/uav/src/CircleFollower.cpp	(revision 431)
+++ trunk/demos/MixedReality/real/uav/src/CircleFollower.cpp	(revision 432)
@@ -61,4 +61,9 @@
 	realVrpnClient->Start();
 
+    //set vrpn as failsafe altitude sensor for mamboedu as us in not working well for the moment
+    if(uav->GetType()=="mamboedu") {
+      SetFailSafeAltitudeSensor(uavVrpn->GetAltitudeSensor());
+    }
+
     getFrameworkManager()->AddDeviceToLog(targetVrpn);
 
Index: trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp
===================================================================
--- trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp	(revision 431)
+++ trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp	(revision 432)
@@ -65,4 +65,9 @@
 	vrpnclient->Start();
 
+    //set vrpn as failsafe altitude sensor for mamboedu as us in not working well for the moment
+    if(uav->GetType()=="mamboedu") {
+      SetFailSafeAltitudeSensor(uavVrpn->GetAltitudeSensor());
+    }
+
     circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
     uavVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),0,0,255);
Index: trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp	(revision 431)
+++ trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp	(revision 432)
@@ -37,7 +37,8 @@
 namespace flair {
 namespace meta {
+  
 
 MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
-    : Object(us, us->ObjectName()) {
+    : AltitudeSensor(us, us->ObjectName()) {
   //TODO: mettre une rotation entre la sortie US et le low pass
   //pour eviter les fausses mesures quand le drone est orienté
Index: trunk/lib/FlairMeta/src/MetaUsRangeFinder.h
===================================================================
--- trunk/lib/FlairMeta/src/MetaUsRangeFinder.h	(revision 431)
+++ trunk/lib/FlairMeta/src/MetaUsRangeFinder.h	(revision 432)
@@ -14,17 +14,17 @@
 #define METAUSRANGEFINDER_H
 
-#include <Object.h>
+#include <AltitudeSensor.h>
 
 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;
+  }
 }
 
@@ -38,5 +38,5 @@
 * Cette classe est adaptée pour un capteur d'altitude.
 */
-class MetaUsRangeFinder : public core::Object {
+class MetaUsRangeFinder : public sensor::AltitudeSensor {
 public:
   MetaUsRangeFinder(sensor::UsRangeFinder *us);
@@ -51,5 +51,5 @@
 private:
   sensor::UsRangeFinder *us;
-  filter::ButterworthLowPass *pbas_z, *pbas_vz;
+  filter::ButterworthLowPass *pbas_z,*pbas_vz;
   filter::EulerDerivative *vz_euler;
   gui::DataPlot1D *vz_plot;
Index: trunk/lib/FlairMeta/src/MetaVrpnObject.cpp
===================================================================
--- trunk/lib/FlairMeta/src/MetaVrpnObject.cpp	(revision 431)
+++ trunk/lib/FlairMeta/src/MetaVrpnObject.cpp	(revision 432)
@@ -27,4 +27,5 @@
 #include <TabWidget.h>
 #include <Matrix.h>
+#include <AltitudeSensor.h>
 
 using std::string;
@@ -33,4 +34,33 @@
 using namespace flair::sensor;
 using namespace flair::filter;
+
+namespace flair {
+namespace sensor {
+class MetaVrpnObjectAltitudeSensor : public AltitudeSensor {
+  public:
+    MetaVrpnObjectAltitudeSensor(meta::MetaVrpnObject* parent,std::string name): AltitudeSensor(parent,name) {
+      this->parent=parent;
+    }
+    ~MetaVrpnObjectAltitudeSensor() {
+    }
+
+    //z and dz must be in uav's frame
+    float z(void) const {
+      Vector3Df uav_pos;
+      parent->GetPosition(uav_pos);
+      return -uav_pos.z;
+    }
+    float Vz(void) const {
+      Vector3Df uav_vel;
+      parent->GetSpeed(uav_vel);
+      return -uav_vel.z;
+    }
+
+  private:
+     meta::MetaVrpnObject* parent;
+      
+};
+}// end namespace sensor
+}// end namespace flair
 
 namespace flair {
@@ -80,4 +110,10 @@
   xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
   xy_plot->AddCurve(Output()->Element(5, 0), Output()->Element(4, 0));
+  
+  altitudeSensor=new MetaVrpnObjectAltitudeSensor(this,name);
+}
+
+sensor::AltitudeSensor *MetaVrpnObject::GetAltitudeSensor(void) const {
+  return altitudeSensor;
 }
 
@@ -102,4 +138,4 @@
 }
 
-} // end namespace sensor
+} // end namespace meta
 } // end namespace flair
Index: trunk/lib/FlairMeta/src/MetaVrpnObject.h
===================================================================
--- trunk/lib/FlairMeta/src/MetaVrpnObject.h	(revision 431)
+++ trunk/lib/FlairMeta/src/MetaVrpnObject.h	(revision 432)
@@ -29,4 +29,7 @@
 		class LowPassFilter;
 	}
+  namespace sensor {
+    class AltitudeSensor;
+  }
 }
 
@@ -51,4 +54,10 @@
   void GetSpeed(core::Vector3Df &speed) const;
   filter::EulerDerivative* GetEulerDerivative(void) const;
+  //get the associated AltitudeSensor
+  //MetaVrpnObject do not derivate from AltitudeSensor because frames are differents
+  //and this could be confusing
+  //MetaVrpnObject is in vrpn frame (earth)
+  //AltitudeSensor is in uav frame
+  sensor::AltitudeSensor *GetAltitudeSensor(void) const;
 
 private:
@@ -59,4 +68,5 @@
   gui::DataPlot1D *vx_opti_plot, *vy_opti_plot, *vz_opti_plot;
   gui::Tab *plot_tab;
+  sensor::AltitudeSensor *altitudeSensor;
 };
 } // end namespace meta
Index: trunk/lib/FlairMeta/src/UavStateMachine.cpp
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 431)
+++ trunk/lib/FlairMeta/src/UavStateMachine.cpp	(revision 432)
@@ -53,8 +53,8 @@
 
 UavStateMachine::UavStateMachine(TargetController *controller):
-        Thread(getFrameworkManager(),"UavStateMachine",50),
-        uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
-    altitudeState=AltitudeState_t::Stopped;
-    uav->UseDefaultPlot();
+  Thread(getFrameworkManager(),"UavStateMachine",50),
+  uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
+  altitudeState=AltitudeState_t::Stopped;
+  uav->UseDefaultPlot();
 
   Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
@@ -112,4 +112,6 @@
   uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
       altitudeTrajectory->GetMatrix()->Element(1), DataPlot::Green);
+      
+  failSafeAltitudeSensor=uav->GetMetaUsRangeFinder();
 }
 
@@ -125,9 +127,9 @@
 
 const TargetController *UavStateMachine::GetTargetController(void) const {
-    return controller;
+  return controller;
 }
 
 MetaDualShock3 *UavStateMachine::GetJoystick(void) const {
-    return joy;
+  return joy;
 }
 
@@ -138,4 +140,9 @@
 const Vector3Df &UavStateMachine::GetCurrentAngularSpeed(void) const {
   return currentAngularSpeed;
+}
+
+void UavStateMachine::SetFailSafeAltitudeSensor(AltitudeSensor *altitudeSensor) {
+  Warn("Replacing the FailSafeAltitudeSensor with %s\n",altitudeSensor->ObjectName().c_str());
+  failSafeAltitudeSensor=altitudeSensor;
 }
 
@@ -147,7 +154,10 @@
 void UavStateMachine::FailSafeAltitudeValues(float &altitude,
                                              float &verticalSpeed) const {
-  altitude = uav->GetMetaUsRangeFinder()->z();
-  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
+  altitude = failSafeAltitudeSensor->z();
+  verticalSpeed = failSafeAltitudeSensor->Vz();
   
+  //a mettre dans le metausrangefinder?
+  //on ne sait pas si failSafeAltitudeSensor est vrpn (dans ce cas pas besoin de rotation)
+  //ou un us range finder
   //+0.04 decalage en z de l'us en simulation
   //faire un reglage
Index: trunk/lib/FlairMeta/src/UavStateMachine.h
===================================================================
--- trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 431)
+++ trunk/lib/FlairMeta/src/UavStateMachine.h	(revision 432)
@@ -26,29 +26,30 @@
 
 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 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;
+    class AltitudeSensor;
+  }
+  namespace meta {
+    class MetaDualShock3;
+    class Uav;
+  }
 }
 
@@ -102,6 +103,6 @@
   };
 
-        UavStateMachine(sensor::TargetController* controller);
-        ~UavStateMachine();
+  UavStateMachine(sensor::TargetController* controller);
+  ~UavStateMachine();
 
   const core::Quaternion &GetCurrentQuaternion(void) const;
@@ -109,13 +110,13 @@
   const core::Vector3Df &GetCurrentAngularSpeed(void) const;
 
-        void Land(void);
-        void EmergencyLand(void);
-        void TakeOff(void);
-        void EmergencyStop(void);
-        //! Used to signal an event
-        /*!
-            \param event the event which occured
-        */
-        virtual void SignalEvent(Event_t event);
+  void Land(void);
+  void EmergencyLand(void);
+  void TakeOff(void);
+  void EmergencyStop(void);
+  //! Used to signal an event
+  /*!
+      \param event the event which occured
+  */
+  virtual void SignalEvent(Event_t event);
 
   virtual const core::AhrsData *GetOrientation(void) const;
@@ -126,4 +127,15 @@
   bool ExitFailSafeMode(void);
   void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
+  
+  /*!
+  * \brief Set the FailSafe AltitudeSensor
+  *
+  * by default, the failsafe AltitudeSensor is the MetaUsRangeFinder
+  * you can change it (by a vrpn one) using this method in the case us range finder is not working well
+  * use it carefully, trusting only vrpn can be risquee
+  *
+  * \param altitudeSensor altitudeSensor to use when is failsafe
+  */
+  void SetFailSafeAltitudeSensor(sensor::AltitudeSensor *altitudeSensor);
 
   gui::GridLayout *GetButtonsLayout(void) const;
@@ -306,6 +318,7 @@
   filter::PidThrust *uZ;
   filter::TrajectoryGenerator1D *altitudeTrajectory;
+  sensor::AltitudeSensor *failSafeAltitudeSensor;
 };
-};
-};
+}; // end namespace meta
+}; // end namespace flair
 #endif // UAVSTATEMACHINE_H
Index: trunk/lib/FlairSensorActuator/src/AltitudeSensor.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/AltitudeSensor.h	(revision 432)
+++ trunk/lib/FlairSensorActuator/src/AltitudeSensor.h	(revision 432)
@@ -0,0 +1,43 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file AltitudeSensor.h
+ * \brief Abstract class for altitude sensor
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2021/07/12
+ * \version 3.4
+ */
+
+#ifndef ALTITUDESENSOR_H
+#define ALTITUDESENSOR_H
+
+#include <Object.h>
+
+
+namespace flair {
+namespace sensor {
+/*! \class AltitudeSensor
+*
+* \brief Abstract class for altitude sensor
+*
+* sensors like us or vrpn should instantiate a class
+* of this type to provide altitude values to
+* the state machine
+* returned vales are in uav coordinates
+*
+*/
+class AltitudeSensor : public core::Object {
+public:
+  AltitudeSensor(core::Object *parent,std::string name): core::Object(parent,name) {};
+  virtual ~AltitudeSensor() {};
+  virtual float z(void) const=0;//in uav frame
+  virtual float Vz(void) const=0;//in uav frame
+  
+private:
+  
+};
+} // end namespace sensor
+} // end namespace flair
+#endif // ALTITUDESENSOR_H
