Index: trunk/lib/FlairMeta/src/Plane.cpp
===================================================================
--- trunk/lib/FlairMeta/src/Plane.cpp	(revision 461)
+++ trunk/lib/FlairMeta/src/Plane.cpp	(revision 461)
@@ -0,0 +1,61 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2022/01/05
+//  filename:   Plane.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class to construct sensors/actuators depending on Plane type
+//
+//
+/*********************************************************************/
+
+#include "Plane.h"
+#include <FrameworkManager.h>
+#include <Tab.h>
+#include <GridLayout.h>
+#include <DataPlot1D.h>
+#include <UavMultiplex.h>
+#include <Servos.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::actuator;
+using namespace flair::filter;
+
+namespace {
+  flair::meta::Plane *uavSingleton = NULL;
+}
+
+namespace flair {
+namespace meta {
+
+Plane *GetPlane(void) { return uavSingleton; }
+
+Plane::Plane(string name, UavMultiplex *multiplex): Uav(name, multiplex) {
+  if (uavSingleton != NULL) {
+    Err("Uav must be instanced only one time\n");
+    return;
+  }
+
+  uavSingleton = this;
+  servos=NULL;
+}
+
+Plane::~Plane() {}
+
+
+void Plane::SetServos(const Servos *insevos) { 
+  servos = (Servos *)insevos;
+}
+
+
+Servos *Plane::GetServos(void) const { return servos; }
+
+} // end namespace meta
+} // end namespace flair
Index: trunk/lib/FlairMeta/src/Plane.h
===================================================================
--- trunk/lib/FlairMeta/src/Plane.h	(revision 461)
+++ trunk/lib/FlairMeta/src/Plane.h	(revision 461)
@@ -0,0 +1,66 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Plane.h
+ * \brief Base class to construct sensors/actuators depending on Plane type
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2022/01/05
+ * \version 3.4
+ */
+
+#ifndef PLANE_H
+#define PLANE_H
+
+#include <Uav.h>
+
+namespace flair {
+	namespace filter {
+		class UavMultiplex;
+	}
+	namespace actuator {
+		class Servos;
+	}
+	
+}
+
+namespace flair {
+namespace meta {
+
+
+/*! \class Plane
+*
+* \brief Base class to construct sensors/actuators depending on Plane type.
+* The Object is created with 
+*  the FrameworkManager as parent. FrameworkManager must be created before.
+* Only one instance of this class is allowed by program.
+*/
+class Plane : public meta::Uav {
+public:
+
+  Plane(std::string name,filter::UavMultiplex *multiplex);
+  ~Plane();
+
+  actuator::Servos *GetServos(void) const;
+  
+
+protected:
+  void SetServos(const actuator::Servos *servos);
+  
+
+private:
+  actuator::Servos *servos;
+  
+};
+
+/*!
+* \brief get Uav
+*
+* \return the Uav
+*/
+Plane *GetPlane(void);
+
+} // end namespace meta
+} // end namespace flair
+#endif // PLANE_H
Index: trunk/lib/FlairMeta/src/PlaneStateMachine.cpp
===================================================================
--- trunk/lib/FlairMeta/src/PlaneStateMachine.cpp	(revision 461)
+++ trunk/lib/FlairMeta/src/PlaneStateMachine.cpp	(revision 461)
@@ -0,0 +1,492 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2022/01/05
+//  filename:   PlaneStateMachine.cpp
+//
+//  author:     Gildas Bayard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    state machine class for plane
+//
+//
+/*********************************************************************/
+
+#include "PlaneStateMachine.h"
+#include "Plane.h"
+#include <GridLayout.h>
+#include <DoubleSpinBox.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <PushButton.h>
+#include <UavMultiplex.h>
+#include <Bldc.h>
+#include <Servos.h>
+#include <Ahrs.h>
+#include <ControlLaw.h>
+#include <Pid.h>
+#include <PidThrust.h>
+#include <NestedSat.h>
+#include <MetaDualShock3.h>
+#include <AhrsData.h>
+#include <BatteryMonitor.h>
+#include <FrameworkManager.h>
+#include <math.h>
+
+using namespace std;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+using namespace flair::actuator;
+using namespace flair::filter;
+using namespace flair::meta;
+
+PlaneStateMachine::PlaneStateMachine(TargetController *controller):
+  Thread(getFrameworkManager(),"PlaneStateMachine",50),
+  uav(GetPlane()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),safeToFly(true){
+
+  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_start = new PushButton(buttonslayout->NewRow(), "start");
+  button_stop = new PushButton(buttonslayout->LastRowLastCol(), "stop");
+  thrust = new DoubleSpinBox(buttonslayout->NewRow(), "fixed thrust",0, 50, 0.1, 2);
+                        
+ 
+  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());
+
+  getFrameworkManager()->AddDeviceToLog(uRoll);
+  uRoll->AddDeviceToLog(uPitch);
+  uRoll->AddDeviceToLog(uYaw);
+
+  joy=new MetaDualShock3("uav high level controller",controller);
+  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
+
+  orientationMode = OrientationMode_t::Manual;
+  torqueMode = TorqueMode_t::Default;
+}
+
+PlaneStateMachine::~PlaneStateMachine() {}
+
+void PlaneStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
+  uRoll->AddDeviceToLog(device);
+}
+
+void PlaneStateMachine::AddDataToControlLawLog(const core::io_data *data) {
+  uRoll->AddDataToLog(data);
+}
+
+const TargetController *PlaneStateMachine::GetTargetController(void) const {
+    return controller;
+}
+
+MetaDualShock3 *PlaneStateMachine::GetJoystick(void) const {
+    return joy;
+}
+
+const Quaternion &PlaneStateMachine::GetCurrentQuaternion(void) const {
+  return currentQuaternion;
+}
+
+const Vector3Df &PlaneStateMachine::GetCurrentAngularSpeed(void) const {
+  return currentAngularSpeed;
+}
+
+
+void PlaneStateMachine::Run() {
+  WarnUponSwitches(true);
+  uav->StartSensors();
+//uav->GetBldc()->SetEnabled(true);
+//uav->GetServos()->SetEnabled(true);
+  if (getFrameworkManager()->ErrorOccured() == true) {
+    SafeStop();
+  }
+
+  while (!ToBeStopped()) {
+    SecurityCheck();
+
+    // get controller inputs
+    CheckJoystick();
+    CheckPushButton();
+
+    if (IsPeriodSet()) {
+      WaitPeriod();
+    } else {
+      WaitUpdate(uav->GetAhrs());
+    }
+    needToComputeDefaultTorques = true;
+    
+    SignalEvent(Event_t::EnteringControlLoop);
+
+    ComputeOrientation();
+   
+    // compute torques to apply
+    ComputeTorques();
+    
+    //check nan/inf problems
+    bool isValuePossible=true;
+    //test it separately to warn for all problems
+    if(!IsValuePossible(currentTorques.roll,"roll torque")) isValuePossible=false;
+    if(!IsValuePossible(currentTorques.pitch,"pitch torque")) isValuePossible=false;
+    if(!IsValuePossible(currentTorques.yaw,"yaw torque")) isValuePossible=false;
+
+    if(!isValuePossible) {
+      if(failSafeMode) {
+        Warn("We are already in safe mode, the uav is going to crash!\n");
+      } else {
+        Thread::Warn("switching back to safe mode\n");
+        EnterFailSafeMode();
+        needToComputeDefaultTorques = true;//should not be necessary, but put it to be sure to compute default thrust/torques
+       
+        ComputeTorques();
+      }
+        
+    }
+
+    // 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(thrust->Value());
+   
+    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
+    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
+    uav->GetUavMultiplex()->SetYawTrim(0);
+    uav->GetUavMultiplex()->Update(GetTime());
+  }
+
+  WarnUponSwitches(false);
+}
+
+bool PlaneStateMachine::IsValuePossible(float value,std::string desc) {
+  if(isnan(value)) {
+    Warn("%s is not an number\n",desc.c_str());
+    return false;
+  } else if(isinf(value)) {
+    Warn("%s is infinite\n",desc.c_str());
+    return false;
+  } else {
+    return true;
+  }
+}
+
+
+void PlaneStateMachine::ComputeOrientation(void) {
+  if (failSafeMode) {
+    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
+                                                          currentAngularSpeed);
+  } else {
+    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
+                                                   currentAngularSpeed);
+  }
+}
+
+const AhrsData *PlaneStateMachine::GetOrientation(void) const {
+  return GetDefaultOrientation();
+}
+
+const AhrsData *PlaneStateMachine::GetDefaultOrientation(void) const {
+  return uav->GetAhrs()->GetDatas();
+}
+
+
+const AhrsData *PlaneStateMachine::ComputeReferenceOrientation(void) {
+  if (orientationMode == OrientationMode_t::Manual) {
+    return GetDefaultReferenceOrientation();
+  } else {
+    return GetReferenceOrientation();
+  }
+}
+
+const AhrsData *PlaneStateMachine::GetDefaultReferenceOrientation(void) const {
+  // We directly control yaw, pitch, roll angles
+  return joy->GetReferenceOrientation();
+}
+
+const AhrsData *PlaneStateMachine::GetReferenceOrientation(void) {
+  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
+               "switching back to safe mode\n");
+  EnterFailSafeMode();
+  return GetDefaultReferenceOrientation();
+}
+
+void PlaneStateMachine::ComputeTorques(void) {
+  if (torqueMode == TorqueMode_t::Default) {
+    ComputeDefaultTorques(currentTorques);
+  } else {
+    ComputeCustomTorques(currentTorques);
+  }
+}
+
+void PlaneStateMachine::ComputeDefaultTorques(Euler &torques) {
+  if (needToComputeDefaultTorques) {
+    const AhrsData *refOrientation = ComputeReferenceOrientation();
+    Quaternion refQuaternion;
+    Vector3Df 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();
+//Printf("%f %f %f %f\n",refAngles.pitch, currentAngles.pitch, currentAngularSpeed.y,torques.pitch);
+    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
+    uRoll->Update(GetTime());
+    torques.roll = uRoll->Output();
+
+    savedDefaultTorques = torques;
+    needToComputeDefaultTorques = false;
+  } else {
+    torques = savedDefaultTorques;
+  }
+}
+
+void PlaneStateMachine::ComputeCustomTorques(Euler &torques) {
+  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
+               "switching back to safe mode\n");
+  EnterFailSafeMode();
+  ComputeDefaultTorques(torques);
+}
+
+
+void PlaneStateMachine::SignalEvent(Event_t event) {
+  switch (event) {
+  case Event_t::Started:
+    Thread::Info("entering 'Started' state\n");
+    break;
+  case Event_t::Stopped:
+    Thread::Info("entering 'Stopped' state\n");
+    break;
+  case Event_t::EmergencyStop:
+    Thread::Info("Emergency stop!\n");
+    break;
+  case Event_t::EnteringFailSafeMode:
+    Thread::Info("Entering fail safe mode\n");
+    break;
+  }
+}
+
+void PlaneStateMachine::EmergencyStop(void) {
+    
+    StopActuators();
+    EnterFailSafeMode();
+    joy->Rumble(0x70);
+    SignalEvent(Event_t::EmergencyStop);
+    
+    //safeToFly=false;
+    //Warn("Emergency stop, UAV will not take off again until program is rerunned\n");
+}
+
+void PlaneStateMachine::StartActuators(void) {
+  //The uav always takes off in fail safe mode
+  flagBatteryLow=false;
+  EnterFailSafeMode();
+  joy->SetLedOFF(4);//DualShock3::led4
+  joy->SetLedOFF(1);//DualShock3::led1
+  joy->Rumble(0x70);
+  joy->SetZRef(0);
+
+  uRoll->Reset();
+  uPitch->Reset();
+  uYaw->Reset();
+
+  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);
+  uav->GetServos()->SetEnabled(true);
+  SignalEvent(Event_t::Started);
+}
+
+void PlaneStateMachine::StopActuators(void) {
+  joy->FlashLed(1, 10, 10); // DualShock3::led1
+  uav->GetBldc()->SetEnabled(false);
+  uav->GetServos()->SetEnabled(false);
+  uav->GetUavMultiplex()->UnlockUserInterface();
+  SignalEvent(Event_t::Stopped);
+  uav->GetAhrs()->UnlockUserInterface();
+
+  uRoll->Reset();
+  uPitch->Reset();
+  uYaw->Reset();
+}
+
+GridLayout *PlaneStateMachine::GetButtonsLayout(void) const {
+  return buttonslayout;
+}
+
+void PlaneStateMachine::SecurityCheck(void) {
+  MandatorySecurityCheck();
+  ExtraSecurityCheck();
+}
+
+void PlaneStateMachine::MandatorySecurityCheck(void) {
+  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
+    flagConnectionLost = true;
+    
+    EnterFailSafeMode();
+  }
+    if(uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
+        flagBatteryLow=true;
+        Thread::Err("Low Battery\n");
+        EnterFailSafeMode();
+    }
+}
+
+void PlaneStateMachine::CheckJoystick(void) {
+  GenericCheckJoystick();
+  ExtraCheckJoystick();
+}
+
+void PlaneStateMachine::GenericCheckJoystick(void) {
+  static bool isEmergencyStopButtonPressed = false;
+  static bool isTakeOffButtonPressed = false;
+  static bool isSafeModeButtonPressed = false;
+
+  if (controller->IsButtonPressed(1)) { // select
+    if (!isEmergencyStopButtonPressed) {
+      isEmergencyStopButtonPressed = true;
+      Thread::Info("Emergency stop from joystick\n");
+      EmergencyStop();
+    }
+  } else
+    isEmergencyStopButtonPressed = false;
+
+  if (controller->IsButtonPressed(0)) { // start
+    if (!isTakeOffButtonPressed) {
+      isTakeOffButtonPressed = true;
+      StartActuators();
+    }
+  } 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 (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
+      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
+      !controller->IsButtonPressed(10)) {
+    if (!isSafeModeButtonPressed) {
+      isSafeModeButtonPressed = true;
+      EnterFailSafeMode();
+    }
+  } else
+    isSafeModeButtonPressed = false;
+}
+
+void PlaneStateMachine::CheckPushButton(void) {
+  GenericCheckPushButton();
+  ExtraCheckPushButton();
+}
+
+void PlaneStateMachine::GenericCheckPushButton(void) {
+  if (button_kill->Clicked() == true)
+    SafeStop();
+  if (button_start_log->Clicked() == true)
+    getFrameworkManager()->StartLog();
+  if (button_stop_log->Clicked() == true)
+    getFrameworkManager()->StopLog();
+  if (button_start->Clicked() == true)
+    StartActuators();
+  if (button_stop->Clicked() == true)
+    StopActuators();
+}
+
+void PlaneStateMachine::EnterFailSafeMode(void) {
+  SetOrientationMode(OrientationMode_t::Manual);;
+  SetTorqueMode(TorqueMode_t::Default);
+
+  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
+                                                        currentAngularSpeed);
+  joy->SetYawRef(currentQuaternion);
+  uYaw->Reset();
+  uPitch->Reset();
+  uRoll->Reset();
+
+  failSafeMode = true;
+  SignalEvent(Event_t::EnteringFailSafeMode);
+}
+
+bool PlaneStateMachine::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 PlaneStateMachine::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;
+}
+
+bool PlaneStateMachine::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;
+}
+
+
+NestedSat* PlaneStateMachine::GetURoll(void) { return uRoll;};
+NestedSat* PlaneStateMachine::GetUPitch(void) { return uPitch;};
+Pid* PlaneStateMachine::GetUYaw(void) { return uYaw;};
Index: trunk/lib/FlairMeta/src/PlaneStateMachine.h
===================================================================
--- trunk/lib/FlairMeta/src/PlaneStateMachine.h	(revision 461)
+++ trunk/lib/FlairMeta/src/PlaneStateMachine.h	(revision 461)
@@ -0,0 +1,224 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2022/01/05
+//  filename:   PlaneStateMachine.h
+//
+//  author:     Gildas Bayard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    state machine for plane
+//
+//
+/*********************************************************************/
+
+#ifndef PLANESTATEMACHINE_H
+#define PLANESTATEMACHINE_H
+
+#include <Thread.h>
+#include <Vector2D.h>
+#include <Vector3D.h>
+#include <Euler.h>
+#include <Quaternion.h>
+
+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;
+}
+namespace sensor {
+class TargetController;
+}
+namespace meta {
+class MetaDualShock3;
+class Plane;
+}
+}
+
+namespace flair {
+namespace meta {
+
+/*! \class PlaneStateMachine
+*
+* \brief State machine for UAV
+*  The Thread is created with 
+*  the FrameworkManager as parent. FrameworkManager must be created before.
+* The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or
+* SetPeriodMS
+*/
+
+class PlaneStateMachine : public core::Thread {
+protected:
+
+  enum class OrientationMode_t { Manual, Custom };
+  const OrientationMode_t &GetOrientationMode(void) const {
+    return orientationMode;
+  }
+  bool SetOrientationMode(const OrientationMode_t &orientationMode);
+
+  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,
+    Stopped,
+    EmergencyStop,
+    Started,
+  };
+
+        PlaneStateMachine(sensor::TargetController* controller);
+        ~PlaneStateMachine();
+
+  const core::Quaternion &GetCurrentQuaternion(void) const;
+
+  const core::Vector3Df &GetCurrentAngularSpeed(void) const;
+
+      
+        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;
+
+  void EnterFailSafeMode(void);
+  bool ExitFailSafeMode(void);
+  
+  gui::GridLayout *GetButtonsLayout(void) const;
+  virtual void ExtraSecurityCheck(void){};
+  virtual void ExtraCheckJoystick(void){};
+  virtual void ExtraCheckPushButton(void){};
+
+  // 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 PlaneStateMachine, 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 PlaneStateMachine 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 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 *GetTargetController(void) const;
+  MetaDualShock3 *GetJoystick(void) const;
+  
+  filter::NestedSat *GetURoll(void);
+  filter::NestedSat *GetUPitch(void);
+  filter::Pid *GetUYaw(void);
+  
+  gui::Tab *setupLawTab, *graphLawTab;
+
+private:
+  bool failSafeMode;
+  void SecurityCheck(void);
+  void MandatorySecurityCheck(void);
+  void CheckJoystick();
+  void GenericCheckJoystick();
+  void CheckPushButton(void);
+  void GenericCheckPushButton(void);
+  void Run(void);
+  void StartActuators(void);
+  void StopActuators(void);
+  bool IsValuePossible(float value,std::string desc);
+
+  Plane *uav;
+  MetaDualShock3 *joy;
+  sensor::TargetController *controller;
+
+  core::Quaternion currentQuaternion;
+  core::Vector3Df currentAngularSpeed;
+
+  const core::AhrsData *ComputeReferenceOrientation(void);
+
+  void ComputeOrientation(void);
+  
+  void ComputeTorques(void);
+  core::Euler currentTorques, savedDefaultTorques;
+  bool needToComputeDefaultTorques;
+
+  gui::PushButton *button_kill,*button_start_log, *button_stop_log;
+  gui::PushButton *button_start,*button_stop;
+  gui::GridLayout *buttonslayout;
+  gui::DoubleSpinBox *thrust;
+  OrientationMode_t orientationMode;
+  TorqueMode_t torqueMode;
+  bool flagBatteryLow;
+  bool flagConnectionLost;
+  bool flagCriticalSensorLost;
+  bool safeToFly;
+  filter::NestedSat *uRoll, *uPitch;
+  filter::Pid *uYaw;
+};
+};
+};
+#endif // PLANESTATEMACHINE_H
Index: trunk/lib/FlairMeta/src/SimuPlane.cpp
===================================================================
--- trunk/lib/FlairMeta/src/SimuPlane.cpp	(revision 461)
+++ trunk/lib/FlairMeta/src/SimuPlane.cpp	(revision 461)
@@ -0,0 +1,66 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2022/01/05
+//  filename:   SimuPlane.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class defining a simulation plane
+//
+//
+/*********************************************************************/
+#ifdef CORE2_64
+
+#include "SimuPlane.h"
+#include <FrameworkManager.h>
+#include <SimulatedImu.h>
+#include <PassthroughAhrs.h>
+#include <SimulatedBldc.h>
+#include <SimulatedServos.h>
+#include <SimulatedUs.h>
+#include <BatteryMonitor.h>
+#include "PlaneMultiplex.h"
+#include <Tab.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+using namespace flair::filter;
+using namespace flair::actuator;
+
+namespace flair {
+namespace meta {
+
+SimuPlane::SimuPlane(string name, uint32_t simu_id,filter::PlaneMultiplex *multiplex)
+    : Plane(name, multiplex) {
+
+
+  SetBldc(new SimulatedBldc(multiplex->GetBldcMultiplex(), multiplex->GetLayout(),"motors",1, simu_id,0));
+  SetServos(new SimulatedServos(multiplex->GetServosMultiplex(),multiplex->GetLayout(),"servos",2, simu_id,0));
+  SetUsRangeFinder(new SimulatedUs("us", simu_id,0, 60));
+  SetAhrs(new PassthroughAhrs(new SimulatedImu("imu", simu_id,0, 70),"ahrs"));
+  Tab *bat_tab = new Tab(getFrameworkManager()->GetTabWidget(), "battery");
+  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
+  GetBatteryMonitor()->SetBatteryValue(12);
+
+}
+
+SimuPlane::~SimuPlane() {}
+
+void SimuPlane::StartSensors(void) {
+  ((SimulatedImu *)(GetAhrs()->GetImu()))->Start();
+  ((SimulatedUs *)GetUsRangeFinder())->Start();
+}
+
+
+
+} // end namespace meta
+} // end namespace flair
+
+#endif
Index: trunk/lib/FlairMeta/src/SimuPlane.h
===================================================================
--- trunk/lib/FlairMeta/src/SimuPlane.h	(revision 461)
+++ trunk/lib/FlairMeta/src/SimuPlane.h	(revision 461)
@@ -0,0 +1,47 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuPlane.h
+ * \brief Class defining a simulation plane
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2022/01/05
+ * \version 4.0
+ */
+
+#ifndef SIMUPLANE_H
+#define SIMUPLANE_H
+
+#include "Plane.h"
+
+namespace flair {
+    namespace filter {
+        class PlaneMultiplex;
+    }
+
+}
+
+namespace flair {
+namespace meta {
+  
+/*! \class SimuPlane
+*
+* \brief Class defining a simulation plane
+*/
+class SimuPlane : public Plane {
+  public:
+    // simu_id: 0 if simulating only one UAV
+    //>0 otherwise
+    SimuPlane(std::string name, uint32_t simu_id,filter::PlaneMultiplex *multiplex);
+    ~SimuPlane();
+    void StartSensors(void);
+    virtual std::string GetType(void) const{return "plane_simu";}
+
+  private:
+    
+  
+};
+} // end namespace meta
+} // end namespace flair
+#endif // SIMUPLANE_H
