Index: /trunk/lib/FlairSensorActuator/CMakeLists.txt
===================================================================
--- /trunk/lib/FlairSensorActuator/CMakeLists.txt	(revision 3)
+++ /trunk/lib/FlairSensorActuator/CMakeLists.txt	(revision 3)
@@ -0,0 +1,44 @@
+PROJECT(FlairSensorActuator)
+cmake_minimum_required(VERSION 2.8)
+include($ENV{FLAIR_ROOT}/flair-dev/cmake-modules/GlobalCmakeFlair.cmake)
+
+find_package(LibXml2 REQUIRED)
+
+execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion
+                OUTPUT_VARIABLE GCC_VERSION)
+
+if (GCC_VERSION VERSION_GREATER 4.3 OR GCC_VERSION VERSION_EQUAL 4.3)
+	if (GCC_VERSION VERSION_GREATER 4.7 OR GCC_VERSION VERSION_EQUAL 4.7)
+		ADD_DEFINITIONS("-std=c++11")
+	else()
+		ADD_DEFINITIONS("-std=c++0x")
+	endif()
+else()
+	message(STATUS "GCC version < 4.3, c+11 is not supported!")
+endif()
+
+INCLUDE_DIRECTORIES(
+	${LIBXML2_INCLUDE_DIR}
+	${CMAKE_CURRENT_SOURCE_DIR}/src
+	${CMAKE_CURRENT_SOURCE_DIR}/src/unexported
+	${CMAKE_CURRENT_SOURCE_DIR}/../FlairCore/src
+	${CMAKE_SYSROOT}/usr/include/opencv
+	${CMAKE_SYSROOT}/usr/include/vrpn
+#	$ENV{IGEP_ROOT}/uav_dev/include
+)
+
+FILE(GLOB SENSOR_SRC "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
+
+SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
+
+ADD_LIBRARY(${PROJECT_NAME}
+	${SENSOR_SRC}
+)
+
+INSTALL(TARGETS ${PROJECT_NAME}
+	LIBRARY DESTINATION $ENV{FLAIR_ROOT}/flair-bin/lib/${ARCH_DIR}
+	ARCHIVE DESTINATION $ENV{FLAIR_ROOT}/flair-bin/lib/${ARCH_DIR}
+)
+
+FILE(GLOB files "${CMAKE_CURRENT_SOURCE_DIR}/src/*.h")
+INSTALL(FILES ${files} DESTINATION $ENV{FLAIR_ROOT}/flair-dev/include/${PROJECT_NAME})
Index: /trunk/lib/FlairSensorActuator/src/AfroBldc.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/AfroBldc.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/AfroBldc.cpp	(revision 3)
@@ -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}
+//  created:    2015/06/15
+//  filename:   AfroBldc.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c
+//
+//
+/*********************************************************************/
+
+#include "AfroBldc.h"
+#include "AfroBldc_impl.h"
+#include <GroupBox.h>
+#include <Layout.h>
+#include <DoubleSpinBox.h>
+#include <SpinBox.h>
+#include <ComboBox.h>
+#include <PushButton.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::sensor;
+using namespace flair::gui;
+
+namespace flair { namespace actuator {
+
+AfroBldc::AfroBldc(const IODevice* parent,Layout* layout,string name,uint8_t motors_count,I2cPort* i2cport) : Bldc(parent,layout,name,motors_count) {
+    pimpl_=new AfroBldc_impl(this,layout,i2cport);
+}
+
+AfroBldc::~AfroBldc() {
+    delete pimpl_;
+}
+
+void AfroBldc::SetMotors(float* values) {
+    pimpl_->SetMotors(values);
+}
+
+} // end namespace actuator
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/AfroBldc.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/AfroBldc.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/AfroBldc.h	(revision 3)
@@ -0,0 +1,100 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file AfroBldc.h
+ * \brief Class for Mikrokopter's blctrlv2
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2015/06/15
+ * \version 4.0
+ */
+
+#ifndef AFROBLDC_H
+#define AFROBLDC_H
+
+#include "Bldc.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class I2cPort;
+    }
+    namespace sensor
+    {
+        class BatteryMonitor;
+    }
+}
+
+class AfroBldc_impl;
+
+namespace flair
+{
+namespace actuator
+{
+    /*! \class AfroBldc
+    *
+    * \brief Class for Mikrokopter's blctrlv2
+    *
+    * blctrlv2 drivers can also monitor the battery level. See GetBatteryMonitor().
+    */
+    class AfroBldc : public Bldc
+    {
+        friend class ::AfroBldc_impl;
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a AfroBldc.
+            *
+            * \param parent parent
+            * \param layout layout
+            * \param name name
+            * \param motors_count number of motors
+            * \param i2cport I2cPort
+            */
+            AfroBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count,core::I2cPort* i2cport);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~AfroBldc();
+
+            /*!
+            * \brief Has speed measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has speed measurement
+            */
+            bool HasSpeedMeasurement(void) const{return false;};
+
+            /*!
+            * \brief Has current measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has current measurement
+            */
+            bool HasCurrentMeasurement(void) const{return false;};
+
+        private:
+            /*!
+            * \brief Set motors values
+            *
+            * Reimplemented from Bldc. \n
+            * Values size must be the same as MotorsCount()
+            *
+            * \param values motor values
+            */
+            void SetMotors(float* values);
+
+            class AfroBldc_impl* pimpl_;
+    };
+} // end namespace actuator
+} // end namespace flair
+#endif // AFROBLDC_H
Index: /trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp	(revision 3)
@@ -0,0 +1,73 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/06/15
+//  filename:   AfroBldc_impl.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c
+//
+//
+/*********************************************************************/
+#include "AfroBldc_impl.h"
+#include "AfroBldc.h"
+#include "Bldc_impl.h"
+#include "I2cPort.h"
+#include <Layout.h>
+#include <SpinBox.h>
+#include <Label.h>
+#include <cvmatrix.h>
+#include <string.h>
+
+#define BASE_ADDRESS 0x29
+#define MAX_VALUE 2047
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+using namespace flair::actuator;
+
+AfroBldc_impl::AfroBldc_impl(AfroBldc* self,Layout *layout,I2cPort* i2cport) {
+    this->self=self;
+    this->i2cport=i2cport;
+    nb_mot=self->MotorsCount();
+}
+
+AfroBldc_impl::~AfroBldc_impl() {
+
+}
+
+void AfroBldc_impl::SetMotors(float* value) {
+    uint16_t tosend_value[nb_mot];
+
+    for(int i=0; i<nb_mot; i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
+
+    i2cport->GetMutex();
+
+    for(int i=0; i<nb_mot; i++) {
+        i2cport->SetSlave(BASE_ADDRESS+i);
+        WriteValue(tosend_value[i]);
+    }
+    i2cport->ReleaseMutex();
+}
+
+//I2cPort mutex must be taken before calling this function
+void AfroBldc_impl::WriteValue(uint16_t value) {
+    uint8_t tx[2];
+    ssize_t written;
+
+    tx[0]=(uint8_t)(value>>3);//msb
+    tx[1]=(value&0x07);//lsb
+    written =i2cport->Write(tx, 2);
+    if(written<0) {
+        self->Err("rt_dev_write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(tx)) {
+        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
+    }
+}
Index: /trunk/lib/FlairSensorActuator/src/BatteryMonitor.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BatteryMonitor.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BatteryMonitor.cpp	(revision 3)
@@ -0,0 +1,60 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/01/24
+//  filename:   BatteryMonitor.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for battery monitor
+//
+//
+/*********************************************************************/
+
+
+#include "BatteryMonitor.h"
+#include <Layout.h>
+#include <Label.h>
+#include <DoubleSpinBox.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace sensor {
+
+BatteryMonitor::BatteryMonitor(const gui::LayoutPosition* position,string name) : GroupBox(position,name) {
+    battery=new Label(this->NewRow(),"battery");
+    battery_thresh=new DoubleSpinBox(this->LastRowLastCol(),"threshold"," V",0,24,.1,1);
+}
+
+BatteryMonitor::~BatteryMonitor() {
+
+}
+
+float BatteryMonitor::GetVoltage(void) const {
+    return batteryvalue;
+}
+
+bool BatteryMonitor::IsBatteryLow(void) const {
+    if(batteryvalue<battery_thresh->Value())
+        return true;
+    else
+        return false;
+}
+
+void BatteryMonitor::SetBatteryValue(float value) {
+    batteryvalue=value;
+    if(value>0) {
+        battery->SetText("battery: %.1fV",value);
+    } else {
+        battery->SetText("battery: unreadable");
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/BatteryMonitor.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BatteryMonitor.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BatteryMonitor.h	(revision 3)
@@ -0,0 +1,89 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file BatteryMonitor.h
+ * \brief Base class for battery monitor
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/01/24
+ * \version 4.0
+ */
+
+#ifndef BATTERYMONITOR_H
+#define BATTERYMONITOR_H
+
+#include <GroupBox.h>
+
+namespace flair
+{
+    namespace gui
+    {
+        class LayoutPosition;
+        class Label;
+        class DoubleSpinBox;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+
+    /*! \class BatteryMonitor
+    *
+    * \brief Base class for battery monitor
+    */
+
+    class BatteryMonitor : public gui::GroupBox
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a BatteryMonitor at given position.
+            *
+            * \param position position
+            * \param name name
+            */
+            BatteryMonitor(const gui::LayoutPosition* position,std::string name);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~BatteryMonitor();
+
+            /*!
+            * \brief Is batteru low?
+            *
+            * \return true if battery is below threshold
+            *
+            */
+            bool IsBatteryLow(void) const;
+
+            /*!
+            * \brief Set battery value
+            *
+            * \param battery value
+            *
+            */
+            void SetBatteryValue(float value);
+
+            /*!
+            * \brief Get battery voltage
+            *
+            * \return battery voltage
+            *
+            */
+            float GetVoltage(void) const;
+
+        private:
+            float batteryvalue;
+            gui::DoubleSpinBox *battery_thresh;
+            gui::Label *battery;
+    };
+} // end namespace sensor
+} // end namespace flair
+
+#endif // BATTERYMONITOR_H
Index: /trunk/lib/FlairSensorActuator/src/BlCtrlV2.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BlCtrlV2.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BlCtrlV2.cpp	(revision 3)
@@ -0,0 +1,59 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2011/09/13
+//  filename:   BlCtrlV2.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c
+//
+//
+/*********************************************************************/
+
+#include "BlCtrlV2.h"
+#include "BlCtrlV2_impl.h"
+#include "BatteryMonitor.h"
+#include <GroupBox.h>
+#include <Layout.h>
+#include <DoubleSpinBox.h>
+#include <SpinBox.h>
+#include <ComboBox.h>
+#include <PushButton.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::sensor;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace actuator
+{
+
+BlCtrlV2::BlCtrlV2(const IODevice* parent,Layout* layout,string name,uint8_t motors_count,I2cPort* i2cport) : Bldc(parent,layout,name,motors_count)
+{
+    pimpl_=new BlCtrlV2_impl(this,layout,i2cport);
+}
+
+BlCtrlV2::~BlCtrlV2()
+{
+    delete pimpl_;
+}
+
+BatteryMonitor* BlCtrlV2::GetBatteryMonitor(void) const
+{
+    return pimpl_->battery;
+}
+
+void BlCtrlV2::SetMotors(float* values)
+{
+    pimpl_->SetMotors(values);
+}
+
+} // end namespace actuator
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/BlCtrlV2.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BlCtrlV2.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BlCtrlV2.h	(revision 3)
@@ -0,0 +1,107 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file BlCtrlV2.h
+ * \brief Class for Mikrokopter's blctrlv2
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2011/09/13
+ * \version 4.0
+ */
+
+#ifndef BLCTRLV2_H
+#define BLCTRLV2_H
+
+#include "Bldc.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class I2cPort;
+    }
+    namespace sensor
+    {
+        class BatteryMonitor;
+    }
+}
+
+class BlCtrlV2_impl;
+
+namespace flair
+{
+namespace actuator
+{
+    /*! \class BlCtrlV2
+    *
+    * \brief Class for Mikrokopter's blctrlv2
+    *
+    * blctrlv2 drivers can also monitor the battery level. See GetBatteryMonitor().
+    */
+    class BlCtrlV2 : public Bldc
+    {
+        friend class ::BlCtrlV2_impl;
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a BlCtrlV2.
+            *
+            * \param parent parent
+            * \param layout layout
+            * \param name name
+            * \param motors_count number of motors
+            * \param i2cport I2cPort
+            */
+            BlCtrlV2(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count,core::I2cPort* i2cport);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~BlCtrlV2();
+
+            /*!
+            * \brief Get battery monitor
+            *
+            * \return BatteryMonitor
+            */
+            sensor::BatteryMonitor* GetBatteryMonitor(void) const;
+
+            /*!
+            * \brief Has speed measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has speed measurement
+            */
+            bool HasSpeedMeasurement(void) const{return true;};
+
+            /*!
+            * \brief Has current measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has current measurement
+            */
+            bool HasCurrentMeasurement(void) const{return true;};
+
+        private:
+            /*!
+            * \brief Set motors values
+            *
+            * Reimplemented from Bldc. \n
+            * Values size must be the same as MotorsCount()
+            *
+            * \param values motor values
+            */
+            void SetMotors(float* values);
+
+            class BlCtrlV2_impl* pimpl_;
+    };
+} // end namespace actuator
+} // end namespace flair
+#endif // BLCTRLV2_H
Index: /trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp	(revision 3)
@@ -0,0 +1,205 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2011/09/13
+//  filename:   BlCtrlV2_impl.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c
+//
+//
+/*********************************************************************/
+#include "BlCtrlV2_impl.h"
+#include "BlCtrlV2.h"
+#include "Bldc_impl.h"
+#include "BatteryMonitor.h"
+#include "I2cPort.h"
+#include <Layout.h>
+#include <SpinBox.h>
+#include <Label.h>
+#include <cvmatrix.h>
+#include <string.h>
+
+#define BASE_ADDRESS 0x29
+#define MAX_VALUE 2047
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+using namespace flair::actuator;
+
+BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2* self,Layout *layout,I2cPort* i2cport) {
+    this->self=self;
+	this->i2cport=i2cport;
+	last_voltage_time=0;
+
+    DetectMotors();
+    //if(nb_mot!=self->MotorsCount()) self->Err("motors count different from multiplex count\n");
+
+    //BatteryMonitor
+    battery=new BatteryMonitor(layout->NewRow(),"battery");
+
+    //user interface
+    GroupBox *setupgroupbox=new GroupBox(layout->NewRow(),"Motors");
+    poles=new SpinBox(setupgroupbox->NewRow(),"nb poles:",0,255,1,14);
+}
+
+BlCtrlV2_impl::~BlCtrlV2_impl() {
+
+}
+
+void BlCtrlV2_impl::SetMotors(float* value) {
+    uint16_t tosend_value[nb_mot];
+    //stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas bloquer sur le mutex de l'output)
+    float speeds[nb_mot];
+    float currents[nb_mot];
+
+    for(int i=0;i<nb_mot;i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
+
+    i2cport->GetMutex();
+
+    for(int i=0;i<nb_mot;i++) {
+        i2cport->SetSlave(BASE_ADDRESS+i);
+        WriteValue(tosend_value[i]);
+    }
+
+    for(int i=0;i<nb_mot;i++) {
+        i2cport->SetSlave(BASE_ADDRESS+i);
+
+        if(i==0 && GetTime()>(last_voltage_time+5*(Time)1000000000)) {
+            //toute les 5 secondes sur moteur 0
+            float voltage;
+            GetCurrentSpeedAndVoltage(currents[i],speeds[i],voltage);
+            battery->SetBatteryValue(voltage);
+            last_voltage_time=GetTime();
+        } else {
+            GetCurrentAndSpeed(currents[i],speeds[i]);
+        }
+    }
+//printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
+/*
+    if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
+    {
+        i2cport->SetSlave(BASE_ADDRESS);
+        battery->SetBatteryValue(ReadVoltage());
+        last_voltage_time=GetTime();
+    }
+*/
+    i2cport->ReleaseMutex();
+
+    //on prend une fois pour toute le mutex et on fait des accès directs
+    cvmatrix* output=self->output;
+    output->GetMutex();
+    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,speeds[i]);
+    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,1,currents[i]);
+    output->ReleaseMutex();
+}
+
+//I2cPort mutex must be taken before calling this function
+void BlCtrlV2_impl::WriteValue(uint16_t value) {
+    uint8_t tx[2];
+    ssize_t written;
+
+    tx[0]=(uint8_t)(value>>3);//msb
+    tx[1]=(value&0x07);;//+16+8; //pour recuperer la vitesse en premier
+    written =i2cport->Write(tx, 2);
+    if(written<0) {
+        self->Err("rt_dev_write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(tx)) {
+        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
+    }
+}
+
+//I2cPort mutex must be taken before calling this function
+void BlCtrlV2_impl::GetCurrentAndSpeed(float &current,float &speed) {
+    ssize_t read;
+    uint8_t value[4];
+    read = i2cport->Read(value, sizeof(value));
+
+    if(read<0) {
+        self->Err("rt_dev_read error (%s)\n",strerror(-read));
+        speed=-1;
+        current=-1;
+    } else if (read != sizeof(value)) {
+        self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
+        speed=-1;
+        current=-1;
+    } else {
+        current=value[0]/10.;
+        speed= value[3]*780./poles->Value();
+    }
+}
+
+//I2cPort mutex must be taken before calling this function
+void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage) {
+    ssize_t read;
+    uint8_t value[6];
+    read = i2cport->Read(value, sizeof(value));
+
+    if(read<0) {
+        self->Err("rt_dev_read error (%s)\n",strerror(-read));
+        speed=-1;
+        voltage=-1;
+        current=-1;
+    } else if (read != sizeof(value)) {
+        self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
+        speed=-1;
+        voltage=-1;
+        current=-1;
+    } else {
+        current=value[0]/10.;
+        voltage=value[5]/10.;
+        speed= value[3]*780./poles->Value();
+    }
+}
+
+void BlCtrlV2_impl::DetectMotors(void) {
+    int nb=0;
+    ssize_t read,nb_write;
+    uint8_t value[3];
+    uint8_t tx[2];
+
+    i2cport->GetMutex();
+
+    for(int i=0;i<MAX_MOTORS;i++) {
+        //printf("test %i\n",i);
+        i2cport->SetSlave(BASE_ADDRESS+i);
+        tx[0]=0;
+        tx[1]=16+8;//16+8 pour recuperer la vitesse
+
+        nb_write = i2cport->Write(tx, 2);
+
+        if (nb_write != sizeof(tx)) {
+            continue;
+        }
+        nb++;
+    }
+
+    for(int i=0;i<MAX_MOTORS;i++) {
+        i2cport->SetSlave(BASE_ADDRESS+i);
+        read = i2cport->Read(value, 3);
+
+        if (read != sizeof(value)) {
+            continue;
+        }
+    }
+
+    i2cport->ReleaseMutex();
+
+    Printf("BlCtrlV2: Dectected motors: %i\n",nb);
+    if(nb==4) {
+        Printf("BlCtrlV2: Configuration X4\n");
+    } else if(nb==8) {
+        Printf("BlCtrlV2: Configuration X8\n");
+    } else {
+        //self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
+    }
+
+    nb_mot=nb;
+}
Index: /trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp	(revision 3)
@@ -0,0 +1,545 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/29
+//  filename:   BlCtrlV2_x4_speed.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c, controle en vitesse
+//
+//
+/*********************************************************************/
+
+#include "BlCtrlV2_x4_speed.h"
+#include "I2cPort.h"
+#include <TabWidget.h>
+#include <Tab.h>
+#include <GroupBox.h>
+#include <SpinBox.h>
+#include <DoubleSpinBox.h>
+#include <ComboBox.h>
+#include <PushButton.h>
+#include <cvmatrix.h>
+#include <Mutex.h>
+#include <FrameworkManager.h>
+#include <DataPlot1D.h>
+#include <math.h>
+#include <string.h>
+
+#define TAU_US 1000
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace actuator
+{
+BlCtrlV2_x4_speed::BlCtrlV2_x4_speed(FrameworkManager* parent,string name,I2cPort* i2cport,uint8_t base_address,uint8_t priority) : Thread(parent,name,priority),IODevice(parent,name)
+{
+    this->i2cport=i2cport;
+	slave_address=base_address;
+	tested_motor=-1;
+	enabled=false;
+	int_av_g=0;
+	int_av_d=0;
+	int_ar_g=0;
+	int_ar_d=0;
+
+	//flight time
+	FILE *file;
+    file=fopen("/etc/flight_time","r");
+    if (file == NULL)
+    {
+        Printf("fichier d'info de vol vide\n");
+        time_sec=0;
+    }
+    else
+    {
+        char ligne[32];
+        fgets(ligne, 32, file);
+        time_sec=atoi(ligne);
+        Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
+        fclose(file);
+    }
+
+    //station sol
+    main_tab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+	Tab *sensor_tab=new Tab(tab,"Reglages");
+	reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
+            poles=new SpinBox(reglages_groupbox->NewRow(),"nb poles",0,255,1);
+            kp=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"kp",0.,255,0.001,4);
+            ki=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"ki",0.,255,0.001,4);
+            min=new SpinBox(reglages_groupbox->NewRow(),"min pwm",0.,2048,1);
+            max=new SpinBox(reglages_groupbox->LastRowLastCol(),"max pwm",0.,2048,1);
+            test=new SpinBox(reglages_groupbox->LastRowLastCol(),"test value",0.,2048,1);
+            start_value=new SpinBox(reglages_groupbox->NewRow(),"valeur demarrage",0,10000,10);
+            trim=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"pas decollage",0,1000,.1);
+
+            av_g=new ComboBox(reglages_groupbox->NewRow(),"avant gauche");
+            av_g->AddItem("1");
+            av_g->AddItem("2");
+            av_g->AddItem("3");
+            av_g->AddItem("4");
+            button_avg=new PushButton(reglages_groupbox->LastRowLastCol(),"test avg");
+
+            av_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"avant droite:");
+            av_d->AddItem("1");
+            av_d->AddItem("2");
+            av_d->AddItem("3");
+            av_d->AddItem("4");
+            button_avd=new PushButton(reglages_groupbox->LastRowLastCol(),"test avd");
+
+            ar_g=new ComboBox(reglages_groupbox->NewRow(),"arriere gauche:");
+            ar_g->AddItem("1");
+            ar_g->AddItem("2");
+            ar_g->AddItem("3");
+            ar_g->AddItem("4");
+            button_arg=new PushButton(reglages_groupbox->LastRowLastCol(),"test arg");
+
+            ar_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"arriere droite:");
+            ar_d->AddItem("1");
+            ar_d->AddItem("2");
+            ar_d->AddItem("3");
+            ar_d->AddItem("4");
+            button_ard=new PushButton(reglages_groupbox->LastRowLastCol(),"test ard");
+
+            pas=new ComboBox(reglages_groupbox->NewRow(),"pas helice avant gauche:");
+            pas->AddItem("normal");
+            pas->AddItem("inverse");
+
+    input=new cvmatrix((IODevice*)this,8,1,floatType);
+
+    cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,2);
+    desc->SetElementName(0,0,"avant gauche");
+	desc->SetElementName(1,0,"arriere droite");
+	desc->SetElementName(2,0,"avant droite");
+	desc->SetElementName(3,0,"arriere gauche");
+
+	desc->SetElementName(0,1,"cons avant gauche");
+	desc->SetElementName(1,1,"cons arriere droite");
+	desc->SetElementName(2,1,"cons avant droite");
+	desc->SetElementName(3,1,"cons arriere gauche");
+    output=new cvmatrix((IODevice*)this,desc,floatType);
+
+
+
+/*
+
+//le 3ieme lu est la tension batteire
+	if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
+    uint16_t pwm_moteur;
+    pwm_moteur=0;
+    ssize_t read;
+    uint8_t rx[8];
+    SetSlave(slave_address);
+
+    for(int j=0;j<10;j++)
+    {
+
+
+        WriteValue(pwm_moteur);
+
+
+        read = rt_dev_read(i2c_fd, rx, sizeof(rx));
+
+        if(read<0)
+        {
+            rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
+        }
+        else if (read != sizeof(rx))
+        {
+            rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
+
+        }
+        for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
+
+        printf("\n");
+
+    }
+
+    if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
+}
+
+BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void)
+{
+    SafeStop();
+    Join();
+    delete main_tab;
+}
+
+void BlCtrlV2_x4_speed::UseDefaultPlot(void)
+{
+    Tab *plot_tab=new Tab(tab,"Mesures");
+        DataPlot1D *av_g_plot=new DataPlot1D(plot_tab->NewRow(),"avg",0,10000);
+            av_g_plot->AddCurve(output->Element(0,0));
+            av_g_plot->AddCurve(output->Element(0,1),DataPlot::Blue);
+        DataPlot1D *av_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"avd",0,10000);
+            av_d_plot->AddCurve(output->Element(2,0));
+            av_d_plot->AddCurve(output->Element(2,1),DataPlot::Blue);
+        DataPlot1D *ar_g_plot=new DataPlot1D(plot_tab->NewRow(),"arg",0,10000);
+            ar_g_plot->AddCurve(output->Element(3,0));
+            ar_g_plot->AddCurve(output->Element(3,1),DataPlot::Blue);
+        DataPlot1D *ar_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"ard",0,10000);
+            ar_d_plot->AddCurve(output->Element(1,0));
+            ar_d_plot->AddCurve(output->Element(1,1),DataPlot::Blue);
+}
+
+float BlCtrlV2_x4_speed::TrimValue(void)
+{
+    return (float)trim->Value();
+}
+
+int BlCtrlV2_x4_speed::StartValue(void)
+{
+    return start_value->Value();
+}
+
+void BlCtrlV2_x4_speed::Run(void)
+{
+    WarnUponSwitches(true);
+
+    SetPeriodUS(TAU_US);
+
+    while(!ToBeStopped())
+    {
+        WaitPeriod();
+
+        Update();
+    }
+
+    WarnUponSwitches(false);
+}
+
+void BlCtrlV2_x4_speed::Update(void)
+{
+    float u_roll,u_pitch,u_yaw,u_gaz;
+    float trim_roll,trim_pitch,trim_yaw;
+    float pwm[4];
+    uint16_t pwm_moteur[4];
+
+
+    //on prend une fois pour toute le mutex et on fait des accès directs
+    input->GetMutex();
+
+    u_roll=input->ValueNoMutex(0,0);
+    u_pitch=input->ValueNoMutex(1,0);
+    u_yaw=input->ValueNoMutex(2,0);
+    u_gaz=input->ValueNoMutex(3,0)+input->ValueNoMutex(7,0)*input->ValueNoMutex(7,0);//ugaz+trim*trim
+    trim_roll=input->ValueNoMutex(4,0);
+    trim_pitch=input->ValueNoMutex(5,0);
+    trim_yaw=input->ValueNoMutex(6,0);
+
+    input->ReleaseMutex();
+
+    if(pas->CurrentIndex()==1)
+    {
+        trim_yaw=-trim_yaw;
+        u_yaw=-u_yaw;
+    }
+
+    //rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
+    //if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
+
+    //avant gauche
+    if(u_gaz+u_pitch+u_roll+u_yaw>0)
+    {
+        pwm[0]=trim_pitch+trim_roll+trim_yaw+sqrtf(u_gaz+u_pitch+u_roll+u_yaw);
+    }else
+    {
+        pwm[0]=trim_pitch+trim_roll+trim_yaw;
+    }
+
+    //arriere gauche
+    if(u_gaz-u_pitch+u_roll-u_yaw>0)
+    {
+        pwm[3]=-trim_pitch+trim_roll-trim_yaw+sqrtf(u_gaz-u_pitch+u_roll-u_yaw);
+    }else
+    {
+        pwm[3]=-trim_pitch+trim_roll-trim_yaw;
+    }
+
+    //arriere droit
+    if(u_gaz-u_pitch-u_roll+u_yaw>0)
+    {
+        pwm[1]=-trim_pitch-trim_roll+trim_yaw+sqrtf(u_gaz-u_pitch-u_roll+u_yaw);
+    }else
+    {
+        pwm[1]=-trim_pitch-trim_roll+trim_yaw;
+    }
+
+    //avant droit
+    if(u_gaz+u_pitch-u_roll-u_yaw>0)
+    {
+        pwm[2]=trim_pitch-trim_roll-trim_yaw+sqrtf(u_gaz+u_pitch-u_roll-u_yaw);
+    }else
+    {
+        pwm[2]=trim_pitch-trim_roll-trim_yaw;
+    }
+
+
+    int_av_g+=ki->Value()*(pwm[0]-speed_av_g);
+    pwm[0]=kp->Value()*(pwm[0]-speed_av_g)+int_av_g;
+
+    int_ar_g+=ki->Value()*(pwm[3]-speed_ar_g);
+    pwm[3]=kp->Value()*(pwm[3]-speed_ar_g)+int_ar_g;
+
+    int_ar_d+=ki->Value()*(pwm[1]-speed_ar_d);
+    pwm[1]=kp->Value()*(pwm[1]-speed_ar_d)+int_ar_d;
+
+    int_av_d+=ki->Value()*(pwm[2]-speed_av_d);
+    pwm[2]=kp->Value()*(pwm[2]-speed_av_d)+int_av_d;
+
+//rt_printf("%f\n",pwm[0]);
+    for(int i=0;i<4;i++) pwm_moteur[i]=SatPWM(pwm[i],min->Value(),max->Value());
+
+    if(button_avg->Clicked()==true)
+    {
+        tested_motor=0;
+        StartTest();
+    }
+    if(button_avd->Clicked()==true)
+    {
+        tested_motor=2;
+        StartTest();
+    }
+    if(button_arg->Clicked()==true)
+    {
+        tested_motor=3;
+        StartTest();
+    }
+    if(button_ard->Clicked()==true)
+    {
+        tested_motor=1;
+        StartTest();
+    }
+
+    if(tested_motor!=-1)
+    {
+        for(int i=0;i<4;i++)
+        {
+            pwm_moteur[i]=0;
+        }
+        pwm_moteur[tested_motor]=(uint16_t)test->Value();
+
+        if(GetTime()>(start_time+2*1000000000)) StopTest();
+    }
+
+
+    i2cport->GetMutex();
+
+    if(enabled==true)
+    {
+        i2cport->SetSlave(slave_address+av_g->CurrentIndex());
+        WriteValue(pwm_moteur[0]);
+
+        i2cport->SetSlave(slave_address+av_d->CurrentIndex());
+        WriteValue(pwm_moteur[2]);
+
+        i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
+        WriteValue(pwm_moteur[3]);
+
+        i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
+        WriteValue(pwm_moteur[1]);
+
+    }
+    else
+    {
+        for(int i=0;i<4;i++)
+        {
+            i2cport->SetSlave(slave_address+i);
+            WriteValue(0);
+        }
+        int_av_g=0;
+        int_av_d=0;
+        int_ar_g=0;
+        int_ar_d=0;
+    }
+
+    i2cport->SetSlave(slave_address+av_g->CurrentIndex());
+    speed_av_g=GetSpeed();
+
+    i2cport->SetSlave(slave_address+av_d->CurrentIndex());
+    speed_av_d=GetSpeed();
+
+    i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
+    speed_ar_g=GetSpeed();
+
+    i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
+    speed_ar_d=GetSpeed();
+
+    i2cport->ReleaseMutex();
+
+
+    //on prend une fois pour toute le mutex et on fait des accès directs
+    output->GetMutex();
+    output->SetValueNoMutex(0,0,speed_av_g);
+    output->SetValueNoMutex(1,0,speed_ar_d);
+    output->SetValueNoMutex(2,0,speed_av_d);
+    output->SetValueNoMutex(3,0,speed_ar_g);
+   // rt_printf("%i %i %i %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
+    output->ReleaseMutex();
+
+    output->SetDataTime(GetTime());
+    ProcessUpdate(output);
+
+}
+
+void BlCtrlV2_x4_speed::StartTest(void)
+{
+    start_time=GetTime();
+    SetEnabled(true);
+}
+
+void BlCtrlV2_x4_speed::StopTest(void)
+{
+    SetEnabled(false);
+    tested_motor=-1;
+}
+
+uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons,uint16_t min,uint16_t max)
+{
+  uint16_t sat_value=(uint16_t)vel_cons;
+
+  if(vel_cons>((float)sat_value+0.5)) sat_value++;
+
+  if(vel_cons<(float)min) sat_value=min;
+  if(vel_cons>(float)max) sat_value=max;
+
+  return sat_value;
+}
+
+void BlCtrlV2_x4_speed::LockUserInterface(void)
+{
+    reglages_groupbox->setEnabled(false);
+}
+
+void BlCtrlV2_x4_speed::UnlockUserInterface(void)
+{
+    reglages_groupbox->setEnabled(true);
+}
+
+void BlCtrlV2_x4_speed::SetEnabled(bool status)
+{
+    enabled=status;
+    if(enabled==true)
+    {
+        LockUserInterface();
+
+        flight_start_time = GetTime();
+    }
+    else
+    {
+        UnlockUserInterface();
+
+        Time now= GetTime();
+        int t_sec;
+        FILE *file;
+        char ligne[32];
+
+        t_sec=(now - flight_start_time)/1000000000;
+        time_sec+=t_sec;
+
+        Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
+        Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
+
+        file=fopen("/etc/flight_time","w");
+        if (file == NULL)
+        {
+            Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
+        }
+        else
+        {
+            sprintf(ligne,"%i",time_sec);
+            fputs(ligne,file);
+            fclose(file);
+        }
+    }
+}
+
+void BlCtrlV2_x4_speed::SetUroll(float value)
+{
+    input->SetValue(0,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetUpitch(float value)
+{
+    input->SetValue(1,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetUyaw(float value)
+{
+    input->SetValue(2,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetUgaz(float value)
+{
+    input->SetValue(3,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetRollTrim(float value)
+{
+    input->SetValue(4,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetPitchTrim(float value)
+{
+    input->SetValue(5,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetYawTrim(float value)
+{
+    input->SetValue(6,0,value);
+}
+
+void BlCtrlV2_x4_speed::SetGazTrim(float value)
+{
+    input->SetValue(7,0,value);
+}
+
+void BlCtrlV2_x4_speed::WriteValue(uint16_t value)
+{
+    unsigned char tx[2];
+    ssize_t written;
+
+    tx[0]=(unsigned char)(value>>3);//msb
+    tx[1]=16+8+(value&0x07);//16+8 pour recuperer la vitesse
+    written = i2cport->Write(tx, 2);
+    if(written<0)
+    {
+        Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
+    }
+    else if (written != 2)
+    {
+        Thread::Err("erreur rt_dev_write %i/2\n",written);
+    }
+}
+
+float BlCtrlV2_x4_speed::GetSpeed(void)
+{
+    ssize_t read;
+    uint8_t value;
+    read = i2cport->Read(&value, 1);
+
+    if(read<0)
+    {
+        Thread::Err("erreur rt_dev_read (%s)\n",strerror(-read));
+    }
+    else if (read != 1)
+    {
+        Thread::Err("erreur rt_dev_read %i/2\n",read);
+
+    }
+
+    return value*780./poles->Value();
+}
+
+} // end namespace actuator
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h	(revision 3)
@@ -0,0 +1,119 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/29
+//  filename:   BlCtrlV2_x4_speed.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c, controle en vitesse
+//
+//
+/*********************************************************************/
+
+
+#ifndef BLCTRLV2_X4_SPEED_H
+#define BLCTRLV2_X4_SPEED_H
+
+#include <IODevice.h>
+#include <Thread.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class cvmatrix;
+        class FrameworkManager;
+        class I2cPort;
+
+    }
+    namespace gui
+    {
+        class TabWidget;
+        class Tab;
+        class SpinBox;
+        class DoubleSpinBox;
+        class ComboBox;
+        class PushButton;
+        class GroupBox;
+    }
+}
+
+namespace flair
+{
+namespace actuator
+{
+    class BlCtrlV2_x4_speed : public core::Thread,public core::IODevice
+    {
+
+        public:
+            BlCtrlV2_x4_speed(core::FrameworkManager* parent,std::string name,core::I2cPort* i2cport,uint8_t base_address,uint8_t priority);
+            ~BlCtrlV2_x4_speed();
+            void UseDefaultPlot(void);
+            void LockUserInterface(void);
+            void UnlockUserInterface(void);
+            void SetEnabled(bool status);
+            void SetUroll(float value);
+            void SetUpitch(float value);
+            void SetUyaw(float value);
+            void SetUgaz(float value);
+            void SetRollTrim(float value);
+            void SetPitchTrim(float value);
+            void SetYawTrim(float value);
+            void SetGazTrim(float value);
+            float TrimValue(void);
+            int StartValue(void);
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(core::io_data *data){};
+            void WriteValue(uint16_t value);
+            float GetSpeed(void);
+            void StartTest(void);
+            void StopTest(void);
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+            void Update(void);
+            gui::Tab* main_tab;
+            gui::TabWidget* tab;
+            gui::GroupBox* reglages_groupbox;
+            gui::SpinBox *min,*max,*test;
+            gui::PushButton *button_avg,*button_avd,*button_arg,*button_ard;
+            gui::ComboBox *av_g,*av_d,*ar_g,*ar_d,*pas;
+            gui::DoubleSpinBox *trim,*kp,*ki;
+            gui::SpinBox *start_value,*poles;
+            core::Time start_time,flight_start_time;
+            int time_sec;
+            float speed_av_g,speed_av_d,speed_ar_g,speed_ar_d;
+            float int_av_g,int_av_d,int_ar_g,int_ar_d;
+
+            //matrix
+            core::cvmatrix *input;
+            core::cvmatrix *output;
+
+            int tested_motor;
+            core::I2cPort* i2cport;
+            uint8_t slave_address;
+
+            bool enabled;
+
+            uint16_t SatPWM(float vel_cons,uint16_t min,uint16_t max);
+    };
+} // end namespace actuator
+} // end namespace flair
+#endif // BLCTRLV2_X4_SPEED_H
Index: /trunk/lib/FlairSensorActuator/src/Bldc.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Bldc.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Bldc.cpp	(revision 3)
@@ -0,0 +1,107 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/11/14
+//  filename:   Bldc.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Virtual class for brushless drivers
+//
+//
+/*********************************************************************/
+
+#include "Bldc.h"
+#include "Bldc_impl.h"
+#include <cvmatrix.h>
+#include <DoubleSpinBox.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace actuator {
+
+Bldc::Bldc(const IODevice* parent,Layout* layout,string name,uint8_t motors_count) : IODevice(parent,name) {
+    pimpl_=new Bldc_impl(this,layout,name,motors_count);
+
+    cvmatrix_descriptor* desc=new cvmatrix_descriptor(motors_count,2);
+    for(int i=0;i<motors_count;i++) {
+        ostringstream speed,current;
+        speed << "speed_" << i;
+        desc->SetElementName(i,0,speed.str());
+
+        current << "current_" << i;
+        desc->SetElementName(i,1,current.str());
+    }
+
+    output=new cvmatrix(this,desc,floatType);
+    AddDataToLog(output);
+}
+
+Bldc::Bldc(const Object* parent,string name,uint8_t motors_count) : IODevice(parent,name) {
+    pimpl_=new Bldc_impl(this,motors_count);
+}
+
+Bldc::~Bldc() {
+    delete pimpl_;
+}
+
+void Bldc::UpdateFrom(const io_data *data) {
+    pimpl_->UpdateFrom(data);
+}
+
+void Bldc::LockUserInterface(void) const {
+    pimpl_->LockUserInterface();
+}
+
+void Bldc::UnlockUserInterface(void) const {
+    pimpl_->UnlockUserInterface();
+}
+
+Layout* Bldc::GetLayout(void) const {
+    return (Layout*)pimpl_->layout;
+}
+
+void Bldc::UseDefaultPlot(TabWidget* tabwidget) {
+    pimpl_->UseDefaultPlot(tabwidget);
+}
+
+uint8_t Bldc::MotorsCount(void) const {
+    return pimpl_->motors_count;
+}
+
+cvmatrix *Bldc::Output(void) const {
+    return output;
+}
+
+bool Bldc::AreEnabled(void) const {
+    return pimpl_->are_enabled;
+}
+
+void Bldc::SetEnabled(bool status) {
+    if(pimpl_->are_enabled!=status) {
+        pimpl_->are_enabled=status;
+        if(pimpl_->are_enabled) {
+            LockUserInterface();
+        } else {
+            UnlockUserInterface();
+        }
+    }
+}
+
+void Bldc::SetPower(int motor_id,float value) {
+    //use output mutex to avoid making a new mutex
+    output->GetMutex();
+    pimpl_->power[motor_id]=value;
+    output->ReleaseMutex();
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Bldc.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Bldc.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Bldc.h	(revision 3)
@@ -0,0 +1,191 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Bldc.h
+ * \brief Base class for brushless motors drivers
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/11/14
+ * \version 4.0
+ */
+
+#ifndef BLDC_H
+#define BLDC_H
+
+#include <IODevice.h>
+#include <stdint.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class cvmatrix;
+    }
+    namespace gui
+    {
+        class Layout;
+        class TabWidget;
+    }
+}
+
+class Bldc_impl;
+
+namespace flair
+{
+namespace actuator
+{
+    /*! \class Bldc
+    *
+    * \brief Base class for brushless motors drivers
+    */
+    class Bldc : public core::IODevice
+    {
+        friend class ::Bldc_impl;
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Bldc.
+            *
+            * \param parent parent
+            * \param layout layout
+            * \param name name
+            * \param motors_count number of motors
+            */
+            Bldc(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Bldc. \n
+            * This contructor must only be called for a simulated device.
+            *
+            * \param parent parent
+            * \param name name
+            * \param motors_count number of motors
+            */
+            Bldc(const core::Object* parent,std::string name,uint8_t motors_count);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Bldc();
+
+            /*!
+            * \brief Lock user interface
+            *
+            */
+            void LockUserInterface(void) const;
+
+            /*!
+            * \brief Unlock user interface
+            *
+            */
+            void UnlockUserInterface(void) const;
+
+            /*!
+            * \brief Use default plot
+            *
+            * \param tabwidget TabWidget to draw plots
+            */
+            void UseDefaultPlot(gui::TabWidget* tabwidget);
+
+            /*!
+            * \brief Output from motors
+            *
+            * First column is real speed if available, secund column is current if available
+            *
+            */
+            core::cvmatrix *Output(void) const;
+
+            /*!
+            * \brief Motors count
+            *
+            * \return number of motors
+            */
+            uint8_t MotorsCount(void) const;
+
+            /*!
+            * \brief Enable motors
+            *
+            * \param true to enable all motors
+            */
+            void SetEnabled(bool status);
+
+            /*!
+            * \brief Are motors enabled?
+            *
+            * \return true if motors are enabled
+            */
+            bool AreEnabled(void) const;
+
+            /*!
+            * \brief Set motor power
+            *
+            * Changes the power (from 0 to 1) of a specific motor. \n
+            * By default power is set to 1 for each motor which has no effect. \n
+            * A value <1 will decrease the power of a motor sent to the reimplemented Bldc class through SetMotors. \n
+            * The power value is applied after applying saturation between min value and max value.
+            * So the resulting value cannot be higher than max value
+            * but it can be lower than min value.
+            *
+            * \param motor_id id of the motor
+            * \param value power value (from 0 to 1)
+            *
+            */
+            void SetPower(int motor_id,float value);
+
+            /*!
+            * \brief Layout
+            *
+            * This the same Layout as passed to the constructor
+            *
+            * \return a Layout
+            */
+            gui::Layout* GetLayout(void) const;
+
+            /*!
+            * \brief Has speed measurement
+            *
+            * \return true if it has speed measurement
+            */
+            virtual bool HasSpeedMeasurement(void) const=0;
+
+            /*!
+            * \brief Has current measurement
+            *
+            * \return true if it has current measurement
+            */
+            virtual bool HasCurrentMeasurement(void) const=0;
+
+        protected:
+            core::cvmatrix *output;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data);
+
+            /*!
+            * \brief Set motors values
+            *
+            * values size must be the same as MotorsCount()
+            *
+            * \param values set motors values
+            */
+            virtual void SetMotors(float* values)=0;
+
+            class Bldc_impl* pimpl_;
+    };
+} // end namespace actuator
+} // end namespace framewor
+#endif // BLDC_H
Index: /trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp	(revision 3)
@@ -0,0 +1,202 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/11/14
+//  filename:   Bldc_impl.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Virtual class for brushless drivers
+//
+//
+/*********************************************************************/
+#include "Bldc_impl.h"
+#include "Bldc.h"
+#include <GridLayout.h>
+#include <GroupBox.h>
+#include <DoubleSpinBox.h>
+#include <cvmatrix.h>
+#include <Label.h>
+#include <PushButton.h>
+#include <DataPlot1D.h>
+#include <TabWidget.h>
+#include <Tab.h>
+#include <FrameworkManager.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::actuator;
+
+Bldc_impl::Bldc_impl(Bldc* self,Layout* layout,string name,uint8_t motors_count) {
+    this->self=self;
+    this->motors_count=motors_count;
+    this->layout=layout;
+	are_enabled=false;
+	is_running=false;
+	tested_motor=-1;
+
+    values=(float*)malloc(motors_count*sizeof(float));
+    power=(float*)malloc(motors_count*sizeof(float));
+    for(int i=0;i<motors_count;i++) power[i]=1;
+
+    //station sol
+	GroupBox* groupbox=new GroupBox(layout->NewRow(),"bldc");
+        flight_time=new Label(groupbox->NewRow(),"flight time");
+        min_value=new DoubleSpinBox(groupbox->NewRow(),"min value:",0,1,.1,2);
+        max_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"max value:",0,1,.1,2);
+        test_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"test value:",0,1,0.1);
+
+    int index=0;
+    button_test=(PushButton**)malloc(motors_count*sizeof(PushButton*));
+    for(int i=0;i<motors_count/2;i++) {
+        for(int j=0;j<2;j++) {
+            ostringstream test_name;
+            test_name << "test motor " << index;
+            button_test[index]=new PushButton(groupbox->At(2+i,j),test_name.str());
+            index++;
+        }
+    }
+
+	//flight time
+	FILE *file;
+    file=fopen("/etc/flight_time","r");
+    if (file == NULL) {
+        time_sec=0;
+    } else {
+        char ligne[32];
+        fgets(ligne, 32, file);
+        time_sec=atoi(ligne);
+        fclose(file);
+    }
+    flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
+}
+
+Bldc_impl::Bldc_impl(Bldc* self,uint8_t motors_count) {
+    this->self=self;
+    this->motors_count=motors_count;
+    values=NULL;
+    button_test=NULL;
+    power=NULL;
+}
+
+Bldc_impl::~Bldc_impl() {
+    if(values!=NULL) free(values);
+    if(button_test!=NULL) free(button_test);
+    if(power!=NULL) free(power);
+}
+
+void Bldc_impl::UseDefaultPlot(TabWidget* tab) {
+    Tab* plot_tab=new Tab(tab,"speeds");
+    plots=new DataPlot1D(plot_tab->NewRow(),"values",0,1);
+    for(int i=0;i<motors_count;i++) {
+        plots->AddCurve(self->output->Element(i));
+    }
+}
+
+void Bldc_impl::UpdateFrom(const io_data *data) {
+    cvmatrix* input=(cvmatrix*)data;
+    bool is_motor_running=false;
+
+    if(values==NULL) return;//nothing to do in simulator
+
+    if(input->Rows()!=motors_count) {
+        self->Err("nb motors mismatch\n");
+        return;
+    }
+
+    input->GetMutex();
+    for(int i=0;i<motors_count;i++) {
+        if(input->ValueNoMutex(i,0)!=0) is_motor_running=true;
+        if(are_enabled) {
+            //Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
+            values[i]=power[i]*Sat(input->ValueNoMutex(i,0));
+            //Printf("%i %f\n",i,values[i]);
+        } else {
+            values[i]=0;
+        }
+    }
+    input->ReleaseMutex();
+
+    if(are_enabled && is_motor_running && !is_running) {
+        flight_start_time = GetTime();
+        is_running=true;
+    }
+    if((!are_enabled || !is_motor_running) && is_running) {
+        Time now= GetTime();
+        int t_sec;
+        FILE *file;
+        char ligne[32];
+
+        t_sec=(now - flight_start_time)/1000000000;
+        time_sec+=t_sec;
+
+        Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
+        //Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
+        flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
+
+        file=fopen("/etc/flight_time","w");
+        if (file == NULL) {
+            Printf("Erreur a l'ouverture du fichier d'info vol\n");
+        } else {
+            sprintf(ligne,"%i",time_sec);
+            fputs(ligne,file);
+            fclose(file);
+        }
+        is_running=false;
+    }
+
+    for(int i=0;i<motors_count;i++) {
+        if(button_test[i]->Clicked()==true) {
+            if(!are_enabled) {
+                tested_motor=i;
+                test_start_time=GetTime();
+                LockUserInterface();
+            } else {
+                self->Warn("testing motor is not possible when enabled\n");
+            }
+        }
+    }
+    if(tested_motor!=-1) {
+        for(int i=0;i<motors_count;i++) {
+            values[i]=0;
+        }
+        values[tested_motor]=test_value->Value();
+
+        if(GetTime()>(test_start_time+2*1000000000)) {
+            tested_motor=-1;
+            UnlockUserInterface();
+        }
+    }
+
+    self->SetMotors(values);
+    self->output->SetDataTime(data->DataTime());
+    self->ProcessUpdate(self->output);
+}
+
+float Bldc_impl::Sat(float value) {
+    float result=value;
+
+    if(result<min_value->Value()) {
+        result=min_value->Value();
+    }
+    if(result>max_value->Value()) {
+        result=max_value->Value();
+    }
+
+    return result;
+}
+
+void Bldc_impl::LockUserInterface(void) const {
+    layout->setEnabled(false);
+}
+
+void Bldc_impl::UnlockUserInterface(void) const {
+    layout->setEnabled(true);
+}
Index: /trunk/lib/FlairSensorActuator/src/Camera.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Camera.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Camera.cpp	(revision 3)
@@ -0,0 +1,113 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/03/06
+//  filename:   Camera.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Virtual class for Camera
+//
+//
+/*********************************************************************/
+
+#include "Camera.h"
+#include <FrameworkManager.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <GroupBox.h>
+#include <GridLayout.h>
+#include <DataPlot1D.h>
+#include <Picture.h>
+#include <highgui.h>
+#include <fstream>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace sensor {
+
+Camera::Camera(const FrameworkManager* parent,string name,uint16_t width,uint16_t height,cvimage::Type::Format format) : IODevice(parent,name) {
+    plot_tab=NULL;
+
+    //do not allocate imagedata, allocation is done by the camera
+    output=new cvimage((IODevice*)this,width,height,format,"out",false);
+
+    //station sol
+    main_tab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+	sensor_tab=new Tab(tab,"Setup");
+	setup_groupbox=new GroupBox(sensor_tab->NewRow(),name);
+	setup_layout=new GridLayout(sensor_tab->NewRow(),"setup");
+}
+
+Camera::Camera(const IODevice* parent,std::string name) : IODevice(parent,name) {
+    plot_tab=NULL;
+    main_tab=NULL;
+    tab=NULL;
+	sensor_tab=NULL;
+	setup_groupbox=NULL;
+
+    output=NULL;
+}
+
+Camera::~Camera() {
+    if(main_tab!=NULL) delete main_tab;
+}
+
+DataType const &Camera::GetOutputDataType() const {
+    return output->GetDataType();
+}
+
+GroupBox* Camera::GetGroupBox(void) const {
+    return setup_groupbox;
+}
+
+GridLayout* Camera::GetLayout(void) const {
+    return setup_layout;
+}
+
+void Camera::UseDefaultPlot(const core::cvimage *image) {
+    if(tab==NULL) {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    plot_tab=new Tab(tab,"Picture");
+        Picture* plot=new Picture(plot_tab->NewRow(),ObjectName(),image);
+}
+
+Tab* Camera::GetPlotTab(void) const {
+    return plot_tab;
+}
+
+uint16_t Camera::Width(void) const {
+    return output->GetDataType().GetWidth();
+}
+
+uint16_t Camera::Height(void) const {
+    return output->GetDataType().GetHeight();
+}
+
+core::cvimage* Camera::Output(void) {
+    return output;
+}
+
+void Camera::SaveToFile(string filename) const {
+    Printf("saving %s, size %i\n",filename.c_str(),output->img->imageSize);
+    std::ofstream pFile;
+	pFile.open (filename);
+	output->GetMutex();
+	pFile.write(output->img->imageData, output->img->imageSize);
+    output->ReleaseMutex();
+
+	pFile.close();
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Camera.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Camera.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Camera.h	(revision 3)
@@ -0,0 +1,147 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Camera.h
+ * \brief Base class for Camera
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/03/06
+ * \version 4.0
+ */
+
+#ifndef CAMERA_H
+#define CAMERA_H
+
+#include <IODevice.h>
+#include <stdint.h>
+#include <cvimage.h>
+
+namespace flair
+{
+    namespace gui
+    {
+        class GroupBox;
+        class Tab;
+        class TabWidget;
+        class Picture;
+        class GridLayout;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class Camera
+    *
+    * \brief Base class for Camera
+    *
+    * Use this class to define a custom Camera.
+    *
+    */
+    class Camera : public core::IODevice
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Camera.
+            *
+            * \param parent parent
+            * \param name name
+            * \param width width
+            * \param height height
+            * \param format image format
+            */
+            Camera(const core::FrameworkManager* parent,std::string name,uint16_t width,uint16_t height,core::cvimage::Type::Format format);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Camera. \n
+            * This contructor must only be called for a simulated device.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            Camera(const core::IODevice* parent,std::string name);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Camera();
+
+            /*!
+            * \brief Use default plot
+            *
+            * \param image image to display
+            */
+            void UseDefaultPlot(const core::cvimage *image);
+
+            /*!
+            * \brief get Layout
+            *
+            * \return a Layout available
+            */
+            gui::GridLayout* GetLayout(void) const;
+
+            /*!
+            * \brief plot tab
+            *
+            * \return plot tab
+            */
+            gui::Tab* GetPlotTab(void) const;
+
+            /*!
+            * \brief Save picture to file
+            *
+            * \param filename filename
+            */
+            void SaveToFile(std::string filename) const;
+
+            /*!
+            * \brief Width
+            *
+            * \return width
+            */
+            uint16_t Width(void) const;
+
+            /*!
+            * \brief Height
+            *
+            * \return height
+            */
+            uint16_t Height(void) const;
+
+             /*!
+            * \brief Output matrix
+            *
+            * Output matrix is of the same size as declared in constructor. \n
+            *
+            * \return the output matrix
+            */
+            core::cvimage* Output(void);
+
+            core::DataType const &GetOutputDataType() const;
+
+        protected:
+            /*!
+            * \brief get GroupBox
+            *
+            * \return a GroupBox available
+            */
+            gui::GroupBox* GetGroupBox(void) const;
+
+            core::cvimage *output;
+
+        private:
+            gui::Tab *main_tab,*sensor_tab,*plot_tab;
+            gui::TabWidget *tab;
+            gui::GroupBox* setup_groupbox;
+            gui::GridLayout* setup_layout;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // CAMERA_H
Index: /trunk/lib/FlairSensorActuator/src/Controller.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Controller.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Controller.cpp	(revision 3)
@@ -0,0 +1,139 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/05/27
+//  filename:   Controller.cpp
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Utility classes for controllers
+//
+//
+/*********************************************************************/
+#include "Controller.h"
+
+using namespace flair::core;
+
+namespace flair { namespace sensor {
+
+RumbleMessage::RumbleMessage(unsigned int leftForce,unsigned int leftTimeout,unsigned int rightForce,unsigned int rightTimeout):Message(sizeof(ControllerAction)+sizeof(leftForce)+sizeof(leftTimeout)+sizeof(rightForce)+sizeof(rightTimeout)) {
+    SetLeftForce(leftForce);
+    SetLeftTimeout(leftTimeout);
+    SetRightForce(rightForce);
+    SetRightTimeout(rightTimeout);
+    buffer[0]=(char)ControllerAction::Rumble;
+}
+
+unsigned int RumbleMessage::GetLeftForce() const {
+    unsigned int leftForce;
+    memcpy(&leftForce,buffer+leftForceOffset,sizeof(leftForce));
+    return leftForce;
+}
+
+unsigned int RumbleMessage::GetLeftTimeout() const {
+    unsigned int leftTimeout;
+    memcpy(&leftTimeout,buffer+leftTimeoutOffset,sizeof(leftTimeout));
+    return leftTimeout;
+}
+
+unsigned int RumbleMessage::GetRightForce() const {
+    unsigned int rightForce;
+    memcpy(&rightForce,buffer+rightForceOffset,sizeof(rightForce));
+    return rightForce;
+}
+
+unsigned int RumbleMessage::GetRightTimeout() const {
+    unsigned int rightTimeout;
+    memcpy(&rightTimeout,buffer+rightTimeoutOffset,sizeof(rightTimeout));
+    return rightTimeout;
+}
+
+void RumbleMessage::SetLeftForce(unsigned int leftForce) {
+    memcpy(buffer+leftForceOffset,&leftForce,sizeof(leftForce));
+}
+
+void RumbleMessage::SetLeftTimeout(unsigned int leftTimeout) {
+    memcpy(buffer+leftTimeoutOffset,&leftTimeout,sizeof(leftTimeout));
+}
+
+void RumbleMessage::SetRightForce(unsigned int rightForce) {
+    memcpy(buffer+rightForceOffset,&rightForce,sizeof(rightForce));
+}
+
+void RumbleMessage::SetRightTimeout(unsigned int rightTimeout){
+    memcpy(buffer+rightTimeoutOffset,&rightTimeout,sizeof(rightTimeout));
+}
+
+SwitchLedMessage::SwitchLedMessage(bool isOn, unsigned int ledId):Message(sizeof(ControllerAction)+sizeof(isOn)+sizeof(ledId)) {
+    if (isOn) SetOn(); else SetOff();
+    SetLedId(ledId);
+}
+
+bool SwitchLedMessage::IsOn() const {
+    bool isOn;
+    memcpy(&isOn,buffer+isOnOffset,sizeof(isOn));
+    return isOn;
+}
+
+unsigned int SwitchLedMessage::GetLedId() const {
+    unsigned int ledId;
+    memcpy(&ledId,buffer+ledIdOffset,sizeof(ledId));
+    return ledId;
+}
+
+void SwitchLedMessage::SetOn() {
+    bool isOn=true;
+    memcpy(buffer+isOnOffset,&isOn,sizeof(isOn));
+}
+
+void SwitchLedMessage::SetOff() {
+    bool isOn=false;
+    memcpy(buffer+isOnOffset,&isOn,sizeof(isOn));
+}
+
+void SwitchLedMessage::SetLedId(unsigned int ledId) {
+    memcpy(buffer+ledIdOffset,&ledId,sizeof(ledId));
+}
+
+FlashLedMessage::FlashLedMessage(unsigned int ledId,unsigned int onTime,unsigned int offTime):Message(sizeof(ControllerAction)+sizeof(ledId)+sizeof(onTime)+sizeof(offTime)) {
+    SetLedId(ledId);
+    SetOnTime(onTime);
+    SetOffTime(offTime);
+}
+
+unsigned int FlashLedMessage::GetLedId() const {
+    unsigned int ledId;
+    memcpy(&ledId,buffer+ledIdOffset,sizeof(ledId));
+    return ledId;
+}
+
+unsigned int FlashLedMessage::GetOnTime() const {
+    unsigned int onTime;
+    memcpy(&onTime,buffer+onTimeOffset,sizeof(onTime));
+    return onTime;
+}
+
+unsigned int FlashLedMessage::GetOffTime() const {
+    unsigned int offTime;
+    memcpy(&offTime,buffer+offTimeOffset,sizeof(offTime));
+    return offTime;
+}
+
+void FlashLedMessage::SetLedId(unsigned int ledId) {
+    memcpy(buffer+ledIdOffset,&ledId,sizeof(ledId));
+}
+
+void FlashLedMessage::SetOnTime(unsigned int onTime) {
+    memcpy(buffer+onTimeOffset,&onTime,sizeof(onTime));
+}
+
+void FlashLedMessage::SetOffTime(unsigned int offTime) {
+    memcpy(buffer+offTimeOffset,&offTime,sizeof(offTime));
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Controller.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Controller.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Controller.h	(revision 3)
@@ -0,0 +1,88 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/05/27
+//  filename:   Controller.h
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Utility classes for controllers
+//
+//
+/*********************************************************************/
+
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include <Object.h>
+#include <string.h>
+
+namespace flair {
+    namespace core {
+        class Message;
+    }
+}
+
+namespace flair { namespace sensor {
+
+    enum class ControllerAction {
+        SetLedOn,
+        SetLedOff,
+        Rumble,
+        FlashLed,
+        Exit
+    };
+
+    class RumbleMessage: public core::Message {
+    public:
+        RumbleMessage(unsigned int leftForce,unsigned int leftTimeout,unsigned int rightForce,unsigned int rightTimeout);
+        unsigned int GetLeftForce() const;
+        unsigned int GetLeftTimeout() const;
+        unsigned int GetRightForce() const;
+        unsigned int GetRightTimeout() const;
+        void SetLeftForce(unsigned int leftForce);
+        void SetLeftTimeout(unsigned int leftTimeout);
+        void SetRightForce(unsigned int rightForce);
+        void SetRightTimeout(unsigned int rightTimeout);
+    private:
+        static const unsigned int leftForceOffset=sizeof(ControllerAction);
+        static const unsigned int leftTimeoutOffset=sizeof(ControllerAction)+sizeof(unsigned int);
+        static const unsigned int rightForceOffset=sizeof(ControllerAction)+2*sizeof(unsigned int);
+        static const unsigned int rightTimeoutOffset=sizeof(ControllerAction)+3*sizeof(unsigned int);
+    };
+
+    class SwitchLedMessage: public core::Message {
+    public:
+        SwitchLedMessage(bool isOn,unsigned int ledId);
+        bool IsOn() const;
+        unsigned int GetLedId() const;
+        void SetOn();
+        void SetOff();
+        void SetLedId(unsigned int ledId);
+    private:
+        static const unsigned int isOnOffset=sizeof(ControllerAction);
+        static const unsigned int ledIdOffset=sizeof(ControllerAction)+sizeof(bool);
+    };
+
+    class FlashLedMessage: public core::Message {
+    public:
+        FlashLedMessage(unsigned int ledId,unsigned int onTime,unsigned int offTime);
+        unsigned int GetLedId() const;
+        unsigned int GetOnTime() const;
+        unsigned int GetOffTime() const;
+        void SetLedId(unsigned int ledId);
+        void SetOnTime(unsigned int onTime);
+        void SetOffTime(unsigned int offTime);
+    private:
+        static const unsigned int ledIdOffset=sizeof(ControllerAction);
+        static const unsigned int onTimeOffset=sizeof(ControllerAction)+sizeof(unsigned int);
+        static const unsigned int offTimeOffset=sizeof(ControllerAction)+2*sizeof(unsigned int);
+    };
+
+}}
+
+#endif // CONTROLLER_H
Index: /trunk/lib/FlairSensorActuator/src/Gps.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Gps.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Gps.cpp	(revision 3)
@@ -0,0 +1,342 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/08/23
+//  filename:   Gps.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant le recepteur gps mb800
+//
+//
+/*********************************************************************/
+
+#include "Gps.h"
+#include "geodesie.h"
+#include <Euler.h>
+#include <cvmatrix.h>
+#include <DataPlot1D.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <GridLayout.h>
+#include <GroupBox.h>
+#include <PushButton.h>
+#include <FrameworkManager.h>
+#include <Map.h>
+#include <GeoCoordinate.h>
+#include <Vector3D.h>
+#include <Label.h>
+#include <string.h>
+
+using std::string;
+using namespace Geodesie;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+Gps::Gps(const FrameworkManager* parent,string name,NMEAFlags_t NMEAFlags) : IODevice(parent,name)
+{
+    this->NMEAFlags=NMEAFlags;
+
+    nmea_zero_INFO(&info);
+    nmea_parser_init(&parser);
+    alt_ref=0;
+
+
+    if((NMEAFlags&GGA)==0)
+    {
+        Err("Enable at least GGA sentence\n");
+    }
+
+    int index=0;
+    if((NMEAFlags&GGA)!=0)
+    {
+        index+=3;
+    }
+    if((NMEAFlags&VTG)!=0)
+    {
+        index+=2;
+    }
+    if((NMEAFlags&GST)!=0)
+    {
+        index+=3;
+    }
+
+    cvmatrix_descriptor* desc=new cvmatrix_descriptor(index,1);
+    index=0;
+    if((NMEAFlags&GGA)!=0)
+    {
+        desc->SetElementName(0,0,"e");
+        desc->SetElementName(1,0,"n");
+        desc->SetElementName(2,0,"u");
+        index+=3;
+    }
+    if((NMEAFlags&VTG)!=0)
+    {
+        desc->SetElementName(index,0,"ve");
+        desc->SetElementName(index+1,0,"vn");
+        index+=2;
+    }
+    if((NMEAFlags&GST)!=0)
+    {
+        desc->SetElementName(index,0,"dev_lat");
+        desc->SetElementName(index+1,0,"dev_lon");
+        desc->SetElementName(index+2,0,"dev_elv");
+        index+=3;
+    }
+    output=new cvmatrix((IODevice*)this,desc,floatType);
+    AddDataToLog(output);
+
+    //station sol
+    main_tab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+	sensor_tab=new Tab(tab,"Reglages");
+	GroupBox* reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
+        button_ref=new PushButton(reglages_groupbox->NewRow(),"set ref");
+        nb_sat_label=new Label(reglages_groupbox->NewRow(),"nb_sat");
+        fix_label=new Label(reglages_groupbox->LastRowLastCol(),"fix");
+
+    position=new GeoCoordinate((IODevice*)this,"position",0,0,0);
+
+    fix=FixQuality_t::Invalid;
+    nb_sat=0;
+    take_ref=false;
+    nb_sat_label->SetText("nb_sat: %i",nb_sat);
+    fix_label->SetText("fix: %i",fix);
+}
+
+Gps::~Gps()
+{
+    nmea_parser_destroy(&parser);
+    delete main_tab;
+}
+
+void Gps::UseDefaultPlot(void)
+{
+    int index=0;
+    plot_tab=new Tab(tab,"Mesures");
+
+    if((NMEAFlags&GGA)!=0)
+    {
+        e_plot=new DataPlot1D(plot_tab->NewRow(),"e",-10,10);
+            e_plot->AddCurve(output->Element(index));
+        n_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"n",-10,10);
+            n_plot->AddCurve(output->Element(index+1));
+        u_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"u",-10,10);
+            u_plot->AddCurve(output->Element(index+2));
+        index+=3;
+    }
+    if((NMEAFlags&VTG)!=0)
+    {
+        ve_plot=new DataPlot1D(plot_tab->NewRow(),"ve",-10,10);
+            ve_plot->AddCurve(output->Element(index));
+        vn_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"vn",-10,10);
+            vn_plot->AddCurve(output->Element(index+1));
+        index+=2;
+    }
+
+    Tab* map_tab=new Tab(tab,"carte");
+        map=new Map(map_tab->NewRow(),"map");
+            map->AddPoint(position,"drone");
+}
+
+DataPlot1D* Gps::EPlot(void) const
+{
+    if((NMEAFlags&GGA)!=0)
+    {
+        return e_plot;
+    }
+    else
+    {
+        Err("GGA sentence not requested\n");
+        return NULL;
+    }
+}
+
+DataPlot1D* Gps::NPlot(void) const
+{
+    if((NMEAFlags&GGA)!=0)
+    {
+        return n_plot;
+    }
+    else
+    {
+        Err("GGA sentence not requested\n");
+        return NULL;
+    }
+}
+
+DataPlot1D* Gps::UPlot(void) const
+{
+    if((NMEAFlags&GGA)!=0)
+    {
+        return u_plot;
+    }
+    else
+    {
+        Err("GGA sentence not requested\n");
+        return NULL;
+    }
+}
+
+DataPlot1D* Gps::VEPlot(void) const
+{
+    if((NMEAFlags&VTG)!=0)
+    {
+        return ve_plot;
+    }
+    else
+    {
+        Err("GGA sentence not requested\n");
+        return NULL;
+    }
+}
+
+DataPlot1D* Gps::VNPlot(void) const
+{
+    if((NMEAFlags&VTG)!=0)
+    {
+        return vn_plot;
+    }
+    else
+    {
+        Err("GGA sentence not requested\n");
+        return NULL;
+    }
+}
+
+Layout* Gps::GetLayout(void) const
+{
+    return sensor_tab;
+}
+
+Tab* Gps::GetPlotTab(void) const
+{
+    return plot_tab;
+}
+
+TabWidget* Gps::GetTab(void) const
+{
+    return tab;
+}
+
+uint16_t Gps::NbSat(void) const
+{
+    output->GetMutex();
+    uint16_t result=nb_sat;
+    output->ReleaseMutex();
+    return result;
+}
+
+Gps::FixQuality_t Gps::FixQuality(void) const
+{
+    output->GetMutex();
+    FixQuality_t result=fix;
+    output->ReleaseMutex();
+    return result;
+}
+
+void Gps::SetRef(void)
+{
+    take_ref=true;
+}
+
+void Gps::GetENUPosition(Vector3D *point)
+{
+    output->GetMutex();
+    point->x=output->ValueNoMutex(0,0);
+    point->y=output->ValueNoMutex(1,0);
+    point->z=output->ValueNoMutex(2,0);
+    output->ReleaseMutex();
+}
+
+void Gps::parseFrame(const char *frame, int frame_size){
+
+    int result;
+
+    result=nmea_parse(&parser, frame, frame_size, &info);
+    if(result!=1)
+    {
+        Warn("unrecognized nmea sentence\n");
+        Warn("%s\n",frame);
+    }
+
+    result=nmea_parse_GPGGA(frame, frame_size, &pack);
+
+    if(result==1)
+    {
+        //Printf("%s\n",frame);
+        //Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
+        //Printf("lat:%f long:%f\n",pos.lat,pos.lon);
+        output->GetMutex();//on utilise le mutex de l'output pour fix et nb_sat
+        if((int)fix!=pack.sig)
+        {
+            fix=(FixQuality_t)pack.sig;
+            fix_label->SetText("fix: %i",fix);
+        }
+        if(nb_sat!=pack.satinuse)
+        {
+            nb_sat=pack.satinuse;
+            nb_sat_label->SetText("nb_sat: %i",nb_sat);
+        }
+        output->ReleaseMutex();
+
+
+        nmea_info2pos(&info,&pos);
+        position->SetCoordinates(Euler::ToDegree(pos.lat),Euler::ToDegree(pos.lon),info.elv);
+
+        if((info.sig==2 && alt_ref==0) || button_ref->Clicked()==true || take_ref==true)
+        {
+            Printf("prise pos ref\n");
+            lat_ref=pos.lat;
+            long_ref=pos.lon;
+            alt_ref=info.elv;
+            take_ref=false;
+        }
+        //if(alt_ref!=0)
+        {
+            double x,y,z;
+            double e,n,u;
+            Geographique_2_ECEF(pos.lon,pos.lat,info.elv,x,y,z);
+            ECEF_2_ENU(x,y,z,e, n, u,long_ref,lat_ref,alt_ref);
+            //Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
+
+            //on prend une fois pour toute le mutex et on fait des accès directs
+            output->GetMutex();
+            output->SetValueNoMutex( 0, 0,e);
+            output->SetValueNoMutex( 1, 0,n);
+            output->SetValueNoMutex( 2, 0,u);
+
+            int index=3;
+            if((NMEAFlags&VTG)!=0)
+            {
+                output->SetValueNoMutex( index, 0,info.speed*1000./3600.*sin(Euler::ToRadian(info.direction)));
+                output->SetValueNoMutex( index+1, 0,info.speed*1000./3600.*cos(Euler::ToRadian(info.direction)));
+                index+=2;
+            }
+            if((NMEAFlags&GST)!=0)
+            {
+                //Thread::Printf("dev_lon:%f dev_lat:%f dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
+                output->SetValueNoMutex( index, 0,info.dev_lat);
+                output->SetValueNoMutex( index+1, 0,info.dev_lon);
+                output->SetValueNoMutex( index+2, 0,info.dev_elv);
+                index+=3;
+            }
+            output->ReleaseMutex();
+
+            output->SetDataTime(GetTime());
+            ProcessUpdate(output);
+        }
+    }
+}
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/Gps.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Gps.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Gps.h	(revision 3)
@@ -0,0 +1,240 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Gps.h
+ * \brief Base class for GPS
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/08/23
+ * \version 4.0
+ */
+
+#ifndef GPS_H
+#define GPS_H
+
+#include <IODevice.h>
+#include <nmea/nmea.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class cvmatrix;
+        class FrameworkManager;
+        class GeoCoordinate;
+        class Vector3D;
+    }
+    namespace gui
+    {
+        class Layout;
+        class DataPlot1D;
+        class Tab;
+        class TabWidget;
+        class PushButton;
+        class Map;
+        class Label;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class Gps
+    *
+    * \brief Base class for GPS
+    */
+    class Gps : public core::IODevice
+    {
+        public:
+            /*!
+            \enum FixQuality_t
+            \brief Fix qualty indicators
+            */
+            enum class FixQuality_t {
+                Invalid=0,/*!< invalid */
+                Gps=1,/*!< Gps */
+                DGps=2,/*!< Differential Gps */
+                Pps=3,/*!< Pps */
+                Rtk=4,/*!< RTK */
+                RtkFloat=5,/*!< RTK float */
+                Estimated=6,/*!< Estimated */
+                Manual=7,/*!< Manual */
+                Simulation=8,/*!< Simulation */
+                };
+
+            /*!
+            \enum NMEAFlags_t
+            \brief NMEA flags
+            */
+            enum NMEAFlags_t {
+                GGA=0x01,/*!< GGA */
+                VTG=0x02,/*!< VTG */
+                GST=0x04,/*!< GST */
+                };
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Gps.
+            *
+            * \param parent parent
+            * \param name name
+            * \param NMEAFlags NMEA sentances to enable
+            */
+            Gps(const core::FrameworkManager* parent,std::string name,NMEAFlags_t NMEAFlags);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Gps();
+
+            /*!
+            * \brief Use default plot
+            *
+            */
+            void UseDefaultPlot(void);
+
+            /*!
+            * \brief East plot
+            *
+            * \return east plot
+            */
+            gui::DataPlot1D* EPlot(void) const;
+
+            /*!
+            * \brief North plot
+            *
+            * \return north plot
+            */
+            gui::DataPlot1D* NPlot(void) const;
+
+            /*!
+            * \brief Up plot
+            *
+            * \return up plot
+            */
+            gui::DataPlot1D* UPlot(void) const;
+
+            /*!
+            * \brief East velocity plot
+            *
+            * \return east velocity plot
+            */
+            gui::DataPlot1D* VEPlot(void) const;
+
+            /*!
+            * \brief North velocity plot
+            *
+            * \return north velocity plot
+            */
+            gui::DataPlot1D* VNPlot(void) const;
+
+            /*!
+            * \brief Main tab
+            *
+            * \return main tab
+            */
+            gui::TabWidget* GetTab(void) const;
+
+            /*!
+            * \brief Setup Layout
+            *
+            * \return setup Layout
+            */
+            gui::Layout* GetLayout(void) const;
+
+            /*!
+            * \brief Plot tab
+            *
+            * \return plot Tab
+            */
+            gui::Tab* GetPlotTab(void) const;
+
+            /*!
+            * \brief Number of used satellites
+            *
+            * \return number of used satellites
+            */
+            uint16_t NbSat(void) const;
+
+            /*!
+            * \brief Fix Quality
+            *
+            * \return fix quality
+            */
+            FixQuality_t FixQuality(void) const;
+
+            /*!
+            * \brief Set reference for ENU coordinates
+            *
+            * The actual position is used as reference to calculate
+            * ENU coordinates.
+            *
+            * \return fix quality
+            */
+            void SetRef(void);
+
+            /*!
+            * \brief Get ENU position
+            *
+            * \param point to store position
+            */
+            void GetENUPosition(core::Vector3D *point);
+
+        protected:
+            /*!
+            * \brief Parse a NMEA frame
+            *
+            * This function must be called by the reimplemented class. \n
+            * When a frame is parsed, GPS datas are filled.
+            *
+            * \param frame NMEA frame
+            * \param frame_size frame size
+            *
+            */
+            void parseFrame(const char *frame, int frame_size);
+
+            NMEAFlags_t NMEAFlags;
+
+        protected:
+            core::GeoCoordinate *position;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            gui::Tab *main_tab,*sensor_tab;
+            gui::TabWidget* tab;
+            gui::PushButton *button_ref;
+            gui::DataPlot1D* e_plot;
+            gui::DataPlot1D* n_plot;
+            gui::DataPlot1D* u_plot;
+            gui::DataPlot1D* ve_plot;
+            gui::DataPlot1D* vn_plot;
+            gui::Tab* plot_tab;
+            gui::Map *map;
+            gui::Label *nb_sat_label,*fix_label;
+            uint16_t nb_sat;
+            FixQuality_t fix;
+            bool take_ref;
+            nmeaINFO info;
+            nmeaPARSER parser;
+            nmeaGPGGA pack;
+            nmeaPOS pos;
+            double lat_ref,long_ref,alt_ref;
+
+            //matrix
+            core::cvmatrix *output;
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // GPS_H
Index: /trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp	(revision 3)
@@ -0,0 +1,53 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2011/08/19
+//  filename:   Gx3_25_imu.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant la centrale 3dmgx3-25
+//
+//
+/*********************************************************************/
+
+#include "Gx3_25_imu.h"
+#include <FrameworkManager.h>
+#include "Gx3_25_imu_impl.h"
+#include <GroupBox.h>
+#include <ImuData.h>
+
+using std::string;
+using namespace flair::core;
+
+namespace flair { namespace sensor {
+
+Gx3_25_imu::Gx3_25_imu(const FrameworkManager* parent,string name,SerialPort* serialport,Command_t command,uint8_t priority) : Imu(parent,name),Thread(parent,name,priority) {
+    pimpl_=new Gx3_25_imu_impl(this,name,serialport,command,GetGroupBox());
+
+    if(command==EulerAnglesAndAngularRates) {
+
+    } else if(command==AccelerationAngularRateAndOrientationMatrix) {
+        Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
+    } else {
+        Thread::Err("command not supported (%i)\n",command);
+    }
+    //AddDataToLog(imudata);
+}
+
+Gx3_25_imu::~Gx3_25_imu() {
+    SafeStop();
+    Join();
+    delete pimpl_;
+}
+
+void Gx3_25_imu::Run(void) {
+    pimpl_->Run();
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Gx3_25_imu.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Gx3_25_imu.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Gx3_25_imu.h	(revision 3)
@@ -0,0 +1,88 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Gx3_25_imu.h
+ * \brief Class for 3dmgx3-25 Imu
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2011/08/19
+ * \version 4.0
+ */
+
+#ifndef GX3_25_IMU_H
+#define GX3_25_IMU_H
+
+#include <Imu.h>
+#include <Thread.h>
+
+namespace flair {
+    namespace core {
+        class FrameworkManager;
+        class SerialPort;
+    }
+}
+
+class Gx3_25_imu_impl;
+
+namespace flair { namespace sensor {
+    /*! \class Gx3_25_imu
+    *
+    * \brief Class for 3dmgx3-25 Imu
+    */
+    class Gx3_25_imu : public Imu, public core::Thread {
+        friend class ::Gx3_25_imu_impl;
+
+        public:
+            /*!
+            \enum Command_t
+            \brief Command for the continuous mode
+            */
+            enum Command_t{
+                EulerAnglesAndAngularRates=0xcf,/*!< Euler angles and angular rates */
+                AccelerationAngularRateAndOrientationMatrix=0xc8,/*!< Acceleration, angular rate and orientation matrix */
+                };
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Gx3_25_imu.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport SerialPort
+            * \param command command for continuous mode
+            * \param priority priority of the Thread
+            */
+            Gx3_25_imu(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Command_t command,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Gx3_25_imu();
+
+        private:
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            class Gx3_25_imu_impl* pimpl_;
+    };
+} // end namespace sensor
+} // end namespace flair
+
+#endif // GX3_25_IMU_H
Index: /trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp	(revision 3)
@@ -0,0 +1,550 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2011/08/19
+//  filename:   Gx3_25_imu_impl.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant la centrale 3dmgx3-25
+//
+//
+/*********************************************************************/
+
+#include "Gx3_25_imu_impl.h"
+#include <FrameworkManager.h>
+#include <SerialPort.h>
+#include <GroupBox.h>
+#include <SpinBox.h>
+#include <CheckBox.h>
+#include <PushButton.h>
+#include <Label.h>
+#include <ImuData.h>
+#include <AhrsData.h>
+#include <Euler.h>
+#include <Vector3D.h>
+#include <RotationMatrix.h>
+
+#include <math.h>
+#include <string.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+
+Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu* self,string name,SerialPort *serialport,Gx3_25_imu::Command_t command,GroupBox* setupgroupbox) {
+    int err = 0;
+
+    this->self=self;
+    this->command=(uint8_t)command;
+    this->setupgroupbox=setupgroupbox;
+    this->serialport=serialport;
+
+    ahrsData=new AhrsData((Imu*)self);
+
+    //station sol
+    button_bias=new PushButton(setupgroupbox->NewRow(),"gyros bias");
+    data_rate=new SpinBox(setupgroupbox->NewRow(),"data rate (Hz):",1,500,1,200);
+    data_rate_label=new Label(setupgroupbox->LastRowLastCol(),"data_rate");
+    gyro_acc_size=new SpinBox(setupgroupbox->NewRow(),"gyro and acc filter win size:",1,32,1,15);
+    mag_size=new SpinBox(setupgroupbox->LastRowLastCol(),"mag filter win size:",1,32,1,17);
+    up_comp=new SpinBox(setupgroupbox->NewRow(),"up compensation (s):",1,1000,1,10);
+    north_comp=new SpinBox(setupgroupbox->LastRowLastCol(),"north compensation (s):",1,1000,1,10);
+    coning=new CheckBox(setupgroupbox->NewRow(),"enable Coning&Sculling:",true);
+    disable_magn=new CheckBox(setupgroupbox->LastRowLastCol(),"disable magnetometer:",true);
+    disable_north_comp=new CheckBox(setupgroupbox->NewRow(),"disable magnetic north compensation:",false);
+    disable_grav_comp=new CheckBox(setupgroupbox->NewRow(),"disable gravity compensation:",false);
+}
+
+Gx3_25_imu_impl::~Gx3_25_imu_impl() {
+}
+
+void Gx3_25_imu_impl::Run(void) {
+    Time imuTime;
+    ImuData* imuData;
+    self->GetDatas(&imuData);
+
+    self->WarnUponSwitches(true);
+
+    //reset IMU to be sure it is at 115200
+    Printf("Gx3_25_imu::Reset IMU at 921600\n");
+    serialport->SetBaudrate(921600);
+    DeviceReset();
+    self->Thread::SleepMS(100);
+    serialport->FlushInput();
+    serialport->SetBaudrate(115200);
+
+    SetBaudrate(921600);
+    Printf("Gx3_25_imu firmware number: %i\n",FirmwareNumber());
+    PrintModelInfo();
+    SamplingSettings();
+    GyrosBias();
+    //RealignUpNorth(true,true);
+
+    //on provoque les 9 ValueChanged
+    for(int i=0; i<9; i++) {
+        if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
+                || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
+            if(setupgroupbox->isEnabled()==true) SamplingSettings();
+        }
+    }
+
+    //periode a passer an argument (reglable)
+    //ou plutot laisser la periode geree par le centrale (polling)
+    //self->SetPeriodMS(2);
+    SetContinuousMode(command);
+
+    //_printf("firmware version: %i\n",get_firmware_number());
+
+    while(!self->ToBeStopped()) {
+        if(command==Gx3_25_imu::EulerAnglesAndAngularRates) {
+            uint8_t response[31] = {0};
+            uint8_t *buf=&response[1];
+            GetData(response,sizeof(response),&imuTime);
+
+            Euler eulerAngles;
+            eulerAngles.roll=Dequeue(&buf);
+            eulerAngles.pitch=Dequeue(&buf);
+            eulerAngles.yaw=Dequeue(&buf);
+
+            Vector3D filteredAngRates;
+            filteredAngRates.x=Dequeue(&buf);
+            filteredAngRates.y=Dequeue(&buf);
+            filteredAngRates.z=Dequeue(&buf);
+
+            ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),filteredAngRates);
+        } else if(command==Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) {
+            uint8_t response[67] = {0};
+            uint8_t *buf=&response[1];
+            GetData(response,sizeof(response),&imuTime);
+
+            Vector3D rawAcc;
+            rawAcc.x=9.80665*Dequeue(&buf);
+            rawAcc.y=9.80665*Dequeue(&buf);
+            rawAcc.z=9.80665*Dequeue(&buf);
+
+            Vector3D filteredAngRates;
+            filteredAngRates.x=Dequeue(&buf);
+            filteredAngRates.y=Dequeue(&buf);
+            filteredAngRates.z=Dequeue(&buf);
+
+            RotationMatrix matrix;
+            matrix(0,0)=Dequeue(&buf);
+            matrix(0,1)=Dequeue(&buf);
+            matrix(0,2)=Dequeue(&buf);
+            matrix(1,0)=Dequeue(&buf);
+            matrix(1,1)=Dequeue(&buf);
+            matrix(1,2)=Dequeue(&buf);
+            matrix(2,0)=Dequeue(&buf);
+            matrix(2,1)=Dequeue(&buf);
+            matrix(2,2)=Dequeue(&buf);
+
+            ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),filteredAngRates);
+            imuData->SetRawAcc(rawAcc);
+        }
+
+        //change settings as soon as possible
+        //we assume that new imu datas will not come so fast
+        //so newt message from imu is an ack from the change settings
+        if(button_bias->Clicked()==true) GyrosBias();
+
+        if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
+                || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
+            if(setupgroupbox->isEnabled()==true) SamplingSettings();
+        }
+
+        imuData->SetDataTime(imuTime);
+        ahrsData->SetDataTime(imuTime);
+        self->UpdateImu();
+        self->ProcessUpdate(ahrsData);
+    }
+    SetContinuousMode(0);
+    SetBaudrate(115200);
+
+    self->WarnUponSwitches(false);
+}
+
+void Gx3_25_imu_impl::GetData(uint8_t* buf,ssize_t buf_size,Time *time) {
+    ssize_t read = 0;
+    ssize_t written = 0;
+    /*
+        written = serialport->Write(&command, sizeof(command));
+        if(written<0)
+        {
+            self->Thread::Err("Write error (%s)\n",strerror(-written));
+        }
+        else if (written != sizeof(command))
+        {
+            self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+        }
+    */
+    read = serialport->Read(buf,buf_size);
+    *time=GetTime();
+    if(read<0) {
+        self->Thread::Err("Read error (%s)\n",strerror(-read));
+    } else if (read != buf_size) {
+        self->Thread::Err("Read error %i/%i\n",read,buf_size);
+    }
+
+    if(CalcChecksum(buf,buf_size)==false) {
+        self->Thread::Err("wrong checksum\n");
+        return;
+    }
+}
+
+float Gx3_25_imu_impl::Dequeue(uint8_t** buf) {
+    union float_4uint8 {
+        float f;
+        uint8_t b[4];
+    }
+    float_value;
+
+    float_value.b[0]=(*buf)[3];
+    float_value.b[1]=(*buf)[2];
+    float_value.b[2]=(*buf)[1];
+    float_value.b[3]=(*buf)[0];
+    (*buf)+=sizeof(float);
+
+    return float_value.f;
+}
+
+void Gx3_25_imu_impl::GyrosBias(void) {
+    if(setupgroupbox->isEnabled()==true) { //devrait toujours etre bon
+        uint8_t response[19] = {0};
+        uint8_t command[5];
+        ssize_t read = 0;
+        ssize_t written = 0;
+
+        Printf("Gx3_25_imu::gyros_bias: %s\n",self->IODevice::ObjectName().c_str());
+
+        command[0]=0xcd;//entete
+        command[1]=0xc1;//confirm
+        command[2]=0x29;//confirm
+        command[3]=0x27;//time MSB en us
+        command[4]=0x10;//time LSB en us
+
+        written = serialport->Write( &command, sizeof(command));
+        if(written<0) {
+            self->Thread::Err("Write error (%s)\n",strerror(-written));
+        } else if (written != sizeof(command)) {
+            self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+        }
+
+        self->SleepUS(1.1*(command[3]*256+command[4]));//on fait un sleep un peu plus grand
+
+        read = serialport->Read(&response[0],sizeof(response));
+        if(read<0) {
+            self->Thread::Err("Read error (%s)\n",strerror(-read));
+        } else if (read != sizeof(response)) {
+            self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+        }
+
+        if(CalcChecksum(response,sizeof(response))==false) {
+            self->Thread::Err("wrong checksum\n");
+            //return -1;
+        }
+
+        Printf("Gx3_25_imu::gyros_bias: %s ok\n",self->IODevice::ObjectName().c_str());
+
+    } else {
+        self->Thread::Err("error locked\n");
+    }
+}
+
+void Gx3_25_imu_impl::SetContinuousMode(uint8_t continuous_command) {
+    uint8_t response[8] = {0};
+    uint8_t command[4];
+    ssize_t read = 0;
+    ssize_t written = 0;
+
+    command[0]=0xc4;//entete
+    command[1]=0xc1;//confirm
+    command[2]=0x29;//confirm
+    command[3]=continuous_command;
+
+    written = serialport->Write(command,sizeof(command));
+    if(written<0) {
+        self->Thread::Err("Write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(command)) {
+        self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+    }
+
+    read = serialport->Read(response,sizeof(response));
+    if(read<0) {
+        self->Thread::Err("Read error (%s)\n",strerror(-read));
+    } else if (read != sizeof(response)) {
+        self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+    }
+
+    if(CalcChecksum(response,sizeof(response))==false) {
+        self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
+    }
+}
+
+void Gx3_25_imu_impl::SamplingSettings(void) {
+    uint8_t response[19] = {0};
+    uint8_t command[20];
+    uint8_t result;
+    ssize_t read = 0;
+    ssize_t written = 0;
+
+    uint16_t rate=1000/data_rate->Value();
+
+    command[0]=0xdb;//entete
+    command[1]=0xa8;//confirm
+    command[2]=0xb9;//confirm
+    command[3]=1;//change values
+    command[4]=(rate>>8)&0xff;//data rate MSB
+    command[5]=rate&0xff;//data rate LSB
+    result=0;
+    if(disable_magn->IsChecked()==true) result|=0x01;
+    if(disable_north_comp->IsChecked()==true) result|=0x04;
+    if(disable_grav_comp->IsChecked()==true) result|=0x08;
+
+    command[6]=result;
+    result=0x01;//Calculate orientation
+    if(coning->IsChecked()==true) result|=0x02;
+
+    command[7]=result;
+    command[8]=gyro_acc_size->Value();//gyro acc filter window
+    command[9]=mag_size->Value();//mag filter window
+    command[10]=(up_comp->Value()>>8)&0xff;//up comp MSB
+    command[11]=up_comp->Value()&0xff;//up comp LSB
+    command[12]=(north_comp->Value()>>8)&0xff;//north comp MSB
+    command[13]=north_comp->Value()&0xff;//north comp LSB
+    command[14]=0;//reserved
+    command[15]=0;//reserved
+    command[16]=0;//reserved
+    command[17]=0;//reserved
+    command[18]=0;//reserved
+    command[19]=0;//reserved
+
+    written = serialport->Write(&command, sizeof(command));
+    if(written<0) {
+        self->Thread::Err("Write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(command)) {
+        self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+    }
+
+    read = serialport->Read(&response[0],sizeof(response));
+    if(read<0) {
+        self->Thread::Err("Read error (%s)\n",strerror(-read));
+    } else if (read != sizeof(response)) {
+        self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+    }
+
+    if(CalcChecksum(response,sizeof(response))==false) {
+        self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
+    } else {
+        data_rate_label->SetText("real: %.2fHz",1000./rate);
+    }
+}
+
+void Gx3_25_imu_impl::SetBaudrate(int value) {
+    uint8_t response[10] = {0};
+    uint8_t command[11];
+    ssize_t read = 0;
+    ssize_t written = 0;
+
+    union int32_4uint8 {
+        int32_t i;
+        uint8_t b[4];
+    }
+    baudrate_value;
+
+    baudrate_value.i=value;
+    Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n",self->IODevice::ObjectName().c_str(),baudrate_value.i);
+
+    command[0]=0xd9;//entete
+    command[1]=0xc3;//confirm
+    command[2]=0x55;//confirm
+    command[3]=1;//primary uart
+    command[4]=1;//chgt temporaire
+    command[5]=baudrate_value.b[3];
+    command[6]=baudrate_value.b[2];
+    command[7]=baudrate_value.b[1];
+    command[8]=baudrate_value.b[0];
+    command[9]=2;//uart enabled
+    command[10]=0;//reserved
+
+    written = serialport->Write(&command, sizeof(command));
+    if(written<0) {
+        self->Thread::Err("Write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(command)) {
+        self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+    }
+
+    read = serialport->Read(&response[0],sizeof(response));
+    if(read<0) {
+        self->Thread::Err("Read error (%s)\n",strerror(-read));
+    } else if (read != sizeof(response)) {
+        self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+    }
+
+    if(CalcChecksum(response,sizeof(response))==false) {
+        self->Thread::Err("wrong checksum\n");
+        return ;
+    }
+
+    serialport->SetBaudrate(value);
+}
+
+int Gx3_25_imu_impl::FirmwareNumber(void) {
+    uint8_t response[7] = {0};
+    uint8_t command;
+    ssize_t read = 0;
+    ssize_t written = 0;
+    union int32_4uint8 {
+        int32_t i;
+        uint8_t b[4];
+    }
+    value;
+
+    command=0xe9;//entete
+
+    written = serialport->Write(&command, sizeof(command));
+    if(written<0) {
+        self->Thread::Err("Write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(command)) {
+        self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+    }
+
+    read = serialport->Read(&response[0],sizeof(response));
+    if(read<0) {
+        self->Thread::Err("Read error (%s)\n",strerror(-read));
+    } else if (read != sizeof(response)) {
+        self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+    }
+
+    if(CalcChecksum(response,sizeof(response))==false) {
+        self->Thread::Err("wrong checksum\n");
+        return -1;
+    }
+
+    value.b[3]=response[1];
+    value.b[2]=response[2];
+    value.b[1]=response[3];
+    value.b[0]=response[4];
+
+    return value.i;
+
+}
+
+void Gx3_25_imu_impl::PrintModelInfo(void) {
+    uint8_t response[20] = {0};
+    uint8_t command[2];
+    ssize_t read = 0;
+    ssize_t written = 0;
+
+    for(int i=0; i<3; i++) {
+        command[0]=0xea;//entete
+        command[1]=i;//entete
+
+        written = serialport->Write(&command, sizeof(command));
+        if(written<0) {
+            self->Thread::Err("Write error (%s)\n",strerror(-written));
+        } else if (written != sizeof(command)) {
+            self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+        }
+
+        read = serialport->Read(&response[0],sizeof(response));
+        if(read<0) {
+            self->Thread::Err("Read error (%s)\n",strerror(-read));
+        } else if (read != sizeof(response)) {
+            self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+        }
+
+        if(CalcChecksum(response,sizeof(response))==false) {
+            self->Thread::Err("wrong checksum\n");
+            //return -1;
+        }
+
+        char* msg=(char*)(&response[2]);
+        msg[16]=0;
+        switch(i) {
+        case 0:
+            Printf("Gx3_25_imu model number: %s\n",msg);
+            break;
+        case 1:
+            Printf("Gx3_25_imu serial number: %s\n",msg);
+            break;
+        case 2:
+            Printf("Gx3_25_imu model name: %s\n",msg);
+            break;
+        }
+    }
+
+}
+
+void Gx3_25_imu_impl::RealignUpNorth(bool realign_up,bool realign_north) {
+    uint8_t response[7] = {0};
+    uint8_t command[10];
+    ssize_t read = 0;
+    ssize_t written = 0;
+
+    command[0]=0xdd;//entete
+    command[1]=0x54;//confirm
+    command[2]=0x4c;//confirm
+    command[3]=0;//send reply
+    command[4]=255;//up realign
+    command[5]=1;//north realign
+    command[6]=0;//reserved
+    command[7]=0;//reserved
+    command[8]=0;//reserved
+    command[9]=0;//reserved
+
+    written = serialport->Write(&command, sizeof(command));
+    if(written<0) {
+        self->Thread::Err("Write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(command)) {
+        self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+    }
+
+    read = serialport->Read(&response[0],sizeof(response));
+    if(read<0) {
+        self->Thread::Err("Read error(%s)\n",strerror(-read));
+    } else if (read != sizeof(response)) {
+        self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
+    }
+
+    if(CalcChecksum(response,sizeof(response))==false) {
+        self->Thread::Err("wrong checksum\n");
+    }
+}
+
+void Gx3_25_imu_impl::DeviceReset(void) {
+    uint8_t command[3];
+    ssize_t written = 0;
+
+    command[0]=0xfe;//entete
+    command[1]=0x9e;//confirm
+    command[2]=0x3a;//confirm
+
+    written = serialport->Write(&command, sizeof(command));
+    if(written<0) {
+        self->Thread::Err("Write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(command)) {
+        self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
+    }
+}
+
+bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf,int size) {
+    uint16_t tChksum,tResponseChksum;
+
+    tChksum = 0;
+    for (int i = 0; i < size - 2; i++) tChksum += buf[i];
+    // Extract the big-endian checksum from reply
+    tResponseChksum = 0;
+    tResponseChksum = buf[size - 2] << 8;
+    tResponseChksum += buf[size - 1];
+
+    if(tChksum!=tResponseChksum)
+        return false;
+    else
+        return true;
+}
Index: /trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp	(revision 3)
@@ -0,0 +1,254 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+
+//  created:    2013/08/23
+//  filename:   HokuyoUTM30Lx.cpp
+//
+//  author:     Cesar Richard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    classe intégrant le telemetre laser Hokuyo UTM 30lx
+//
+//
+/*********************************************************************/
+
+#include "HokuyoUTM30Lx.h"
+#include <SerialPort.h>
+#include <FrameworkManager.h>
+#include <RangeFinderPlot.h>
+#include <cvmatrix.h>
+#include <Tab.h>
+#include <sstream>
+#include <iomanip>
+#include <string.h>
+#include <iostream>
+
+using namespace std;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager* parent,string name,SerialPort *serialport,uint8_t priority) : LaserRangeFinder(parent,name), Thread(parent,name,priority)
+{
+    main_tab=new Tab(parent->GetTabWidget(),name);
+	cvmatrix_descriptor* desc=new cvmatrix_descriptor(1081,1);
+    output=new cvmatrix((IODevice*)this,desc,SignedIntegerType(16));
+
+    bufRetMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"BufRetMut");
+    sendingCmdMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"SendingCmdMut");
+
+    this->serialport=serialport;
+    serialport->SetRxTimeout(100000000);
+}
+
+HokuyoUTM30Lx::~HokuyoUTM30Lx()
+{
+    delete main_tab;
+    SafeStop();
+    Join();
+}
+
+void HokuyoUTM30Lx::Run(void)
+{
+	/** Debut init **/
+    char c,lastC;
+    int startStep;
+    bool mustDecode=false;
+    stringstream ss;
+    vector<string> ret;
+    lastC=c=0;
+
+	/** Fin init **/
+	resetConfig();
+	startLaser();
+	Printf("Laser started\r\n");
+
+    /** Debut running loop **/
+    WarnUponSwitches(true);
+
+    while(!ToBeStopped())
+    {
+        ss.clear();
+        ss.str("");
+        do{
+            lastC=c;
+            serialport->Read(&c,1);
+            ss << c;
+        }while(!(c=='\n'&&lastC=='\n'));
+        ret=explode(ss.str(),'\n');
+        if(!checkSum(ret.at(1)))
+            perror("Bad Checksum !");
+        if(ret.at(0).substr(0,2)=="MD")
+            mustDecode=true;
+
+        startStep=atoi(ret.at(0).substr(2,4).c_str());
+        ret.erase(ret.begin());
+        bufRetMut->GetMutex();
+        bufRet.push(ret.at(0).substr(0,2));
+        bufRetMut->ReleaseMutex();
+        if(mustDecode){
+            mustDecode=false;
+            ss.clear();
+            ss.str("");
+            do{
+                lastC=c;
+                serialport->Read(&c,1);
+                ss << c;
+            }while(!(c=='\n'&&lastC=='\n'));
+            ret=explode(ss.str(),'\n');
+            ret.erase(ret.begin());
+            ret.erase(ret.begin());
+            if(ret.at(0).size()==5)
+                if(!checkSum(ret.at(0)))
+                    perror("TimeStamp checksum error!");
+            ret.erase(ret.begin());
+            Printf("!post\r\n");
+            ret.pop_back();
+            decodeDatas(ret, startStep);
+        }
+    }
+    /** fin running loop **/
+    stopLaser();
+    WarnUponSwitches(false);
+}
+
+
+void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep){
+            stringstream ss;
+            for (unsigned i=0; i<datas.size(); i++){
+                if(datas.at(i).size()!=0){
+                    if(!checkSum(datas.at(i)))
+                        perror("Datas checksum error !");
+                    datas.at(i).erase(datas.at(i).end()-1);
+                    ss << datas.at(i);
+                }
+            }
+            output->GetMutex();
+            for(unsigned i=0;i<1080;i++){
+            //TODO: remettre -1 pour les points non lus
+                output->SetValueNoMutex(i,0,0);
+            }
+            for(unsigned i=0;i<ss.str().size();i+=3){
+                output->SetValueNoMutex(startStep+(i/3),0,decode3car(ss.str().substr(i,3).c_str()));
+            }
+            UpdateFrom(output);
+            ProcessUpdate(output);
+            output->ReleaseMutex();
+
+}
+
+bool HokuyoUTM30Lx::checkSum(string data){
+    return (char)encodeSum(data.substr(0,data.size()-1).c_str(),data.size()-1)==data.at(data.size()-1);
+}
+
+void HokuyoUTM30Lx::startLaser(){
+    string ret = sendCommand("BM");
+    #ifdef VERBOSE
+        if(ret == "00"){
+            cout << "BM: Alright !" << endl;
+        }else if(ret == "01"){
+            cout << "BM: Laser malfunction !" << endl;
+        }else if(ret == "02"){
+            cout << "BM: Laser already started !" << endl;
+        }
+    #endif
+}
+
+void HokuyoUTM30Lx::stopLaser(){
+    sendCommand("QT");
+}
+
+void HokuyoUTM30Lx::resetConfig(){
+    sendCommand("RS");
+}
+
+vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter){
+    istringstream split(str);
+    vector<string> tokens;
+    for (string each; getline(split, each, delimiter); tokens.push_back(each));
+    return tokens;
+}
+
+int HokuyoUTM30Lx::encodeSum(const char* code, int byte) {
+    unsigned char value = 0;
+    int i;
+    for (i = 0; i < byte; ++i) {
+        value+=code[i];
+    }
+    value &= 0x3f;
+    value += 0x30;
+    return value;
+}
+
+float HokuyoUTM30Lx::decode2car(const char* data){
+    return ((data[0]-0x30)<<6)|((data[1]-0x30));
+}
+float HokuyoUTM30Lx::decode3car(const char* data){
+    return ((data[0]-0x30)<<12)|((data[1]-0x30)<<6)|((data[2]-0x30));
+}
+float HokuyoUTM30Lx::decode4car(const char* data){
+    return ((data[0]-0x30)<<18)|((data[1]-0x30)<<12)|((data[2]-0x30)<<6)|((data[3]-0x30));
+}
+
+string HokuyoUTM30Lx::sendCommand(string command){
+    char c;
+    string ret="00";
+    c='\n';
+    sendingCmdMut->GetMutex();
+    serialport->Write(command.c_str(),command.size());
+    serialport->Write(&c,1);
+    sendingCmdMut->ReleaseMutex();
+    return ret;
+}
+
+void HokuyoUTM30Lx::getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber){
+    stringstream ss;
+    string ret;
+    ss << "MD" << std::setfill('0') << std::setw(4) << startStep << std::setw(4) << endStep << std::setw(2) << clusterCount << std::setw(1) << interval << std::setw(2) << scanNumber;
+    ret = sendCommand(ss.str());
+    #ifdef VERBOSE
+        if(ret == "00"){
+            cout << "MD: Alright !" << endl;
+        }else if(ret == "01"){
+            cout << "MD: Laser malfunction !" << endl;
+        }else if(ret == "02"){
+            cout << "MD: End Step has non-numeric value !" << endl;
+        }else if(ret == "03"){
+            cout << "MD: Cluster Count has non-numeric value !" << endl;
+        }else if(ret == "04"){
+            cout << "MD: End Step is out of range !" << endl;
+        }else if(ret == "05"){
+            cout << "MD: End Step is smaller than Starting Step !" << endl;
+        }else if(ret == "06"){
+            cout << "MD: Scan Interval has non-numeric value !" << endl;
+        }else if(ret == "07"){
+            cout << "MD: Number of Scan has non-numeric value !" << endl;
+        }else if(ret == "98"){
+            cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx operation." << endl;
+        }else{
+            /* Concerne :
+            21~49 --- Processing stopped to verify the error.
+            50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
+            cout << "MD: Malfunction !" << endl;
+        }
+    #endif
+    }
+cvmatrix* HokuyoUTM30Lx::getDatas(){
+    return output;
+}
+
+void HokuyoUTM30Lx::UseDefaultPlot(void)
+{
+    plot=new RangeFinderPlot(main_tab->NewRow(),"plot","x",-100,100,"y",-0,100,output,-45,225,output->Rows());
+}
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h	(revision 3)
@@ -0,0 +1,175 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file HokuyoUTM30Lx.h
+ * \brief Classe intégrant le telemetre laser Hokuyo UTM 30lx
+ * \author César Richard, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/07/22
+ * \version 1.0
+ */
+
+#ifndef HOKUYOUTM30LX_H
+#define HOKUYOUTM30LX_H
+
+#include <Thread.h>
+#include <LaserRangeFinder.h>
+#include <Mutex.h>
+
+#include <queue>
+#include <vector>
+
+namespace flair
+{
+    namespace core
+    {
+        class cvmatrix;
+        class FrameworkManager;
+        class SerialPort;
+        class Mutex;
+    }
+    namespace gui
+    {
+        class Tab;
+        class TabWidget;
+        class RangeFinderPlot;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class HokuyoUTM30Lx
+    *
+    * \brief Classe intégrant le telemetre laser Hokuyo UTM 30lx
+    */
+    class HokuyoUTM30Lx : public core::Thread, public LaserRangeFinder
+    {
+        public:
+
+             /*!
+            * \brief Constructor
+            *
+            * Construct a Hokuyo UTM30-Lx.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport serialport
+            * \param priority priority of the Thread
+            */
+            HokuyoUTM30Lx(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,uint8_t priority);
+            void getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber=0);
+            core::cvmatrix* getDatas(void);
+
+            /*!
+            * \brief Use default plot
+            *
+            */
+            void UseDefaultPlot(void);
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~HokuyoUTM30Lx();
+
+        private:
+            core::SerialPort *serialport;
+            core::Mutex* bufRetMut;
+            core::Mutex* sendingCmdMut;
+            gui::Tab* main_tab;
+            gui::TabWidget* tab;
+            gui::RangeFinderPlot* plot;
+
+            //matrix
+            core::cvmatrix *output;
+
+			std::queue<std::string> bufRet;
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+            /*!
+            * \brief Send a command
+            * \param command Command to send (see Hokuyo UTM 30-LX doc for more informations)
+            * \return Return code
+            */
+            std::string sendCommand(std::string command);
+            /*!
+            * \brief Start the laser
+            *
+            */
+            void startLaser(void);
+            /*!
+            * \brief Stop the laser
+            *
+            */
+			void stopLaser(void);
+			/*!
+            * \brief Stop and reset the laser's settings
+            *
+            */
+		    void resetConfig(void);
+		    /*!
+            * \brief Decode incomming datas
+            * \param datas Datas to decode
+            * \param startStep Set the first mesured point
+            * Decode mesured points from incoming datas and fill the output matrix
+            */
+		    void decodeDatas(std::vector<std::string> datas, int startStep);
+            /*!
+            * \brief Explode a string into a vector
+            * \param str The string to explode
+            * \param delimiter The character separating elements
+            * \return A vector containing the elements
+            */
+			static std::vector<std::string> explode(const std::string str, char delimiter);
+			/*!
+            * \brief Calculate the checksum
+            * \param code Data from which calculate
+            * \param byte Data's size
+            * \return A character corresponding to the code's checksum
+            */
+			static int encodeSum(const char* code, int byte);
+			/*!
+            * \brief Check if a data correspond to its checksum
+            * \param data Datas to check
+            */
+			static bool checkSum(std::string data);
+			/*!
+            * \brief Decode datas using the 2 character encoding
+            * \param data Datas to decode
+            * \return Decoded datas
+            */
+			static float decode2car(const char* data);
+			/*!
+            * \brief Decode datas using the 3 character encoding
+            * \param data Datas to decode
+            * \return Decoded datas
+            */
+			static float decode3car(const char* data);
+			/*!
+            * \brief Decode datas using the 4 character encoding
+            * \param data Datas to decode
+            * \return Decoded datas
+            */
+			static float decode4car(const char* data);
+
+			/*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // HOKUYOUTM30LX_H
Index: /trunk/lib/FlairSensorActuator/src/HostEthController.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/HostEthController.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/HostEthController.cpp	(revision 3)
@@ -0,0 +1,263 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/03/30
+//  filename:   HostEthController.cpp
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
+//
+//
+/*********************************************************************/
+#include "HostEthController.h"
+#include <Controller.h>
+#include <cvmatrix.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <Label.h>
+#include <DataPlot1D.h>
+#include <FrameworkManager.h>
+#include <TargetController.h>
+#include <TcpSocket.h>
+#include <Socket.h>
+#include <cstring>
+#include <string>
+#include <cmath> //for sqrt
+#include <sched.h>
+
+#include <iostream>
+
+using namespace flair::core;
+using namespace flair::gui;
+using std::string;
+
+namespace flair
+{
+namespace sensor
+{
+
+HostEthController::HostEthController(const FrameworkManager* parent,string name,string _address,int _port,uint32_t period,uint32_t _bitsPerAxis,uint8_t priority) :
+    Thread(parent,name,priority),IODevice(parent,name),tab(new Tab(parent->GetTabWidget(),name)),axisNumber(0),buttonNumber(0),targetAddress(_address),targetPort(_port),
+    bitsPerAxis(_bitsPerAxis),dataFrameBuffer(NULL),meaningfulDataAvailable(false) {
+    tabWidget=new TabWidget(tab->NewRow(),name);
+
+    const bool blocking=true;
+    controlSocket=new TcpSocket((Thread*)this,"eth controller control socket", blocking, !blocking);
+    dataSocket=new Socket((Thread*)this,"eth controller data socket",_address+":"+std::to_string(_port+1));
+    dataSender=new DataSender((Thread*)this, this, "eth controller data sender thread");
+    dataSender->SetPeriodMS(period);
+    dataSender->Start();
+
+    //test binary
+    /*
+    uint16_t testValue=0b0111011010; //0x1DA
+    char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; // 0xAB65B6
+    writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
+    Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
+    */
+    connectionEstablishedMutex=new Mutex((Thread*)this);
+}
+
+HostEthController::~HostEthController() {
+    SafeStop();
+    Join();
+
+    if(!getFrameworkManager()->ConnectionLost()) delete tab;
+}
+
+void HostEthController::DrawUserInterface() {
+    Tab* plotTab=new Tab(tabWidget,"Measures");
+    axisPlot=new DataPlot1D *[axisNumber];
+    for (unsigned int i=0; i<axisNumber;i++) {
+        //Start a new row or add up to the current row? We try to keep a 4/3 ratio
+        unsigned int columns=sqrt(4.0*axisNumber/3.0);
+        LayoutPosition *position;
+        if (i%columns==0) {
+            position=plotTab->NewRow();
+        } else {
+            position=plotTab->LastRowLastCol();
+        }
+        axisPlot[i]=new DataPlot1D(position,axis->Name(i,0),-(1<<(nativeBitsPerAxis-1))*1.2,(1<<(nativeBitsPerAxis-1))*1.5);
+        axisPlot[i]->AddCurve(axis->Element(i));
+    }
+    //we don't plot the button state for now
+}
+
+string HostEthController::GetAxisDescription(unsigned int axis) {
+    return string("axis")+std::to_string(axis);
+}
+
+string HostEthController::GetButtonDescription(unsigned int button) {
+    return string("button")+std::to_string(button);
+}
+
+bool HostEthController::ControllerInitialization() {
+
+    buttonOffset=(axisNumber*bitsPerAxis)/8;
+    if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
+    dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
+    if ((buttonNumber%8)!=0) dataFrameSize++;
+    dataFrameBuffer=new char[dataFrameSize];
+    return true;
+}
+
+void HostEthController::SendControllerInfo() {
+    //send axis info
+    controlSocket->WriteUInt32((uint32_t)axisNumber,0);
+    controlSocket->WriteUInt32(bitsPerAxis,0);
+    for (unsigned int i=0; i<axisNumber; i++) {
+        //Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
+        int stringLength=GetAxisDescription(i).length();
+        controlSocket->WriteUInt32((uint32_t)stringLength,0);
+        controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength,0);
+    }
+
+    //send button info
+    controlSocket->WriteUInt32((uint32_t)buttonNumber,0);
+    for (unsigned int i=0; i<buttonNumber; i++) {
+        int stringLength=GetButtonDescription(i).length();
+        controlSocket->WriteUInt32((uint32_t)stringLength,0);
+        controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,0);
+    }
+}
+
+bool HostEthController::ConnectedWithTarget() {
+    char message[1024];
+    ssize_t sent,received;
+    static bool connectionEstablished=false;
+
+    connectionEstablishedMutex->GetMutex();
+    if (!connectionEstablished && controlSocket->Connect(targetPort,targetAddress,10)) {
+        Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
+        SendControllerInfo();
+        connectionEstablished=true;
+    }
+    connectionEstablishedMutex->ReleaseMutex();
+    return connectionEstablished;
+}
+
+void HostEthController::Run() {
+    static int divider=0;
+    Message *msgControllerAction=new Message(1024);
+    if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
+        SafeStop();
+        Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
+    }
+
+    if(buttonNumber%8!=0) {
+        SafeStop();
+        Thread::Err("Button number is not multiple of 8\n");
+    }
+
+    while(!ToBeStopped()) {
+        //Thread::Info("Debug: entering acquisition loop\n");
+        if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
+
+        if (IsDataFrameReady()) {  //wait for next data frame
+            meaningfulDataAvailable=true;
+            GetAxisData();
+            GetButtonData();
+
+            if (ConnectedWithTarget()) {
+                //read for commands from the target (remote control state change requests such as rumble or led on/off)
+                ssize_t bytesReceived=controlSocket->RecvMessage((char*)msgControllerAction->buffer,msgControllerAction->bufferSize);
+                if(bytesReceived>0) {
+                    //if the message is a cnx exit request we manage it here, if not it will be managed by the derived class
+                    ControllerAction action;
+                    memcpy(&action,msgControllerAction->buffer,sizeof(ControllerAction));
+                    if (action==ControllerAction::Exit) {
+                        //Thread::Info("Debug: exit request received from server\n");
+                        SafeStop();
+                    }
+                    ProcessMessage(msgControllerAction);
+                }
+            }
+        } else {//try to connect even if host is not sending anything
+            ConnectedWithTarget();
+        }
+        //Thread::Info("Debug: exiting acquisition loop\n");
+    }
+}
+
+bool HostEthController::writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits) {
+    if (valueSizeInBits>16) return false;
+    uint8_t remainingBitsToWrite=valueSizeInBits;
+    //skip first bytes
+    buffer+=offsetInBits/8;
+    offsetInBits-=(offsetInBits/8)*8;
+    while (remainingBitsToWrite>0) {
+        uint8_t remainingBitsInByteBeforeWrite=8-offsetInBits;
+        uint8_t bitsToWrite=remainingBitsToWrite<remainingBitsInByteBeforeWrite?remainingBitsToWrite:remainingBitsInByteBeforeWrite;
+        uint8_t remainingBitsInByteAfterWrite=remainingBitsInByteBeforeWrite-bitsToWrite;
+        //write in the current byte
+        uint8_t byteMask=((1<<bitsToWrite)-1)<<remainingBitsInByteAfterWrite;
+        (*buffer)&=~byteMask;
+        uint16_t valueMask=(1<<remainingBitsToWrite)-1;
+        (*buffer)|=((value&valueMask)>>(remainingBitsToWrite-bitsToWrite))<<remainingBitsInByteAfterWrite;
+        //update state
+        remainingBitsToWrite-=bitsToWrite;
+        offsetInBits=(offsetInBits+bitsToWrite)%8;
+        buffer++;
+    }
+    return true;
+}
+
+void HostEthController::BuildDataFrame() {
+    //int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
+    for (unsigned int i=0;i<axisNumber;i++) {
+        //We shift value to always be positive (so that division/multiplication by power of 2 can easily be done with bit shifts)
+        uint16_t shiftedNativeAxisValue=axis->Value(i,0)+(1<<(nativeBitsPerAxis-1));
+        //int16_t nativeAxisValue=testValue[i];
+        uint16_t scaledAxisValue;
+        if (bitsPerAxis>nativeBitsPerAxis) {
+            scaledAxisValue=shiftedNativeAxisValue<<(bitsPerAxis-nativeBitsPerAxis);
+        } else {
+            scaledAxisValue=shiftedNativeAxisValue>>(nativeBitsPerAxis-bitsPerAxis);
+        }
+        //Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
+        unsigned int offsetInBits=i*bitsPerAxis;
+        writeBits(scaledAxisValue,bitsPerAxis,dataFrameBuffer,offsetInBits);
+    }
+    //Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0], dataFrameBuffer[1], dataFrameBuffer[2]);
+
+    int currentButton=0;
+    uint8_t buttonArray[buttonNumber/8];
+    for (unsigned int i=0;i<buttonNumber/8;i++) {
+        buttonArray[i]=0;
+        for(unsigned int j=0;j<8;j++) {
+            bool buttonValue=button->Value(currentButton,0);
+            if(buttonValue) buttonArray[i]+=1<<j;
+            currentButton++;
+        }
+
+        dataSocket->HostToNetwork((char *)&buttonArray[i],sizeof(uint8_t));
+        memcpy(dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),&buttonArray[i],sizeof(uint8_t));
+    }
+}
+
+HostEthController::DataSender::DataSender(Object *parent,HostEthController* _hostEthController,string name,uint8_t priority):Thread(parent,name,priority),hostEthController(_hostEthController) {
+
+}
+
+void HostEthController::DataSender::Run() {
+    if(getFrameworkManager()->ErrorOccured()==true) {
+        SafeStop();
+    }
+
+    while(!ToBeStopped()) {
+        WaitPeriod();
+        if (hostEthController->meaningfulDataAvailable && hostEthController->ConnectedWithTarget()) {
+            //send the data
+            hostEthController->BuildDataFrame();
+            hostEthController->dataSocket->SendMessage(hostEthController->dataFrameBuffer,hostEthController->dataFrameSize);
+        }
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/HostEthController.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/HostEthController.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/HostEthController.h	(revision 3)
@@ -0,0 +1,108 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/03/30
+//  filename:   HostEthController.h
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
+//
+//
+/*********************************************************************/
+
+#ifndef HOSTETHCONTROLLER_H
+#define HOSTETHCONTROLLER_H
+
+#include <IODevice.h>
+#include <Thread.h>
+#include <stdint.h>
+
+namespace flair {
+    namespace core {
+        class FrameworkManager;
+        class cvmatrix;
+        class TcpSocket;
+        class Socket;
+        class Mutex;
+    }
+    namespace gui {
+        class Tab;
+        class TabWidget;
+        class DataPlot1D;
+    }
+}
+
+namespace flair { namespace sensor {
+    enum class ControllerAction;
+
+    /*! \class HostEthController
+    *
+    * \brief Base Class for host side remote controls that talks to target side through ethernet connection
+    *
+    * There are 2 communication channels:
+    *   - 1 connection with the ground station to display the values. Output for analog sticks is normalized in the range [-1, 1] (float values)
+    *   - 1 connection with the target to send the controller values (and receive controller state modification requests)
+    */
+    class HostEthController : public core::Thread, public core::IODevice {
+    public:
+        HostEthController(const core::FrameworkManager* parent,std::string name,std::string address,int port,uint32_t period=10,uint32_t _bitsPerAxis=7,uint8_t priority=0);
+        ~HostEthController();
+        void DrawUserInterface();
+    protected:
+        std::string controllerName;
+        core::TcpSocket *controlSocket; //connection to the target
+        core::Socket *dataSocket;
+        std::string targetAddress;
+        int targetPort;
+        gui::Tab *tab;
+        gui::TabWidget *tabWidget;
+        virtual bool IsDataFrameReady() { return true;};
+        virtual void CompleteDataFrameGrab() {};
+//        int8_t *datas;
+//        uint8_t dataSize;
+        char *dataFrameBuffer;
+        size_t dataFrameSize;
+        virtual void ProcessMessage(core::Message *controllerAction) {};
+
+        virtual std::string GetAxisDescription(unsigned int axis);
+        virtual void GetAxisData()=0; //responsible for getting the axis data from the hardware
+        unsigned int axisNumber;
+        core::cvmatrix* axis;
+        gui::DataPlot1D **axisPlot;
+        uint32_t bitsPerAxis;
+        uint32_t nativeBitsPerAxis;
+
+        virtual std::string GetButtonDescription(unsigned int button);
+        virtual void GetButtonData()=0; //responsible for getting the button data from the hardware
+        unsigned int buttonNumber;
+        core::cvmatrix* button;
+        uint8_t buttonOffset;
+        bool meaningfulDataAvailable;
+
+    private:
+        class DataSender : public core::Thread {
+        public:
+            DataSender(Object* parent,HostEthController* hostEthController,std::string name,uint8_t priority=0);
+            void Run();
+        private:
+            HostEthController* hostEthController;
+        };
+        DataSender *dataSender;
+
+        bool ControllerInitialization();
+        bool ConnectedWithTarget();
+        void SendControllerInfo();
+        void Run();
+        void BuildDataFrame();
+        bool writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits);
+        core::Mutex *connectionEstablishedMutex;
+    };
+
+}}
+
+#endif // HOSTETHCONTROLLER_H
Index: /trunk/lib/FlairSensorActuator/src/Imu.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Imu.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Imu.cpp	(revision 3)
@@ -0,0 +1,134 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/01/16
+//  filename:   Imu.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Virtual class for Imu
+//
+//
+/*********************************************************************/
+
+#include "Imu.h"
+#include <FrameworkManager.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <GroupBox.h>
+#include <GridLayout.h>
+#include <DataPlot1D.h>
+#include <ImuData.h>
+#include <OneAxisRotation.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace sensor {
+
+Imu::Imu(const FrameworkManager* parent,string name) : IODevice(parent,name) {
+    imuData=new ImuData(this);
+
+    //station sol
+    mainTab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(mainTab->NewRow(),name);
+    sensorTab=new Tab(tab,"Reglages");
+	setupGroupbox=new GroupBox(sensorTab->NewRow(),name);
+	rotation=new OneAxisRotation(sensorTab->NewRow(),"post rotation");
+}
+
+Imu::Imu(const IODevice* parent,std::string name) : IODevice(parent,name) {
+    imuData=new ImuData(this);
+    mainTab=NULL;
+    tab=NULL;
+    sensorTab=NULL;
+	setupGroupbox=NULL;
+	rotation=NULL;
+}
+
+Imu::~Imu() {
+    if(mainTab!=NULL) delete mainTab;
+}
+
+void Imu::GetDatas(ImuData **outImuData) const {
+    *outImuData=imuData;
+}
+void Imu::UpdateImu() {
+    if(rotation==NULL) {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+    Vector3D rawAcc,rawMag,rawGyr;
+    imuData->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
+    rotation->ComputeRotation(rawAcc);
+    rotation->ComputeRotation(rawGyr);
+    rotation->ComputeRotation(rawMag);
+    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
+}
+
+GroupBox* Imu::GetGroupBox(void) const {
+    return setupGroupbox;
+}
+
+Layout* Imu::GetLayout(void) const {
+    return sensorTab;
+}
+
+void Imu::LockUserInterface(void) const {
+    if(sensorTab==NULL) {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+    sensorTab->setEnabled(false);
+}
+
+void Imu::UnlockUserInterface(void) const {
+    if(sensorTab==NULL) {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+    sensorTab->setEnabled(true);
+}
+
+void Imu::UseDefaultPlot(void) {
+    if(tab==NULL) {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    plotTab=new Tab(tab,"IMU");
+    axPlot=new DataPlot1D(plotTab->NewRow(),"acc_x",-10,10);
+    axPlot->AddCurve(imuData->Element(ImuData::RawAx));
+    ayPlot=new DataPlot1D(plotTab->LastRowLastCol(),"acc_y",-10,10);
+    ayPlot->AddCurve(imuData->Element(ImuData::RawAy));
+    azPlot=new DataPlot1D(plotTab->LastRowLastCol(),"acc_z",-10,10);
+    azPlot->AddCurve(imuData->Element(ImuData::RawAz));
+
+    if(plotTab==NULL) plotTab=new Tab(tab,"IMU");
+    gxPlot=new DataPlot1D(plotTab->NewRow(),"gyr_x",-500,500);
+    gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg));
+    gyPlot=new DataPlot1D(plotTab->LastRowLastCol(),"gyr_y",-500,500);
+    gyPlot->AddCurve(imuData->Element(ImuData::RawGyDeg));
+    gzPlot=new DataPlot1D(plotTab->LastRowLastCol(),"gyr_z",-500,500);
+    gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg));
+
+    if(plotTab==NULL)  plotTab=new Tab(tab,"IMU");
+    mxPlot=new DataPlot1D(plotTab->NewRow(),"mag_x",-500,500);
+    mxPlot->AddCurve(imuData->Element(ImuData::RawMx));
+    myPlot=new DataPlot1D(plotTab->LastRowLastCol(),"mag_y",-500,500);
+    myPlot->AddCurve(imuData->Element(ImuData::RawMy));
+    mzPlot=new DataPlot1D(plotTab->LastRowLastCol(),"mag_z",-500,500);
+    mzPlot->AddCurve(imuData->Element(ImuData::RawMz));
+}
+
+Tab* Imu::GetPlotTab(void) const {
+    return plotTab;
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Imu.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Imu.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Imu.h	(revision 3)
@@ -0,0 +1,144 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Imu.h
+ * \brief Base class for Imu
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/01/16
+ * \version 4.0
+ */
+
+#ifndef IMU_H
+#define IMU_H
+
+#include <IODevice.h>
+
+namespace flair {
+    namespace core {
+        class ImuData;
+        class OneAxisRotation;
+    }
+    namespace gui {
+        class Tab;
+        class TabWidget;
+        class GroupBox;
+        class Layout;
+        class DataPlot1D;
+    }
+}
+
+class Ahrs_impl;
+
+namespace flair { namespace sensor {
+    /*! \class Imu
+    *
+    * \brief Base class for Imu
+    *
+    * Use this class to define a custom Imu.
+    *
+    */
+    class Imu : public core::IODevice {
+        friend class ::Ahrs_impl;
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct an Imu.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            Imu(const core::FrameworkManager *parent,std::string name);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct an Imu. \n
+            * This contructor must only be called for a simulated device.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            Imu(const core::IODevice *parent,std::string name);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Imu();
+
+            /*!
+            * \brief Setup Layout
+            *
+            * \return setup Layout
+            */
+            gui::Layout *GetLayout(void) const;
+
+            /*!
+            * \brief Lock user interface
+            *
+            */
+            void LockUserInterface(void) const;
+
+            /*!
+            * \brief Unlock user interface
+            *
+            */
+            void UnlockUserInterface(void) const;
+
+            /*!
+            * \brief Use default plot
+            *
+            */
+            void UseDefaultPlot(void);
+
+            /*!
+            * \brief Plot tab
+            *
+            * \return plot Tab
+            */
+            gui::Tab *GetPlotTab(void) const;
+
+        protected:
+            /*!
+            * \brief Setup GroupBox
+            *
+            * \return setup GroupBox
+            */
+            gui::GroupBox *GetGroupBox(void) const;
+
+            /*!
+            * \brief UpdateImu
+            *
+            * The reimplemented class must call this function as soon as IMU datas are available. \n
+            * It handles the data rotation if it was defined.
+            *
+            */
+            void UpdateImu();
+
+            /*!
+            * \brief Get imu datas
+            *
+            * \param imuData imu datas
+            */
+            void GetDatas(core::ImuData **imuData) const;
+
+
+        private:
+            gui::Tab *mainTab,*sensorTab,*plotTab;
+            gui::TabWidget* tab;
+            gui::GroupBox *setupGroupbox;
+            core::OneAxisRotation* rotation;
+            core::ImuData *imuData;
+
+            gui::DataPlot1D *axPlot,*ayPlot,*azPlot;
+            gui::DataPlot1D *gxPlot,*gyPlot,*gzPlot;
+            gui::DataPlot1D *mxPlot,*myPlot,*mzPlot;
+    };
+} // end namespace sensor
+} // end namespace flair
+
+#endif // IMU_H
Index: /trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp	(revision 3)
@@ -0,0 +1,94 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/07/22
+//  filename:   LaserRangeFinder.cpp
+//
+//  author:     César RICHARD
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Classe generique des recepteurs LaserRangeFinder
+//
+//
+/*********************************************************************/
+
+#include "LaserRangeFinder.h"
+#include <cvmatrix.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <GridLayout.h>
+#include <GroupBox.h>
+#include <PushButton.h>
+#include <FrameworkManager.h>
+#include <Label.h>
+#include <RangeFinderPlot.h>
+#include <string.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace sensor {
+
+LaserRangeFinder::LaserRangeFinder(const FrameworkManager* parent,string name) : IODevice(parent,name) {
+    cvmatrix_descriptor* desc=new cvmatrix_descriptor(360,1);
+    output=new cvmatrix(this,desc,floatType);
+    AddDataToLog(output);
+
+    //station sol
+    main_tab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+    sensor_tab=new Tab(tab,"Reglages");
+    setup_groupbox=new GroupBox(sensor_tab->NewRow(),name);
+
+}
+
+LaserRangeFinder::LaserRangeFinder(const IODevice* parent,std::string name) : IODevice(parent,name)
+{
+    plot_tab=NULL;
+    main_tab=NULL;
+    tab=NULL;
+    sensor_tab=NULL;
+    setup_groupbox=NULL;
+
+    output=NULL;
+}
+
+LaserRangeFinder::~LaserRangeFinder()
+{
+
+}
+
+
+GroupBox* LaserRangeFinder::GetGroupBox(void) const
+{
+return setup_groupbox;
+}
+
+Layout* LaserRangeFinder::GetLayout(void) const
+{
+return sensor_tab;
+}
+
+
+RangeFinderPlot* LaserRangeFinder::GetPlot(void) const
+{
+return plot;
+}
+
+void LaserRangeFinder::UseDefaultPlot(void)
+{
+    if(tab==NULL)
+    {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    plot_tab=new Tab(tab,"Mesures");
+    plot=new RangeFinderPlot(plot_tab->NewRow(),"plot","x",-30,30,"y",-30,30,output,0,359,output->Rows());
+}
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h	(revision 3)
@@ -0,0 +1,140 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file LaserRangeFinder.h
+ * \brief Classe generique des telemetres laser
+ * \author César RICHARD, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/07/22
+ * \version 1.0
+ */
+
+#ifndef LASERRANGEFINDER_H
+#define LASERRANGEFINDER_H
+
+#include <IODevice.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class cvmatrix;
+
+    }
+    namespace gui
+    {
+        class Tab;
+        class TabWidget;
+        class GroupBox;
+        class Layout;
+        class RangeFinderPlot;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class LaserRangeFinder
+    *
+    * \brief Classe generique intégrant les telemetres laser
+    */
+    class LaserRangeFinder : public core::IODevice
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Laser Range Finder.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            LaserRangeFinder(const core::FrameworkManager* parent,std::string name);
+            /*!
+            * \brief Constructor
+            *
+            * Construct a UsRangeFinder. Simulation part.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            LaserRangeFinder(const core::IODevice* parent,std::string name);
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~LaserRangeFinder();
+
+            /*!
+            * \brief Use default plot
+            *
+            */
+            void UseDefaultPlot(void);
+
+            /*!
+            * \brief Plot
+            *
+            * \return DataPlot1D
+            */
+            gui::RangeFinderPlot* GetPlot(void) const;
+
+            /*!
+            * \brief Setup Layout
+            *
+            * \return a Layout available
+            */
+            gui::Layout* GetLayout(void) const;
+
+            /*!
+            * \brief Plot tab
+            *
+            * \return plot Tab
+            */
+            gui::Tab* GetPlotTab(void) const;
+
+            /*!
+            * \brief Value
+            *
+            * \return output value
+            */
+            float Value(void) const;
+
+        protected:
+        /*!
+            * \brief Output matrix
+            *
+            * \return output matrix
+            */
+            core::cvmatrix *output;
+
+            /*!
+            * \brief Setup GroupBox
+            *
+            * \return a GroupBox available
+            */
+            gui::GroupBox* GetGroupBox(void) const;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            gui::Tab* main_tab;
+            gui::TabWidget *tab;
+            gui::GroupBox* setup_groupbox;
+            gui::Tab* sensor_tab;
+            gui::RangeFinderPlot* plot;
+            gui::Tab* plot_tab;
+
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // LASERRANGEFINDER_H
Index: /trunk/lib/FlairSensorActuator/src/Mb800.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Mb800.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Mb800.cpp	(revision 3)
@@ -0,0 +1,129 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/08/23
+//  filename:   Mb800.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant le recepteur gps mb800
+//
+//
+/*********************************************************************/
+
+#include "Mb800.h"
+#include <SerialPort.h>
+#include <FrameworkManager.h>
+#include <string.h>
+
+using std::string;
+using namespace flair::core;
+
+namespace flair
+{
+namespace sensor
+{
+
+Mb800::Mb800(const FrameworkManager* parent,string name,SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Gps(parent,name,NMEAFlags), Thread(parent,name,priority)
+{
+    this->serialport=serialport;
+    serialport->SetRxTimeout(100000000);
+}
+
+Mb800::~Mb800()
+{
+    SafeStop();
+    Join();
+}
+
+void Mb800::Run(void)
+{
+    /** Debut init **/
+    char response[200] = {0};
+    int size;
+
+    /** Fin init **/
+
+    /** Debut config **/
+    char to_send[]="$PASHS,NME,ALL,A,OFF\r\n";
+    serialport->Write(to_send, sizeof(to_send));
+
+    {
+        char to_send[]="$PASHS,CPD,AFP,95.0\r\n";
+        serialport->Write(to_send, sizeof(to_send));
+    }
+    {
+        char to_send[]="$PASHS,DIF,PRT,C,RT3\r\n";
+        serialport->Write(to_send, sizeof(to_send));
+    }
+
+    if((NMEAFlags&GGA)!=0)
+    {
+        char to_send[]="$PASHS,NME,GGA,A,ON,0.05\r\n";
+        size=serialport->Write(to_send, sizeof(to_send));
+        //Printf("ecrit %i\n",size);
+    }
+    if((NMEAFlags&VTG)!=0)
+    {
+        char to_send[]="$PASHS,NME,VTG,A,ON,0.05\r\n";
+        size=serialport->Write(to_send, sizeof(to_send));
+        //Printf("%i\n",size);
+    }
+    if((NMEAFlags&GST)!=0)
+    {
+        char to_send[]="$PASHS,NME,GST,A,ON,0.05\r\n";
+        size=serialport->Write(to_send, sizeof(to_send));
+        //Printf("%i\n",size);
+    }
+
+    Sync();
+
+    /** Fin config **/
+
+    /** Debut running loop **/
+    WarnUponSwitches(true);
+
+    while(!ToBeStopped())
+    {
+        SleepMS(10);
+        size=0;
+        while(!ToBeStopped())
+        {
+            ssize_t read = serialport->Read(&response[size],1);
+            if(read<0)
+            {
+                Thread::Err("erreur Read (%s)\n",strerror(-read));
+            }
+
+            if(response[size]==0x0a) break;
+            size++;
+        }
+        size++;
+        parseFrame(response, size);
+    }
+    /** fin running loop **/
+    WarnUponSwitches(false);
+}
+
+void Mb800::Sync(void)
+{
+    char data=0;
+    ssize_t read = 0;
+
+    //attente fin trame
+    while(data!=0x0a)
+    {
+        read = serialport->Read(&data,1);
+        if(read<0)
+        {
+            Thread::Err("erreur Read (%s)\n",strerror(-read));
+        }
+    }
+}
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/Mb800.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Mb800.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Mb800.h	(revision 3)
@@ -0,0 +1,81 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Mb800.h
+ * \brief Class for mb800 gps receiver
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/08/23
+ * \version 4.0
+ */
+
+#ifndef MB800_H
+#define MB800_H
+
+#include <Thread.h>
+#include <Gps.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class SerialPort;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class Mb800
+    *
+    * \brief Class for mb800 gps receiver
+    */
+    class Mb800 : public core::Thread, public Gps
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Mb800.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport serialport
+            * \param NMEAFlags NMEA sentances to enable
+            * \param priority priority of the Thread
+            */
+            Mb800(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Mb800();
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            void Sync(void);
+            core::SerialPort *serialport;
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // MB800_H
Index: /trunk/lib/FlairSensorActuator/src/Novatel.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Novatel.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Novatel.cpp	(revision 3)
@@ -0,0 +1,103 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/08/23
+//  filename:   Novatel.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant le recepteur gps Novatel
+//
+//
+/*********************************************************************/
+
+#include "Novatel.h"
+#include <SerialPort.h>
+#include <FrameworkManager.h>
+#include <string.h>
+
+using std::string;
+using namespace flair::core;
+
+namespace flair
+{
+namespace sensor
+{
+
+Novatel::Novatel(const FrameworkManager* parent,string name,SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Gps(parent,name,NMEAFlags), Thread(parent,name,priority)
+{
+    this->serialport=serialport;
+}
+
+Novatel::~Novatel()
+{
+    SafeStop();
+    Join();
+}
+
+void Novatel::Run(void)
+{
+    char response[200] = {0};
+    int size;
+    ssize_t written;
+
+    WarnUponSwitches(true);
+
+    char to_send[]="log gpggalong ontime 0.05\n";
+    written=serialport->Write(to_send, sizeof(to_send));
+    if(written<0)
+    {
+        Thread::Err("erreur write (%s)\n",strerror(-written));
+    }
+    char to_send2[]="log gpvtg ontime 0.05\n";
+    written=serialport->Write(to_send2, sizeof(to_send2));
+    if(written<0)
+    {
+        Thread::Err("erreur write (%s)\n",strerror(-written));
+    }
+
+    Sync();
+
+    while(!ToBeStopped())
+    {
+        size=0;
+        while(1)
+        {
+            //ssize_t read = rt_dev_read(uart_fd, &response[size],1);
+            ssize_t Read = serialport->Read(&response[size],1);
+            if(Read<0)
+            {
+                Thread::Err("erreur Read (%s)\n",strerror(-Read));
+            }
+            if(response[size]==0x0a) break;
+            size++;
+        }
+        size++;
+        parseFrame(response, size);
+    }
+
+    WarnUponSwitches(false);
+}
+
+void Novatel::Sync(void)
+{
+    char data=0;
+    ssize_t Read = 0;
+
+    //attente fin trame
+    while(data!=0x0a)
+    {
+        Read = serialport->Read(&data,1);
+        if(Read<0)
+        {
+            Thread::Err("erreur Read (%s)\n",strerror(-Read));
+        }
+    }
+}
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/Novatel.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Novatel.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Novatel.h	(revision 3)
@@ -0,0 +1,81 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Novatel.h
+ * \brief Class for Novatel gps receiver
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/08/23
+ * \version 4.0
+ */
+
+#ifndef Novatel_H
+#define Novatel_H
+
+#include <Thread.h>
+#include <Gps.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class SerialPort;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class Novatel
+    *
+    * \brief Class for Novatel gps receiver
+    */
+    class Novatel : public core::Thread, public Gps
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Novatel.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport serialport
+            * \param NMEAFlags NMEA sentances to enable
+            * \param priority priority of the Thread
+            */
+            Novatel(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Novatel();
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            core::SerialPort *serialport;
+            void Sync(void);
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // Novatel_H
Index: /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.cpp	(revision 3)
@@ -0,0 +1,134 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/01/24
+//  filename:   ParrotBatteryMonitor.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Parrot battery monitor
+//
+//
+/*********************************************************************/
+
+#include "ParrotBatteryMonitor.h"
+#include <Layout.h>
+#include <LayoutPosition.h>
+#include <Unix_I2cPort.h>
+
+#define INTERFACE_GROUP_ADDRESS 0x49
+#define INTBR_GPBR1 0x91
+
+//INTBR_GPBR1 bits
+#define DEFAULT_MADC_CLK_EN (1<<4)	/* MADC clock enable */
+#define MADC_HFCLK_EN       (1<<7)	/* HFCLK clock divider enable */
+
+#define MADC_GROUP_ADDRESS 0x4a
+#define MADC_CTRL1 0x00
+#define MADC_SW1SELECT_LSB 0x06
+#define MADC_SW1AVERAGE_LSB 0x08
+#define MADC_CTRL_SW1 0x12
+#define MADC_GPCH0_LSB 0x37
+#define MADC_GPCH0_MSB 0x38
+
+//MADC_CTRL1 bits
+#define MADC_MADCON     (1<<0)	/* MADC power on */
+
+//MADC_SW1SELECT_LSB bits
+#define	MADC_ADCIN0		(1<<0)
+
+//MADC_SW1AVERAGE_LSB bits
+#define	MADC_AV_CH0		(1<<0)
+
+//MADC_CTRL_SW1 bits
+#define	MADC_EOC_SW1	(1<<1) /* end of convrsion SW1 */
+#define	MADC_SW1		(1<<5) /* start an all channel convrsion */
+
+using std::string;
+using namespace flair::core;
+
+namespace flair {
+namespace sensor {
+
+ParrotBatteryMonitor::ParrotBatteryMonitor(const gui::LayoutPosition* position,string name) : Thread(position->getLayout(),name,5),BatteryMonitor(position,name) {
+    uint8_t tx[2];
+
+    port=new Unix_I2cPort((BatteryMonitor*)this,"i2c1","/dev/i2c-1");
+
+    //interface group address
+    port->SetSlave(INTERFACE_GROUP_ADDRESS);
+
+    //enable clocks
+    tx[0]=INTBR_GPBR1;
+    tx[1]=MADC_HFCLK_EN|DEFAULT_MADC_CLK_EN;
+    port->Write(tx,2);
+
+    //MADC group address
+    port->SetSlave(MADC_GROUP_ADDRESS);
+
+    // turn on MADC
+    tx[0]=MADC_CTRL1;
+    tx[1]=MADC_MADCON;
+    port->Write(tx,2);
+
+    // select channel 0
+    tx[0]=MADC_SW1SELECT_LSB;
+    tx[1]=MADC_ADCIN0;
+    port->Write(tx,2);
+
+    // setup averaging on channel 0
+    tx[0]=MADC_SW1AVERAGE_LSB;
+    tx[1]=MADC_AV_CH0;
+    port->Write(tx,2);
+}
+
+ParrotBatteryMonitor::~ParrotBatteryMonitor() {
+    SafeStop();
+    Join();
+}
+
+void ParrotBatteryMonitor::Run(void) {
+    unsigned char lsb, msb;
+    int adc_value;
+    uint8_t tx[2];
+    uint8_t rx;
+
+    SetPeriodMS(1000);
+
+    while(!ToBeStopped()) {
+        // start an all channel conversion
+        tx[0]=MADC_CTRL_SW1;
+        tx[1]=MADC_SW1;
+        port->Write(tx,2);
+
+        WaitPeriod();
+
+        //check end of conversion in CTRL_SW1
+        //(conversion should be ended as period is 1s)
+        port->Write(tx,1);
+        port->Read(&rx,1);
+        if((rx&MADC_EOC_SW1)==MADC_EOC_SW1) {
+            //read value
+            tx[0]=MADC_GPCH0_LSB;
+            port->Write(tx,1);
+            port->Read(&lsb,1);
+
+            tx[0]=MADC_GPCH0_MSB;
+            port->Write(tx,1);
+            port->Read(&msb,1);
+
+            adc_value = (lsb >> 6) | (msb << 2);
+
+            //convert to volts
+            //thanks to Paparazzi (https://wiki.paparazziuav.org) for the coefficient
+            SetBatteryValue(adc_value*0.013595166);
+        }
+    }
+}
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.h	(revision 3)
@@ -0,0 +1,68 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file ParrotBatteryMonitor.h
+ * \brief Parrot battery monitor
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/01/24
+ * \version 4.0
+ */
+
+#ifndef PARROTBATTERYMONITOR_H
+#define PARROTBATTERYMONITOR_H
+
+#include <BatteryMonitor.h>
+#include <Thread.h>
+
+namespace flair
+{
+namespace gui
+{
+    class Layout;
+}
+namespace core
+{
+    class Unix_I2cPort;
+}
+
+namespace sensor
+{
+    /*! \class ParrotBatteryMonitor
+    *
+    * \brief Parrot battery monitor
+    */
+    class ParrotBatteryMonitor : public core::Thread,public BatteryMonitor
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a ParrotBatteryMonitor at given position.
+            *
+            * \param position position
+            * \param name name
+            */
+            ParrotBatteryMonitor(const gui::LayoutPosition* position,std::string name);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~ParrotBatteryMonitor();
+
+        private:
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            core::Unix_I2cPort* port;
+    };
+}; // end namespace sensor
+}; // end namespace flair
+#endif // PARROTBATTERYMONITOR_H
Index: /trunk/lib/FlairSensorActuator/src/ParrotBldc.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBldc.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotBldc.cpp	(revision 3)
@@ -0,0 +1,46 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/12/19
+//  filename:   ParrotBldc.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs ardrone
+//
+//
+/*********************************************************************/
+
+#include "ParrotBldc.h"
+#include "ParrotBldc_impl.h"
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace actuator
+{
+
+ParrotBldc::ParrotBldc(const IODevice* parent,Layout* layout,string name,SerialPort* serialport) : Bldc(parent,layout,name,4)
+{
+     pimpl_=new ParrotBldc_impl(this,serialport);
+}
+
+ParrotBldc::~ParrotBldc()
+{
+    delete pimpl_;
+}
+
+void ParrotBldc::SetMotors(float* value)
+{
+    pimpl_->SetMotors(value);
+}
+
+} // end namespace actuator
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/ParrotBldc.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBldc.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotBldc.h	(revision 3)
@@ -0,0 +1,96 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file ParrotBldc.h
+ * \brief Class for Ardrone 2 Bldc
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/12/19
+ * \version 4.0
+ */
+
+#ifndef PARROTBLDC_H
+#define PARROTBLDC_H
+
+#include "Bldc.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class cvmatrix;
+        class SerialPort;
+    }
+    namespace gui
+    {
+        class Layout;
+    }
+}
+
+class ParrotBldc_impl;
+
+namespace flair
+{
+namespace actuator
+{
+    /*! \class ParrotBldc
+    *
+    * \brief Class for Ardrone 2 Bldc
+    */
+    class ParrotBldc : public Bldc
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a ParrotBldc.
+            *
+            * \param parent parent
+            * \param layout layout
+            * \param name name
+            * \param serialport serialport
+            */
+            ParrotBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,core::SerialPort* serialport);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~ParrotBldc();
+
+            /*!
+            * \brief Has speed measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has speed measurement
+            */
+            bool HasSpeedMeasurement(void) const{return false;};
+
+            /*!
+            * \brief Has current measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has current measurement
+            */
+            bool HasCurrentMeasurement(void) const{return false;};
+
+        private:
+            /*!
+            * \brief Set motors values
+            *
+            * Reimplemented from Bldc. \n
+            * Values size must be the same as MotorsCount()
+            *
+            * \param values motor values
+            */
+            void SetMotors(float* value);
+
+            class ParrotBldc_impl* pimpl_;
+    };
+} // end namespace actuator
+} // end namespace framewor
+#endif // PARROTBLDC_H
Index: /trunk/lib/FlairSensorActuator/src/ParrotBldc_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBldc_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotBldc_impl.cpp	(revision 3)
@@ -0,0 +1,231 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/12/19
+//  filename:   ParrotBldc_impl.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Classe integrant les moteurs ardrone
+//
+//  thanks to Paparazzi (https://wiki.paparazziuav.org) for the protocol
+//
+//
+/*********************************************************************/
+#include "ParrotBldc_impl.h"
+#include "ParrotBldc.h"
+#include <SerialPort.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+
+#define MAX_VALUE           0x1ff
+
+#define MOT_LEDOFF          0
+#define MOT_LEDRED          1
+#define MOT_LEDGREEN        2
+#define MOT_LEDORANGE       3
+
+#define GPIO_MOTOR1         171
+#define GPIO_MOTOR2         172
+#define GPIO_MOTOR3         173
+#define GPIO_MOTOR4         174
+
+#define GPIO_IRQ_FLIPFLOP   175
+#define GPIO_IRQ_INPUT      176
+#define GPIO_MB_GREEN_LED   180
+#define GPIO_MB_RED_LED     181
+
+#define GPIO_MAGIC          'p'
+#define GPIO_DIRECTION      _IOW(GPIO_MAGIC, 0, GpioDirection_t)
+#define GPIO_READ           _IOWR(GPIO_MAGIC, 1, GpioData_t)
+#define GPIO_WRITE          _IOW(GPIO_MAGIC, 2, GpioData_t)
+
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::actuator;
+
+ParrotBldc_impl::ParrotBldc_impl(ParrotBldc* self,SerialPort* serialport) {
+    this->self=self;
+    this->serialport=serialport;
+
+    fd=open("/dev/gpio",O_RDWR);
+    if (fd <= 0) self->Err("/dev/gpio open error\n");
+
+    SetGPIODirection(GPIO_MB_GREEN_LED,GpioMode_t::GpioOutputLow);
+    SetGPIODirection(GPIO_MB_RED_LED,GpioMode_t::GpioOutputLow);
+    SetGPIOValue(GPIO_MB_RED_LED,false);
+    SetGPIOValue(GPIO_MB_GREEN_LED,true);
+
+    SetGPIODirection(GPIO_MOTOR1,GpioMode_t::GpioOutputLow);
+    SetGPIODirection(GPIO_MOTOR2,GpioMode_t::GpioOutputLow);
+    SetGPIODirection(GPIO_MOTOR3,GpioMode_t::GpioOutputLow);
+    SetGPIODirection(GPIO_MOTOR4,GpioMode_t::GpioOutputLow);
+    SetGPIODirection(GPIO_IRQ_FLIPFLOP,GpioMode_t::GpioOutputLow);
+    SetGPIODirection(GPIO_IRQ_INPUT,GpioMode_t::GpioInput);
+
+    ResetBldc();
+}
+
+ParrotBldc_impl::~ParrotBldc_impl() {
+    SetGPIOValue(GPIO_MB_GREEN_LED,false);
+    SetGPIOValue(GPIO_MB_RED_LED,true);
+    SetLeds(MOT_LEDRED,MOT_LEDRED, MOT_LEDRED, MOT_LEDRED);
+
+    close(fd);
+}
+
+void ParrotBldc_impl::ResetBldc() {
+    SetGPIOValue(GPIO_IRQ_FLIPFLOP,false);
+    usleep(20000);
+    SetGPIOValue(GPIO_IRQ_FLIPFLOP,true);
+
+    //all select lines inactive
+    SetGPIOValue(GPIO_MOTOR1,false);
+    SetGPIOValue(GPIO_MOTOR2,false);
+    SetGPIOValue(GPIO_MOTOR3,false);
+    SetGPIOValue(GPIO_MOTOR4,false);
+
+    //configure motors
+    uint8_t reply[121];
+    for(int i=0; i<4; i++) {
+        SetGPIOValue(GPIO_MOTOR1 + i,true);
+
+        WriteCmd(0xe0,reply,2);
+        if(reply[1]==0x50) {
+            self->Warn("Init motor %i\n",i);
+            WriteCmd(0x91,reply,121);
+            WriteCmd(0xa1,reply,2);
+            WriteCmd(i+1,reply,1);
+            usleep(200000);//wait before reconfiguring motor, otherwise it does not reply
+            WriteCmd(0xe0,reply,2);
+        }
+        if(reply[1]!=0x00) {
+            self->Err("Init motor error %i\n",i);
+        }
+
+        WriteCmd(i+1,reply,1);
+        SetGPIOValue(GPIO_MOTOR1+i,false);
+    }
+
+    //all select lines active
+    SetGPIOValue(GPIO_MOTOR1,true);
+    SetGPIOValue(GPIO_MOTOR2,true);
+    SetGPIOValue(GPIO_MOTOR3,true);
+    SetGPIOValue(GPIO_MOTOR4,true);
+
+    //start multicast
+    for(int i=0; i<6; i++) WriteCmd(0xa0,reply,1);
+
+    //reset IRQ flipflop
+    //on error GPIO_IRQ_INPUT reads 1, this code resets GPIO_IRQ_INPUT to 0
+    SetGPIOValue(GPIO_IRQ_FLIPFLOP,false);
+    SetGPIOValue(GPIO_IRQ_FLIPFLOP,true);
+
+    usleep(200000);//wait, otherwise leds stay red when motor is stopped
+    SetLeds(MOT_LEDGREEN,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN);
+
+    is_reseted=true;
+}
+
+void ParrotBldc_impl::SetGPIODirection(int gpio,GpioMode_t mode) {
+    GpioDirection_t dir;
+
+    dir.gpio = gpio;
+    dir.mode = mode;
+    if(ioctl(fd, GPIO_DIRECTION, &dir)<0) self->Err("ioctl error\n");
+}
+
+void ParrotBldc_impl::SetGPIOValue(int gpio,bool value) {
+    GpioData_t data;
+
+    data.gpio = gpio;
+    if(value) {
+        data.value = 1;
+    } else {
+        data.value = 0;
+    }
+    if(ioctl(fd, GPIO_WRITE, &data)<0) self->Err("ioctl error\n");
+}
+
+void ParrotBldc_impl::SetMotors(float* value) {
+    uint8_t tx[5];
+    uint8_t rx[5];
+    uint16_t mot_value[4];
+
+    for(int i=0; i<4; i++) mot_value[i]=(uint16_t)(MAX_VALUE*value[i]);
+
+    if(mot_value[0]!=0 || mot_value[1]!=0 || mot_value[2]!=0 || mot_value[3]!=0) {
+
+        tx[0] = 0x20 | ((mot_value[0]&MAX_VALUE)>>4);
+        tx[1] = ((mot_value[0]&MAX_VALUE)<<4) | ((mot_value[1]&MAX_VALUE)>>5);
+        tx[2] = ((mot_value[1]&MAX_VALUE)<<3) | ((mot_value[3]&MAX_VALUE)>>6);
+        tx[3] = ((mot_value[3]&MAX_VALUE)<<2) | ((mot_value[2]&MAX_VALUE)>>7);
+        tx[4] = ((mot_value[2]&MAX_VALUE)<<1);
+        ssize_t written=serialport->Write(tx, sizeof(tx));
+
+        if(written<0) {
+            self->Err("write error (%s)\n",strerror(-written));
+        } else if (written != sizeof(tx)) {
+            self->Err("write error %i/%i\n",written,sizeof(tx));
+        }
+
+        ssize_t read=serialport->Read(rx, sizeof(rx));
+        if(read<0) {
+            self->Err("read error (%s)\n",strerror(-read));
+        } else if (read != sizeof(rx)) {
+            self->Err("read error %i/%i\n",read,sizeof(rx));
+        }
+        for(int i=0; i<5; i++) {
+            if(rx[i]!=tx[i]) {
+                self->Warn("wrong response\n");
+                for(int i=0; i<5; i++) Printf("%x %x",tx[i],rx[i]);
+                Printf("\n");
+            }
+        }
+
+        is_reseted=false;
+
+    } else {
+        if(is_reseted==false) ResetBldc();
+    }
+}
+
+int ParrotBldc_impl::WriteCmd(uint8_t cmd, uint8_t *reply, int replylen) {
+    ssize_t written=serialport->Write(&cmd, 1);
+    if(written<0) {
+        self->Err("write error (%s)\n",strerror(-written));
+    } else if (written != 1) {
+        self->Err("write error %i/1\n",written);
+    }
+
+    return serialport->Read(reply, replylen);
+}
+
+void ParrotBldc_impl::SetLeds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) {
+    uint8_t tx[2];
+
+    led0 &= 0x03;
+    led1 &= 0x03;
+    led2 &= 0x03;
+    led3 &= 0x03;
+
+    //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3);
+
+    tx[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1);
+    tx[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0);
+
+    ssize_t written=serialport->Write(tx, sizeof(tx));
+    if(written<0) {
+        self->Err("write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(tx)) {
+        self->Err("write error %i/%i\n",written,sizeof(tx));
+    }
+
+    serialport->Read(tx, sizeof(tx));//flush
+}
Index: /trunk/lib/FlairSensorActuator/src/ParrotCamH.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamH.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotCamH.cpp	(revision 3)
@@ -0,0 +1,1053 @@
+//  created:    2014/07/16
+//  filename:   ParrotCamH.cpp
+//
+//  author:     Guillaume Sanahuja
+//
+//  version:    $Id: $
+//
+//  purpose:    class for ardrone2 horizontal camera
+//
+//
+/*********************************************************************/
+
+/*
+* A V4L2 driver for Aptina MT9M114 cameras.
+*
+* Copyright 2011 Aldebaran Robotics Written
+* by Joseph Pinkasfeld with substantial inspiration from ov7670 code.
+*
+* Authors:
+* joseph pinkasfeld <joseph.pinkasfeld@gmail.com>
+* Ludovic SMAL <lsmal@aldebaran-robotics.com>
+* Corentin Le Molgat <clemolgat@aldebaran-robotics.com>
+* Arne Böckmann <arneboe@tzi.de>
+*
+* This file may be distributed under the terms of the GNU General
+* Public License, version 2.
+*/
+
+#include "ParrotCamH.h"
+#include <Unix_I2cPort.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#define I2C_8BIT 1
+#define I2C_16BIT 2
+#define I2C_32BIT 4
+
+/* Registers */
+#define REG_CHIP_ID 0x0000
+#define REG_MON_MAJOR_VERSION 0x8000
+#define REG_MON_MINOR_VERION 0x8002
+#define REG_MON_RELEASE_VERSION 0x8004
+#define REG_RESET_AND_MISC_CONTROL 0x001A
+#define REG_PAD_SLEW_CONTROL 0x001E
+#define REG_COMMAND_REGISTER 0x0080
+#define HOST_COMMAND_APPLY_PATCH 0x0001
+#define HOST_COMMAND_SET_STATE 0x0002
+#define HOST_COMMAND_REFRESH 0x0004
+#define HOST_COMMAND_WAIT_FOR_EVENT 0x0008
+#define HOST_COMMAND_OK 0x8000
+#define REG_ACCESS_CTL_STAT 0x0982
+#define REG_PHYSICAL_ADDRESS_ACCESS 0x098A
+#define REG_LOGICAL_ADDRESS_ACCESS 0x098E
+#define MCU_VARIABLE_DATA0 0x0990
+#define MCU_VARIABLE_DATA1 0x0992
+#define REG_RESET_REGISTER 0x301A
+#define REG_DAC_TXLO_ROW 0x316A
+#define REG_DAC_TXLO 0x316C
+#define REG_DAC_LD_4_5 0x3ED0
+#define REG_DAC_LD_6_7 0x3ED2
+#define REG_DAC_ECL 0x316E
+#define REG_DELTA_DK_CONTROL 0x3180
+#define REG_SAMP_COL_PUP2 0x3E14
+#define REG_COLUMN_CORRECTION 0x30D4
+#define REG_LL_ALGO 0xBC04
+#define LL_EXEC_DELTA_DK_CORRECTION 0x0200
+#define REG_CAM_DGAIN_RED 0xC840
+#define REG_CAM_DGAIN_GREEN_1 0xC842
+#define REG_CAM_DGAIN_GREEN_2 0xC844
+#define REG_CAM_DGAIN_BLUE 0xC846
+#define REG_CAM_SYSCTL_PLL_ENABLE 0xC97E
+#define REG_CAM_SYSCTL_PLL_DIVIDER_M_N 0xC980
+#define REG_CAM_SYSCTL_PLL_DIVIDER_P 0xC982
+#define REG_CAM_SENSOR_CFG_Y_ADDR_START 0xC800
+#define REG_CAM_SENSOR_CFG_X_ADDR_START 0xC802
+#define REG_CAM_SENSOR_CFG_Y_ADDR_END 0xC804
+#define REG_CAM_SENSOR_CFG_X_ADDR_END 0xC806
+#define REG_CAM_SENSOR_CFG_PIXCLK 0xC808
+#define REG_CAM_SENSOR_CFG_ROW_SPEED 0xC80C
+#define REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN 0xC80E
+#define REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX 0xC810
+#define REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES 0xC812
+#define REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK 0xC814
+#define REG_CAM_SENSOR_CFG_FINE_CORRECTION 0xC816
+#define REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW 0xC818
+#define REG_CAM_SENSOR_CFG_REG_0_DATA 0xC826
+#define REG_CAM_SENSOR_CONTROL_READ_MODE 0xC834
+#define CAM_SENSOR_CONTROL_VERT_FLIP_EN 0x0002
+#define CAM_SENSOR_CONTROL_HORZ_FLIP_EN 0x0001
+#define CAM_SENSOR_CONTROL_BINNING_EN 0x0330
+#define CAM_SENSOR_CONTROL_SKIPPING_EN 0x0110
+#define CAM_MON_HEARTBEAT 0x8006 //the frame counter. updates on vertical blanking.
+#define REG_CAM_CROP_WINDOW_XOFFSET 0xC854
+#define REG_CAM_CROP_WINDOW_YOFFSET 0xC856
+#define REG_CAM_CROP_WINDOW_WIDTH 0xC858
+#define REG_CAM_CROP_WINDOW_HEIGHT 0xC85A
+#define REG_CAM_CROP_CROPMODE 0xC85C
+#define REG_CAM_OUTPUT_WIDTH 0xC868
+#define REG_CAM_OUTPUT_HEIGHT 0xC86A
+#define REG_CAM_OUTPUT_FORMAT 0xC86C
+#define REG_CAM_OUTPUT_OFFSET 0xC870
+#define REG_CAM_PORT_OUTPUT_CONTROL 0xC984
+#define REG_CAM_OUPUT_FORMAT_YUV 0xC86E
+#define REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART 0xC914
+#define REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART 0xC916
+#define REG_CAM_STAT_AWB_CLIP_WINDOW_XEND 0xC918
+#define REG_CAM_STAT_AWB_CLIP_WINDOW_YEND 0xC91A
+#define REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART 0xC91C
+#define REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART 0xC91E
+#define REG_CAM_STAT_AE_INITIAL_WINDOW_XEND 0xC920
+#define REG_CAM_STAT_AE_INITIAL_WINDOW_YEND 0xC922
+#define REG_CAM_PGA_PGA_CONTROL 0xC95E
+#define REG_SYSMGR_NEXT_STATE 0xDC00
+#define REG_SYSMGR_CURRENT_STATE 0xDC01
+#define REG_PATCHLDR_LOADER_ADDRESS 0xE000
+#define REG_PATCHLDR_PATCH_ID 0xE002
+#define REG_PATCHLDR_FIRMWARE_ID 0xE004
+#define REG_PATCHLDR_APPLY_STATUS 0xE008
+#define REG_AUTO_BINNING_MODE 0xE801
+#define REG_CAM_SENSOR_CFG_MAX_ANALOG_GAIN 0xC81C
+#define REG_CROP_CROPMODE 0xC85C
+#define REG_CAM_AET_AEMODE 0xC878
+#define REG_CAM_AET_TARGET_AVG_LUMA 0xC87A
+#define REG_CAM_AET_TARGET_AVERAGE_LUMA_DARK 0xC87B
+#define REG_CAM_AET_BLACK_CLIPPING_TARGET 0xC87C
+#define REG_CAM_AET_AE_MAX_VIRT_AGAIN 0xC886
+#define REG_CAM_AET_MAX_FRAME_RATE 0xC88C
+#define REG_CAM_AET_MIN_FRAME_RATE 0xC88E
+#define REG_CAM_AET_TARGET_GAIN 0xC890
+#define REG_AE_ALGORITHM 0xA404
+#define REG_AE_TRACK_MODE 0xA802
+#define REG_AE_TRACK_AE_TRACKING_DAMPENING_SPEED 0xA80A
+#define REG_CAM_LL_START_BRIGHTNESS 0xC926
+#define REG_CAM_LL_STOP_BRIGHTNESS 0xC928
+#define REG_CAM_LL_START_GAIN_METRIC 0xC946
+#define REG_CAM_LL_STOP_GAIN_METRIC 0xC948
+#define REG_CAM_LL_START_TARGET_LUMA_BM 0xC952
+#define REG_CAM_LL_STOP_TARGET_LUMA_BM 0xC954
+#define REG_CAM_LL_START_SATURATION 0xC92A
+#define REG_CAM_LL_END_SATURATION 0xC92B
+#define REG_CAM_LL_START_DESATURATION 0xC92C
+#define REG_CAM_LL_END_DESATURATION 0xC92D
+#define REG_CAM_LL_START_DEMOSAIC 0xC92E
+#define REG_CAM_LL_START_AP_GAIN 0xC92F
+#define REG_CAM_LL_START_AP_THRESH 0xC930
+#define REG_CAM_LL_STOP_DEMOSAIC 0xC931
+#define REG_CAM_LL_STOP_AP_GAIN 0xC932
+#define REG_CAM_LL_STOP_AP_THRESH 0xC933
+#define REG_CAM_LL_START_NR_RED 0xC934
+#define REG_CAM_LL_START_NR_GREEN 0xC935
+#define REG_CAM_LL_START_NR_BLUE 0xC936
+#define REG_CAM_LL_START_NR_THRESH 0xC937
+#define REG_CAM_LL_STOP_NR_RED 0xC938
+#define REG_CAM_LL_STOP_NR_GREEN 0xC939
+#define REG_CAM_LL_STOP_NR_BLUE 0xC93A
+#define REG_CAM_LL_STOP_NR_THRESH 0xC93B
+#define REG_CAM_LL_START_CONTRAST_BM 0xC93C
+#define REG_CAM_LL_STOP_CONTRAST_BM 0xC93E
+#define REG_CAM_LL_GAMMA 0xC940
+#define REG_CAM_LL_START_CONTRAST_GRADIENT 0xC942
+#define REG_CAM_LL_STOP_CONTRAST_GRADIENT 0xC943
+#define REG_CAM_LL_START_CONTRAST_LUMA_PERCENTAGE 0xC944
+#define REG_CAM_LL_STOP_CONTRAST_LUMA_PERCENTAGE 0xC945
+#define REG_CAM_LL_START_FADE_TO_BLACK_LUMA 0xC94A
+#define REG_CAM_LL_STOP_FADE_TO_BLACK_LUMA 0xC94C
+#define REG_CAM_LL_CLUSTER_DC_TH_BM 0xC94E
+#define REG_CAM_LL_CLUSTER_DC_GATE_PERCENTAGE 0xC950
+#define REG_CAM_LL_SUMMING_SENSITIVITY_FACTOR 0xC951
+#define REG_CAM_LL_MODE 0xBC02 //might be BC07.
+#define REG_CCM_DELTA_GAIN 0xB42A
+#define REG_CAM_HUE_ANGLE 0xC873
+// AWB
+#define REG_AWB_AWB_MODE 0xC909
+#define REG_AWB_COL_TEMP 0xC8F0//color temp, only writeable if awb mode is manual. in kelvin
+#define REG_AWB_COL_TEMP_MAX 0xC8EE//maximum color temp in kelvin
+#define REG_AWB_COL_TEMP_MIN 0xC8EC//minimum color temp in kelvin
+// UVC
+#define REG_UVC_AE_MODE 0xCC00
+#define REG_UVC_AUTO_WHITE_BALANCE_TEMPERATURE 0xCC01
+#define REG_UVC_AE_PRIORITY 0xCC02
+#define REG_UVC_POWER_LINE_FREQUENCY 0xCC03
+#define REG_UVC_EXPOSURE_TIME 0xCC04
+#define REG_UVC_BACKLIGHT_COMPENSATION 0xCC08
+#define REG_UVC_BRIGHTNESS 0xCC0A //set brightness in auto exposure mode.
+#define REG_UVC_CONTRAST 0xCC0C //not exactly what the name suggests. See chip documentation
+#define REG_UVC_GAIN 0xCC0E
+#define REG_UVC_HUE 0xCC10
+#define REG_UVC_SATURATION 0xCC12
+#define REG_UVC_SHARPNESS 0xCC14
+#define REG_UVC_GAMMA 0xCC16
+#define REG_UVC_WHITE_BALANCE_TEMPERATURE 0xCC18
+#define REG_UVC_FRAME_INTERVAL 0xCC1C
+#define REG_UVC_MANUAL_EXPOSURE 0xCC20
+#define REG_UVC_FLICKER_AVOIDANCE 0xCC21
+#define REG_UVC_ALGO 0xCC22
+#define REG_UVC_RESULT_STATUS 0xCC24
+/**This variable selects the system event that the host wishes to wait for.
+* 1: end of frame
+* 2: start of frame */
+#define REG_CMD_HANDLER_WAIT_FOR_EVENT 0xFC00
+/** This variable determines the number of system event occurrences for which the
+* Command Handler component will wait */
+#define REG_CMD_HANDLER_NUM_WAIT_EVENTS 0xFC02
+/**Result status code for last refresh command. Updates after refresh command.
+* Possible values:
+0x00: ENOERR - refresh successful
+0x13: EINVCROPX - invalid horizontal crop configuration
+0x14: EINVCROPY - invalid vertical crop configuration
+0x15: EINVTC - invalid Tilt Correction percentage
+*/
+#define REG_SEQ_ERROR_CODE 0x8406
+/* SYS_STATE values (for SYSMGR_NEXT_STATE and SYSMGR_CURRENT_STATE) */
+#define MT9M114_SYS_STATE_ENTER_CONFIG_CHANGE 0x28
+#define MT9M114_SYS_STATE_STREAMING 0x31
+#define MT9M114_SYS_STATE_START_STREAMING 0x34
+#define MT9M114_SYS_STATE_ENTER_SUSPEND 0x40
+#define MT9M114_SYS_STATE_SUSPENDED 0x41
+#define MT9M114_SYS_STATE_ENTER_STANDBY 0x50
+#define MT9M114_SYS_STATE_STANDBY 0x52
+#define MT9M114_SYS_STATE_LEAVE_STANDBY 0x54
+
+static struct regval_list mt9m114_720p36_regs[] = {
+    {REG_LOGICAL_ADDRESS_ACCESS, 2, 0x0000},
+    {REG_CAM_SENSOR_CFG_Y_ADDR_START, 2, 124},
+    {REG_CAM_SENSOR_CFG_X_ADDR_START, 2, 4},
+    {REG_CAM_SENSOR_CFG_Y_ADDR_END, 2, 851},
+    {REG_CAM_SENSOR_CFG_X_ADDR_END, 2, 1291},
+    {REG_CAM_SENSOR_CFG_PIXCLK, 4, 48000000},
+    {REG_CAM_SENSOR_CFG_ROW_SPEED, 2, 0x0001},
+    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 2, 219},
+    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 2, 1558},
+    {REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 2, 778},
+    {REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK, 2, 1689},
+    {REG_CAM_SENSOR_CFG_FINE_CORRECTION, 2, 96},
+    {REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW, 2, 723},
+    {REG_CAM_SENSOR_CFG_REG_0_DATA, 2, 0x0020},
+// {REG_CAM_SENSOR_CONTROL_READ_MODE, 1, 0x0000},
+    {REG_CAM_CROP_WINDOW_XOFFSET, 2, 0x0000},
+    {REG_CAM_CROP_WINDOW_YOFFSET, 2, 0x0000},
+    {REG_CAM_CROP_WINDOW_WIDTH, 2, 1280},
+    {REG_CAM_CROP_WINDOW_HEIGHT, 2, 720},
+    {REG_CROP_CROPMODE, 1, 0x03},
+    {REG_CAM_OUTPUT_WIDTH, 2, 1280},
+    {REG_CAM_OUTPUT_HEIGHT, 2, 720},
+    {REG_CAM_AET_AEMODE, 1, 0x00},
+    {REG_CAM_AET_MAX_FRAME_RATE, 2, 0x24AB},
+    {REG_CAM_AET_MIN_FRAME_RATE, 2, 0x24AB},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART, 2, 0x0000},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART, 2, 0x0000},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_XEND, 2, 1279},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_YEND, 2, 719},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART, 2, 0x0000},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART, 2, 0x0000},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_XEND, 2, 255},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_YEND, 2, 143},
+    { 0xffff, 0xffff ,0xffff }	/* END MARKER */
+};
+
+static struct regval_list mt9m114_vga_30_scaling_regs[] = {
+    {REG_LOGICAL_ADDRESS_ACCESS, 2, 0x0000},
+    {REG_CAM_SENSOR_CFG_Y_ADDR_START, 2, 4},
+    {REG_CAM_SENSOR_CFG_X_ADDR_START, 2, 4},
+    {REG_CAM_SENSOR_CFG_Y_ADDR_END, 2, 971},
+    {REG_CAM_SENSOR_CFG_X_ADDR_END, 2, 1291},
+    {REG_CAM_SENSOR_CFG_PIXCLK, 4, 48000000},
+    {REG_CAM_SENSOR_CFG_ROW_SPEED, 2, 1},//FIXME according to the documentation this value is unused, however we still set the default. No idea why
+    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 2, 219},
+    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 2, 1460},
+    {REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 2, 1006},//FIXME might be a typo. default value is 1007
+    {REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK, 2, 1591},//FIXME might be a typo? default is 1589
+    {REG_CAM_SENSOR_CFG_FINE_CORRECTION, 2, 96},
+    {REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW, 2, 963},
+    {REG_CAM_SENSOR_CFG_REG_0_DATA, 2, 0x0020},
+// {REG_CAM_SENSOR_CONTROL_READ_MODE, 1, 0x0000},
+    {REG_CAM_CROP_WINDOW_XOFFSET, 2, 0x0000},
+    {REG_CAM_CROP_WINDOW_YOFFSET, 2, 0x0000},
+    {REG_CAM_CROP_WINDOW_WIDTH, 2, 1280},
+    {REG_CAM_CROP_WINDOW_HEIGHT, 2, 960},
+    {REG_CROP_CROPMODE, 1, 3},
+    {REG_CAM_OUTPUT_WIDTH, 2, 640},
+    {REG_CAM_OUTPUT_HEIGHT, 2, 480},
+    {REG_CAM_AET_AEMODE, 1, 0x00},
+    {REG_CAM_AET_MAX_FRAME_RATE, 2, 0x1DFD},
+    {REG_CAM_AET_MIN_FRAME_RATE, 2, 0x1DFD},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART, 2, 0x0000},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART, 2, 0x0000},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_XEND, 2, 639},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_YEND, 2, 479},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART, 2, 0x0000},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART, 2, 0x0000},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_XEND, 2, 127},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_YEND, 2, 95},
+// {REG_AUTO_BINNING_MODE,1, 0x00},
+    { 0xffff, 0xffff ,0xffff }//array end,
+};
+
+static struct regval_list mt9m114_qvga_30_scaling_regs[] = {
+    {REG_LOGICAL_ADDRESS_ACCESS, 2, 0x0000},
+    {REG_CAM_SENSOR_CFG_Y_ADDR_START, 2, 4},
+    {REG_CAM_SENSOR_CFG_X_ADDR_START, 2, 4},
+    {REG_CAM_SENSOR_CFG_Y_ADDR_END, 2, 971},
+    {REG_CAM_SENSOR_CFG_X_ADDR_END, 2, 1291},
+    {REG_CAM_SENSOR_CFG_PIXCLK, 4, 48000000},
+    {REG_CAM_SENSOR_CFG_ROW_SPEED, 2, 1},
+    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 2, 219},
+    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 2, 1460},
+    {REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 2, 1006},
+    {REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK, 2, 1591},
+    {REG_CAM_SENSOR_CFG_FINE_CORRECTION, 2, 96},
+    {REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW, 2, 963},
+    {REG_CAM_SENSOR_CFG_REG_0_DATA, 2, 0x0020},
+    //{REG_CAM_SENSOR_CONTROL_READ_MODE, 1, 0x0000},
+    {REG_CAM_CROP_WINDOW_XOFFSET, 2, 0x0000},
+    {REG_CAM_CROP_WINDOW_YOFFSET, 2, 0x0000},
+    {REG_CAM_CROP_WINDOW_WIDTH, 2, 1280},
+    {REG_CAM_CROP_WINDOW_HEIGHT, 2, 960},
+    {REG_CROP_CROPMODE, 1, 3},
+    {REG_CAM_OUTPUT_WIDTH, 2, 320},
+    {REG_CAM_OUTPUT_HEIGHT, 2, 240},
+    {REG_CAM_AET_AEMODE, 1, 0x00},
+    {REG_CAM_AET_MAX_FRAME_RATE, 2, 0x1DFD},
+    {REG_CAM_AET_MIN_FRAME_RATE, 2, 0x1DFD},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART, 2, 0x0000},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART, 2, 0x0000},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_XEND, 2, 319},
+    {REG_CAM_STAT_AWB_CLIP_WINDOW_YEND, 2, 239},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART, 2, 0x0000},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART, 2, 0x0000},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_XEND, 2, 63},
+    {REG_CAM_STAT_AE_INITIAL_WINDOW_YEND, 2, 47},
+//  {REG_AUTO_BINNING_MODE,1, 0x00},
+
+    { 0xffff, 0xffff ,0xffff }
+};
+/*
+static struct regval_list mt9m114_qvga_30_scaling_regs[] = {
+  {REG_CAM_OUTPUT_WIDTH, 2, 320},
+  {REG_CAM_OUTPUT_HEIGHT, 2, 240},
+  { 0xffff, 0xffff ,0xffff }
+};*/
+static struct regval_list pga_regs[] = {
+    { 0x098E, 2, 0},
+    { 0xC95E, 2, 3},
+    { 0xC95E, 2, 2},
+    { 0x3640, 2, 368},
+    { 0x3642, 2, 3787},
+    { 0x3644, 2, 22480},
+    { 0x3646, 2, 33549},
+    { 0x3648, 2, 62062},
+    { 0x364A, 2, 32303},
+    { 0x364C, 2, 18603},
+    { 0x364E, 2, 26192},
+    { 0x3650, 2, 52556},
+    { 0x3652, 2, 44686},
+    { 0x3654, 2, 32431},
+    { 0x3656, 2, 23244},
+    { 0x3658, 2, 7056},
+    { 0x365A, 2, 64140},
+    { 0x365C, 2, 37614},
+    { 0x365E, 2, 32207},
+    { 0x3660, 2, 19178},
+    { 0x3662, 2, 26800},
+    { 0x3664, 2, 45101},
+    { 0x3666, 2, 43151},
+    { 0x3680, 2, 13964},
+    { 0x3682, 2, 1869},
+    { 0x3684, 2, 9871},
+    { 0x3686, 2, 32394},
+    { 0x3688, 2, 38832},
+    { 0x368A, 2, 492},
+    { 0x368C, 2, 2894},
+    { 0x368E, 2, 4687},
+    { 0x3690, 2, 45006},
+    { 0x3692, 2, 34192},
+    { 0x3694, 2, 973},
+    { 0x3696, 2, 2349},
+    { 0x3698, 2, 25323},
+    { 0x369A, 2, 41294},
+    { 0x369C, 2, 46959},
+    { 0x369E, 2, 3405},
+    { 0x36A0, 2, 47531},
+    { 0x36A2, 2, 38860},
+    { 0x36A4, 2, 22506},
+    { 0x36A6, 2, 37359},
+    { 0x36C0, 2, 3569},
+    { 0x36C2, 2, 36620},
+    { 0x36C4, 2, 30224},
+    { 0x36C6, 2, 11116},
+    { 0x36C8, 2, 42739},
+    { 0x36CA, 2, 1681},
+    { 0x36CC, 2, 61514},
+    { 0x36CE, 2, 13265},
+    { 0x36D0, 2, 44462},
+    { 0x36D2, 2, 51635},
+    { 0x36D4, 2, 23184},
+    { 0x36D6, 2, 39789},
+    { 0x36D8, 2, 22480},
+    { 0x36DA, 2, 3885},
+    { 0x36DC, 2, 64882},
+    { 0x36DE, 2, 3505},
+    { 0x36E0, 2, 46314},
+    { 0x36E2, 2, 26864},
+    { 0x36E4, 2, 36813},
+    { 0x36E6, 2, 41555},
+    { 0x3700, 2, 1325},
+    { 0x3702, 2, 60557},
+    { 0x3704, 2, 46961},
+    { 0x3706, 2, 13199},
+    { 0x3708, 2, 25234},
+    { 0x370A, 2, 10253},
+    { 0x370C, 2, 36912},
+    { 0x370E, 2, 46449},
+    { 0x3710, 2, 17713},
+    { 0x3712, 2, 19282},
+    { 0x3714, 2, 10509},
+    { 0x3716, 2, 53295},
+    { 0x3718, 2, 38417},
+    { 0x371A, 2, 8881},
+    { 0x371C, 2, 26834},
+    { 0x371E, 2, 27981},
+    { 0x3720, 2, 39469},
+    { 0x3722, 2, 34321},
+    { 0x3724, 2, 5232},
+    { 0x3726, 2, 20978},
+    { 0x3740, 2, 35307},
+    { 0x3742, 2, 49806},
+    { 0x3744, 2, 62036},
+    { 0x3746, 2, 23250},
+    { 0x3748, 2, 27830},
+    { 0x374A, 2, 8111},
+    { 0x374C, 2, 51085},
+    { 0x374E, 2, 33653},
+    { 0x3750, 2, 24914},
+    { 0x3752, 2, 29270},
+    { 0x3754, 2, 5133},
+    { 0x3756, 2, 5933},
+    { 0x3758, 2, 52436},
+    { 0x375A, 2, 13362},
+    { 0x375C, 2, 18166},
+    { 0x375E, 2, 37550},
+    { 0x3760, 2, 39566},
+    { 0x3762, 2, 61300},
+    { 0x3764, 2, 23602},
+    { 0x3766, 2, 26198},
+    { 0x3782, 2, 480},
+    { 0x3784, 2, 672},
+    { 0xC960, 2, 2800},
+    { 0xC962, 2, 31149},
+    { 0xC964, 2, 22448},
+    { 0xC966, 2, 30936},
+    { 0xC968, 2, 29792},
+    { 0xC96A, 2, 4000},
+    { 0xC96C, 2, 33143},
+    { 0xC96E, 2, 33116},
+    { 0xC970, 2, 33041},
+    { 0xC972, 2, 32855},
+    { 0xC974, 2, 6500},
+    { 0xC976, 2, 31786},
+    { 0xC978, 2, 26268},
+    { 0xC97A, 2, 32319},
+    { 0xC97C, 2, 29650},
+    { 0xC95E, 2, 3},
+    { 0xffff, 0xffff ,0xffff}
+};
+
+static struct regval_list ccm_awb_regs[] = {
+    { 0xC892, 2, 615},
+    { 0xC894, 2, 65306},
+    { 0xC896, 2, 65459},
+    { 0xC898, 2, 65408},
+    { 0xC89A, 2, 358},
+    { 0xC89C, 2, 3},
+    { 0xC89E, 2, 65434},
+    { 0xC8A0, 2, 65204},
+    { 0xC8A2, 2, 589},
+    { 0xC8A4, 2, 447},
+    { 0xC8A6, 2, 65281},
+    { 0xC8A8, 2, 65523},
+    { 0xC8AA, 2, 65397},
+    { 0xC8AC, 2, 408},
+    { 0xC8AE, 2, 65533},
+    { 0xC8B0, 2, 65434},
+    { 0xC8B2, 2, 65255},
+    { 0xC8B4, 2, 680},
+    { 0xC8B6, 2, 473},
+    { 0xC8B8, 2, 65318},
+    { 0xC8BA, 2, 65523},
+    { 0xC8BC, 2, 65459},
+    { 0xC8BE, 2, 306},
+    { 0xC8C0, 2, 65512},
+    { 0xC8C2, 2, 65498},
+    { 0xC8C4, 2, 65229},
+    { 0xC8C6, 2, 706},
+    { 0xC8C8, 2, 117},
+    { 0xC8CA, 2, 284},
+    { 0xC8CC, 2, 154},
+    { 0xC8CE, 2, 261},
+    { 0xC8D0, 2, 164},
+    { 0xC8D2, 2, 172},
+    { 0xC8D4, 2, 2700},
+    { 0xC8D6, 2, 3850},
+    { 0xC8D8, 2, 6500},
+    { 0xC914, 2, 0},
+    { 0xC916, 2, 0},
+    { 0xC918, 2, 1279},
+    { 0xC91A, 2, 719},
+    { 0xC904, 2, 51},
+    { 0xC906, 2, 64},
+    { 0xC8F2, 1, 3},
+    { 0xC8F3, 1, 2},
+    { 0xC906, 2, 60},
+    { 0xC8F4, 2, 0},
+    { 0xC8F6, 2, 0},
+    { 0xC8F8, 2, 0},
+    { 0xC8FA, 2, 59172},
+    { 0xC8FC, 2, 5507},
+    { 0xC8FE, 2, 8261},
+    { 0xC900, 2, 1023},
+    { 0xC902, 2, 124},
+    { 0xC90C, 1, 128},
+    { 0xC90D, 1, 128},
+    { 0xC90E, 1, 128},
+    { 0xC90F, 1, 136},
+    { 0xC910, 1, 128},
+    { 0xC911, 1, 128},
+    { 0xffff, 0xffff ,0xffff}
+};
+
+
+using std::string;
+using namespace flair::core;
+
+namespace flair {
+namespace sensor {
+
+//construct V4LCamera with 1280*720 because
+//CV_CAP_PROP_FRAME_WIDTH and CV_CAP_PROP_FRAME_HEIGHT does not work properly (isp problem?)
+ParrotCamH::ParrotCamH(const FrameworkManager* parent,string name,uint16_t width,uint16_t height,uint8_t priority) : V4LCamera(parent,name,1,1280,720,cvimage::Type::Format::UYVY,priority) {
+    //handle resolution
+    if(width==1280 && height==720) {
+        wh_regval_list=&mt9m114_720p36_regs[0];
+    } else if(width==640 && height==360) {
+        wh_regval_list=&mt9m114_vga_30_scaling_regs[0];
+    } else if(width==320 && height==180) {
+        wh_regval_list=&mt9m114_qvga_30_scaling_regs[0];
+    } else {
+        Thread::Err("Invalid width (%i) and height (%i)\n",width,height);
+        wh_regval_list=&mt9m114_qvga_30_scaling_regs[0];//default value
+    }
+
+    //realocate output, because Camera was created with 1280*720
+    //do not allocate imagedata, allocation is done by the camera
+    delete output;
+    output=new cvimage((IODevice*)this,width,height,cvimage::Type::Format::UYVY,"out",false);
+
+    //i2cport
+    //we assume there is no other device on this bus
+    i2cport=new Unix_I2cPort((Thread*)this,"camv-i2c","/dev/i2c-3");
+    i2cport->SetSlave(0x5d);
+
+    mt9m114_detect();
+}
+
+ParrotCamH::~ParrotCamH() {
+
+}
+
+void ParrotCamH::mt9m114_detect(void) {
+    uint32_t chip_id;
+    uint32_t mon_major_version;
+    uint32_t mon_minor_version;
+    uint32_t mon_release_version;
+
+    soc1040_read_reg(REG_CHIP_ID, &chip_id,2);
+    soc1040_read_reg(REG_MON_MAJOR_VERSION, &mon_major_version,2);
+    soc1040_read_reg(REG_MON_MINOR_VERION, &mon_minor_version,2);
+    soc1040_read_reg(REG_MON_RELEASE_VERSION, &mon_release_version,2);
+
+    if(chip_id!=0)
+        Printf("MT9M114 found : chip_id:%x major:%x minor:%x release:%x\n", chip_id,mon_major_version,mon_minor_version,mon_release_version);
+
+    if (chip_id != 0x2481) { // default chipid
+        Thread::Err("invalid chipid\n");
+        return;
+    }
+
+    mt9m114_init();
+}
+
+void ParrotCamH::mt9m114_init(void) {
+    int ret = 0;
+    ret += mt9m114_reset();
+
+    ret += mt9m114_PLL_settings();
+    //ret += mt9m114_write_array(mt9m114_vga_30_scaling_regs);
+    ret += mt9m114_write_array(wh_regval_list);
+    ret += mt9m114_sensor_optimization();
+    ret += mt9m114_errata_1();
+    ret += mt9m114_errata_2();
+    ret += mt9m114_write_array(pga_regs);
+    ret += mt9m114_write_array(ccm_awb_regs);
+    /*
+    ret += mt9m114_CPIPE_preference();
+    ret += mt9m114_features();
+    ret += mt9m114_write_array(uvc_ctrl_regs);
+    ret += mt9m114_change_config();
+    ret += mt9m114_patch2_black_lvl_correction_fix();
+    ret += mt9m114_patch3_adaptive_sensitivity();*/
+    if (ret != 0) {
+        Thread::Err("MT9M114","MT9M114 : init fail\n");
+    }
+
+}
+
+int ParrotCamH::mt9m114_reset(void) {
+    uint32_t v;
+    Printf("MT9M114 : Resetting chip!\n");
+
+    int fd=open("/dev/v4l-subdev8",O_WRONLY);//part of the reset sequence?
+    close(fd);
+
+    soc1040_read_reg( REG_RESET_AND_MISC_CONTROL, &v,2);
+    soc1040_write_reg(REG_RESET_AND_MISC_CONTROL, v|0x01,2);
+    SleepMS(100); //FIXME This sleep shouldn't be here according to the documentation
+    soc1040_write_reg(REG_RESET_AND_MISC_CONTROL, v & (~1),2);
+    SleepMS(100); //datasheet documentation
+    mt9m114_errata_2();
+
+    system("media-ctl -f \"\\\"soc1040\\\":0[UYVY2X8 1280x720]\"");//part of the reset sequence?
+    return 0;
+}
+
+int ParrotCamH::mt9m114_PLL_settings(void) {
+    soc1040_write_reg(REG_LOGICAL_ADDRESS_ACCESS, 0x0000,2);
+    soc1040_write_reg(REG_CAM_SYSCTL_PLL_ENABLE, 1, 1); //no idea what it does. 1 is default
+    soc1040_write_reg(REG_CAM_SYSCTL_PLL_DIVIDER_M_N, 0x0120,2); //no idea, default is 0x09a0
+    soc1040_write_reg(REG_CAM_SYSCTL_PLL_DIVIDER_P, 0x0700,2);//no idea, default is 0x0700
+    return 0;
+}
+
+int ParrotCamH::mt9m114_errata_1(void) {
+    soc1040_write_reg( REG_SAMP_COL_PUP2,0xFF39,2); //no idea, register is undocumented
+    return 0;
+}
+
+int ParrotCamH::mt9m114_errata_2(void) {
+    //FIXME this doesn't make sense
+    //bit 2 is reserved and the default value is 0 but it is set to 1.
+    //bit 5 is reserved, default is 0 but it is set to 1
+    //MSB is the actual reset register. No idea why the others are changed.
+    soc1040_write_reg(REG_RESET_REGISTER, 564,2); //1000110100
+    return 0;
+}
+
+int ParrotCamH::mt9m114_sensor_optimization(void) {
+    //FIXME all registers used in here are undocumented. No idea about their purpose.
+    //      Maybe some commands for the fpga?
+    soc1040_write_reg( REG_DAC_TXLO_ROW,0x8270,2);
+    soc1040_write_reg(REG_DAC_TXLO, 0x8270,2);
+    soc1040_write_reg(REG_DAC_LD_4_5, 0x3605,2);
+    soc1040_write_reg(REG_DAC_LD_6_7, 0x77FF,2);
+    soc1040_write_reg(REG_DAC_ECL, 0xC233,2);
+    soc1040_write_reg(REG_DELTA_DK_CONTROL, 0x87FF,2);
+    soc1040_write_reg(REG_COLUMN_CORRECTION, 0x6080,2);
+    soc1040_write_reg(REG_AE_TRACK_MODE,0x0008,2);
+    return 0;
+}
+
+void ParrotCamH::SetGain(float value) {
+    int ret = 0;
+    uint16_t shiftedValue = value*255;
+
+    printf("gain %i\n",shiftedValue);
+    ret = soc1040_write_reg(REG_UVC_GAIN,shiftedValue,2);
+
+    if(0 != ret) {
+        Thread::Err("error writing value. errno: 0x%x\n",  ret);
+        return ;
+    }
+
+    ret = check_uvc_status( __func__);
+    if(0 != ret) {
+        //error has already been printed by check_uvc_status.
+        return;
+    }
+    uint32_t v;
+    soc1040_read_reg(REG_UVC_GAIN,&v,2);
+    printf("read %i\n",v);
+    mt9m114_refresh();
+}
+
+void ParrotCamH::SetBrightness(float value) {
+    int ret = 0;
+    uint16_t shiftedValue = value*255;
+
+    ret = soc1040_write_reg(REG_UVC_BRIGHTNESS,shiftedValue,2);
+
+    if(0 != ret) {
+        Thread::Err("error writing value. errno: 0x%x\n", ret);
+        return;
+    }
+    ret = check_uvc_status( __func__);
+
+    if(0 != ret) {
+        //error has already been printed by check_uvc_status.
+        return;
+    }
+
+    mt9m114_refresh();
+}
+
+void ParrotCamH::SetSaturation(float value) {
+    int ret = 0;
+    uint16_t shiftedValue = value*255;
+
+    ret = soc1040_write_reg(REG_UVC_SATURATION,shiftedValue,2);
+
+    if(0 != ret) {
+        Thread::Err("error writing value. errno: 0x%x\n",  ret);
+        return ;
+    }
+
+    ret = check_uvc_status( __func__);
+    if(0 != ret) {
+        //error has already been printed by check_uvc_status.
+        return;
+    }
+
+    mt9m114_refresh();
+}
+
+void ParrotCamH::SetExposure(float value) {
+    int ret = 0;
+    uint32_t shiftedValue = value*350;//trouve experimentalment
+
+    ret = soc1040_write_reg(REG_UVC_EXPOSURE_TIME,shiftedValue,4);
+
+    if(0 != ret) {
+        Thread::Err("error writing value. errno: 0x%x\n",  ret);
+        return ;
+    }
+
+    ret = check_uvc_status( __func__);
+    if(0 != ret) {
+        //error has already been printed by check_uvc_status.
+        return;
+    }
+
+    mt9m114_refresh();
+}
+
+void ParrotCamH::SetHue(float value) {
+    int8_t shiftedValue = (value-.5)*255;
+
+    int ret = soc1040_write_reg(REG_CAM_HUE_ANGLE, shiftedValue,1);
+
+    if(0 != ret) {
+        Thread::Err("error writing value. errno: 0x%x\n",  ret);
+        return ;
+    }
+
+    mt9m114_refresh();
+}
+//not exactly what the name suggests. See chip documentation
+void ParrotCamH::SetContrast(float value) {
+    int ret = 0;
+    uint16_t shiftedValue = 16+(value*48);//min 16, max 64
+
+    ret = soc1040_write_reg(REG_UVC_CONTRAST,shiftedValue,2);
+
+    if(0 != ret) {
+        Thread::Err("error writing value. errno: 0x%x\n", ret);
+        return ;
+    }
+    ret = check_uvc_status( __func__);
+    if(0 != ret) {
+        //error has already been printed by check_uvc_status.
+        return ;
+    }
+    mt9m114_refresh();
+}
+
+void ParrotCamH::SetAutoExposure(bool value) {
+    if(value == true) {
+        soc1040_write_reg( REG_UVC_AE_MODE, 0x02,1);
+    } else {
+        soc1040_write_reg( REG_UVC_AE_MODE, 0x01,1);
+    }
+
+    mt9m114_refresh();
+}
+
+/**
+ * soc1040_read_reg - Read a value from a register in an soc1040 sensor device
+ * @reg: register address / offset
+ * @val: stores the value that gets read
+ * @data_length: length of data to be read
+ *
+ * Read a value from a register in an soc1040 sensor device.
+ * The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int ParrotCamH::soc1040_read_reg(uint16_t reg, uint32_t *val,uint16_t data_length) {
+    int err;
+    unsigned char data[4];
+    unsigned char add[2];
+
+    if (data_length != I2C_8BIT && data_length != I2C_16BIT
+            && data_length != I2C_32BIT)
+        return -EINVAL;
+
+    /* Write addr - high byte goes out first */
+    add[0] = (uint8_t) (reg >> 8);;
+    add[1] = (uint8_t) (reg & 0xff);
+    err=i2cport->Write(add,2);
+
+    /* Read back data */
+    if (err >= 0) {
+        err = i2cport->Read(data,data_length);
+    }
+    if (err >= 0) {
+        *val = 0;
+        /* high byte comes first */
+        if (data_length == I2C_8BIT)
+            *val = data[0];
+        else if (data_length == I2C_16BIT)
+            *val = data[1] + (data[0] << 8);
+        else
+            *val = data[3] + (data[2] << 8) +
+                   (data[1] << 16) + (data[0] << 24);
+        return 0;
+    }
+    Thread::Err("read from offset 0x%x error %d\n", reg, err);
+    return err;
+}
+
+/**
+ * Write a value to a register in soc1040 sensor device.
+ * @reg: Address of the register to read value from.
+ * @val: Value to be written to a specific register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+int ParrotCamH::soc1040_write_reg(uint16_t reg,uint32_t val, uint16_t data_length) {
+    int err = 0;
+    unsigned char data[6];
+
+    if (data_length != I2C_8BIT && data_length != I2C_16BIT
+            && data_length != I2C_32BIT)
+        return -EINVAL;
+
+
+    /* high byte goes out first */
+    data[0] = (uint8_t) (reg >> 8);
+    data[1] = (uint8_t) (reg & 0xff);
+    if (data_length == I2C_8BIT) {
+        data[2] = val & 0xff;
+    } else if (data_length == I2C_16BIT) {
+        data[2] = (val >> 8) & 0xff;
+        data[3] = val & 0xff;
+    } else {
+        data[2] = (val >> 24) & 0xff;
+        data[3] = (val >> 16) & 0xff;
+        data[4] = (val >> 8) & 0xff;
+        data[5] = val & 0xff;
+    }
+
+    err = i2cport->Write(data,2+data_length);
+
+    if (err >= 0) return 0;
+
+    Thread::Err("write from offset 0x%x error %d\n", reg, err);
+
+    return err;
+}
+
+/*
+ * Write a list of register settings; ff/ff stops the process.
+ */
+int ParrotCamH::mt9m114_write_array(struct regval_list *vals) {
+    int i=0;
+
+    while ((vals[i].reg_num != 0xffff) || (vals[i].value != 0xffff)) {
+        int ret = soc1040_write_reg(vals[i].reg_num,  vals[i].value,vals[i].size);
+        if (ret < 0) {
+            return ret;
+        }
+        i++;
+    }
+    return 0;
+}
+/**
+* Reads the uvc_result_status register.
+* If it contains an error the error is printed to dmesg together with funcName.
+*
+* @note uvc_result_status updates on vertical blanking. Therefore it might not
+* contain the correct value if you do not wait until vertical blanking.
+* @param funcName Name of the calling function.
+* @return In case of error: The errno code or -1 if the error code is unknown. 0 Otherwise.
+*/
+int ParrotCamH::check_uvc_status(const char* funcName) {
+    uint32_t result;
+    soc1040_read_reg(REG_UVC_RESULT_STATUS, &result,1);
+    /* uvc_result_status can contain the following error codes:
+    0x00 ENOERR no error - change was accepted and actioned.
+    0x08 EACCES permission denied.
+    0x09 EBUSY entity busy, cannot support operation.
+    0x0C EINVAL invalid argument.
+    0x0E ERANGE parameter out-of-range.
+    0x0F ENOSYS operation not supported.
+    */
+    switch(result) {
+    case 0: //no error
+        //Printf("%s REG_UVC_RESULT_STATUS: ENOERR\n", funcName);
+        return 0;
+    case 0x08: //EACCES
+        Printf("%s REG_UVC_RESULT_STATUS: EACCES\n", funcName);
+        return EACCES;
+    case 0x09: //EBUSY
+        Printf("%s REG_UVC_RESULT_STATUS: EBUSY\n", funcName);
+        return EBUSY;
+    case 0x0C: //EINVAL
+        Printf("%s REG_UVC_RESULT_STATUS: EINVAL\n", funcName);
+        return EINVAL;
+    case 0x0E: //ERANGE
+        Printf("%s REG_UVC_RESULT_STATUS: ERANGE\n", funcName);
+        return ERANGE;
+    case 0x0F: //ENOSYS
+        Printf("%s REG_UVC_RESULT_STATUS: ENOSYS\n", funcName);
+        return ENOSYS;
+    default:
+        Printf("%s REG_UVC_RESULT_STATUS: Unknown error code\n", funcName);
+        return -1;
+    }
+}
+
+/**
+* Refresh subsystems without requiring a sensor configuration change.
+* @note This call blocks till the next frame.
+* @return
+*/
+int ParrotCamH::mt9m114_refresh(void) {
+    //Printf("referesh\n");
+    uint32_t v=0;//a temporary variable used for several read commands
+    //make sure that the refresh command is really processed and that
+    //exposure and user changes are processed as well.
+    soc1040_read_reg(REG_UVC_ALGO, &v,2);
+    v |= 0b111;
+    soc1040_write_reg(REG_UVC_ALGO, v,2);
+    //Changes to REG_UVC_ALGO take effect on vertical blanking, therefore wait one frame.
+    mt9m114_wait_num_frames(1);
+    // First check that the FW is ready to accept a Refresh command
+    soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
+    if (v & HOST_COMMAND_REFRESH) {
+        Thread::Warn("Refresh cmd bit is already set 0x%x\n",v);
+        return -1;
+    }
+    // Issue the Refresh command, and set the 'OK' bit at the time time so
+    //we can detect if the command fails
+    soc1040_write_reg(REG_COMMAND_REGISTER, HOST_COMMAND_REFRESH|HOST_COMMAND_OK,2);
+    // Wait for the FW to complete the command
+    poll_command_register_bit(HOST_COMMAND_REFRESH);
+    // Check the 'OK' bit to see if the command was successful
+    soc1040_read_reg( REG_COMMAND_REGISTER, &v,2);
+    if ( !(v & HOST_COMMAND_OK)) {
+        Thread::Err("refresh command fail\n");
+        return -1;
+    }
+    //check refresh command error code
+    soc1040_read_reg(REG_SEQ_ERROR_CODE, &v,1);
+    if(v != 0) {
+        Thread::Err("Refresh ERROR: %x\n", v);
+    }
+    soc1040_read_reg( REG_UVC_RESULT_STATUS, &v,1);
+    //Printf("REG_UVC_RESULT_STATUS: %x\n", v);
+    //the refresh command schedules an update on the next end of frame.
+    //It does not wait until the end of frame is actually reached.
+    //Therefore we need to wait until the end of the frame manually.
+    mt9m114_wait_num_frames(1);
+    //Printf("referesh ok\n");
+    return 0;
+}
+
+/**
+* Waits until a number of frames have passed
+* This method can be used to wait for vertical blanking as vertical blanking
+* occurs at the end of a frame.
+* @param sd
+* @param numFrames the number of frames to wait
+* @return -1 in case of error, 0 otherwise.
+*/
+int ParrotCamH::mt9m114_wait_num_frames(uint16_t numFrames) {
+    //Printf("wait\n");
+    //FIXME copy & paste from set_state_command
+    uint32_t v=0;
+    uint32_t frameCountBefore = 0;
+    uint32_t frameCountAfter = 0;
+    //get the current frame count
+    soc1040_read_reg(CAM_MON_HEARTBEAT,&frameCountBefore,2);
+    //Printf("frame %i\n",frameCountBefore);
+    //specify for which event we want to wait: 2 = start of next frame
+    soc1040_write_reg(REG_CMD_HANDLER_WAIT_FOR_EVENT, 2, 2);
+    //specify for how much frames we want to wait
+    soc1040_write_reg( REG_CMD_HANDLER_NUM_WAIT_EVENTS, numFrames,2);
+    // (Optional) First check that the FW is ready to accept a new command
+    soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
+    if (v & HOST_COMMAND_WAIT_FOR_EVENT) {
+        //This should never happen as long as nobody opens the driver device in async mode.
+        Thread::Warn("Host command wait for event already set 0x%x\n",v);
+        return -1;
+    }
+    // (Mandatory) Issue the wait for command
+    // We set the 'OK' bit so we can detect if the command fails. The chip will unset the OK bit if everything is ok.
+    soc1040_write_reg(REG_COMMAND_REGISTER, HOST_COMMAND_WAIT_FOR_EVENT | HOST_COMMAND_OK,2);
+    // Wait for the FW to complete the command (clear the HOST_COMMAND_1 bit)
+    poll_command_register_bit(HOST_COMMAND_WAIT_FOR_EVENT);
+    // Check the 'OK' bit to see if the command was successful
+    soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
+    if ( !(v & HOST_COMMAND_OK)) {
+        Thread::Warn("wait for end of frame failed: TIMEOUT?!\n");
+        return -1;
+    }
+    //read frame count after
+    soc1040_read_reg(CAM_MON_HEARTBEAT, &frameCountAfter,2);
+    if(frameCountBefore == frameCountAfter) {
+        Thread::Warn("wait for end of frame failed. Frame is still the same.\n");
+    }
+    //Printf("wait ok\n");
+    return 0;
+}
+
+/**
+*
+* @param bit_mask
+* @return 0 if everything is ok, 1 otherwise
+*/
+int ParrotCamH::poll_command_register_bit(uint16_t bit_mask) {
+    int i=0;
+    uint32_t v=0;
+    //Printf("poll_command_register_bit\n");
+    for (i = 0; i < 500; i++) {
+        SleepMS(10);
+        soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
+        if (!(v & bit_mask)) {
+            //Printf("poll_command_register_bit ok\n");
+            return 0;
+        }
+    }
+    Printf("poll_command_register_bit ko\n");
+    return 1;
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/ParrotCamH.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamH.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotCamH.h	(revision 3)
@@ -0,0 +1,145 @@
+/*!
+ * \file ParrotCamH.h
+ * \brief Class for ardrone2 horizontal camera
+ * \author Guillaume Sanahuja
+ * \date 2014/07/16
+ * \version 4.0
+ */
+
+#ifndef PARROTCAMH_H
+#define PARROTCAMH_H
+
+#include "V4LCamera.h"
+
+struct regval_list {
+  uint16_t reg_num;
+  uint16_t size;
+  uint32_t value;
+};
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class I2cPort;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class ParrotCamH
+    *
+    * \brief Class for ardrone2 horizontal cameraH
+    */
+    class ParrotCamH : public V4LCamera
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a ParrotCamH.
+            *
+            * \param parent parent
+            * \param name name
+            * \param width width
+            * \param height height
+            * \param priority priority of the Thread
+            */
+            ParrotCamH(const core::FrameworkManager* parent,std::string name,uint16_t width,uint16_t height,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~ParrotCamH();
+
+        private:
+            /*!
+            * \brief Set Gain
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetGain(float value);
+
+            /*!
+            * \brief Set Exposure
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetExposure(float value);
+
+
+            /*!
+            * \brief Set Auto Exposure
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value
+            */
+            void SetAutoExposure(bool value);
+
+            /*!
+            * \brief Set Brightness
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetBrightness(float value);
+
+            /*!
+            * \brief Set Contrast
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetContrast(float value);
+
+            /*!
+            * \brief Set Saturation
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetSaturation(float value);
+
+            /*!
+            * \brief Set Hue
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetHue(float value);
+
+
+
+            void mt9m114_detect(void);
+            void mt9m114_init(void);
+            int mt9m114_reset(void);
+            int mt9m114_errata_1(void);
+            int mt9m114_errata_2(void);
+            int mt9m114_PLL_settings(void);
+            int mt9m114_write_array(struct regval_list *vals);
+            int mt9m114_sensor_optimization(void);
+            int soc1040_read_reg(uint16_t reg, uint32_t *val,uint16_t data_length);
+            int soc1040_write_reg(uint16_t reg,uint32_t val, uint16_t data_length);
+            int check_uvc_status(const char* funcName);
+            int mt9m114_refresh(void);
+            int mt9m114_wait_num_frames(uint16_t numFrames);
+            int poll_command_register_bit(uint16_t bit_mask);
+            core::I2cPort* i2cport;
+            struct regval_list *wh_regval_list;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // PARROTCAMH_H
Index: /trunk/lib/FlairSensorActuator/src/ParrotCamV.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamV.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotCamV.cpp	(revision 3)
@@ -0,0 +1,370 @@
+//  created:    2014/07/16
+//  filename:   ParrotCamV.cpp
+//
+//  author:     Guillaume Sanahuja
+//
+//  version:    $Id: $
+//
+//  purpose:    class for ardrone2 horizontal camera
+//
+//
+/*********************************************************************/
+
+//control functions from V4L2 driver:
+
+/*
+ * A V4L2 driver for OmniVision OV7670 cameras.
+ *
+ * Copyright 2006 One Laptop Per Child Association, Inc.  Written
+ * by Jonathan Corbet with substantial inspiration from Mark
+ * McClelland's ovcamchip code.
+ *
+ * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
+ *
+ * This file may be distributed under the terms of the GNU General
+ * Public License, version 2.
+ */
+
+#include "ParrotCamV.h"
+#include <Unix_I2cPort.h>
+
+#define PI ((float)3.14159265358979323846)
+
+/* Registers */
+#define REG_GAIN	0x00	/* Gain lower 8 bits (rest in vref) */
+#define REG_BLUE	0x01	/* blue gain */
+#define REG_RED		0x02	/* red gain */
+#define REG_VREF	0x03	/* Pieces of GAIN, VSTART, VSTOP */
+#define REG_COM1	0x04	/* Control 1 */
+#define  COM1_CCIR656	  0x40  /* CCIR656 enable */
+#define REG_BAVE	0x05	/* U/B Average level */
+#define REG_GbAVE	0x06	/* Y/Gb Average level */
+#define REG_AECHH	0x07	/* AEC MS 5 bits */
+#define REG_RAVE	0x08	/* V/R Average level */
+#define REG_COM2	0x09	/* Control 2 */
+#define  COM2_SSLEEP	  0x10	/* Soft sleep mode */
+#define REG_PID		0x0a	/* Product ID MSB */
+#define REG_VER		0x0b	/* Product ID LSB */
+#define REG_COM3	0x0c	/* Control 3 */
+#define  COM3_SWAP	  0x40	  /* Byte swap */
+#define  COM3_SCALEEN	  0x08	  /* Enable scaling */
+#define  COM3_DCWEN	  0x04	  /* Enable downsamp/crop/window */
+#define REG_COM4	0x0d	/* Control 4 */
+#define REG_COM5	0x0e	/* All "reserved" */
+#define REG_COM6	0x0f	/* Control 6 */
+#define REG_AECH	0x10	/* More bits of AEC value */
+#define REG_CLKRC	0x11	/* Clocl control */
+#define   CLK_EXT	  0x40	  /* Use external clock directly */
+#define   CLK_SCALE	  0x3f	  /* Mask for internal clock scale */
+#define REG_COM7	0x12	/* Control 7 */
+#define   COM7_RESET	  0x80	  /* Register reset */
+#define   COM7_FMT_MASK	  0x38
+#define   COM7_FMT_VGA	  0x00
+#define	  COM7_FMT_CIF	  0x20	  /* CIF format */
+#define   COM7_FMT_QVGA	  0x00	  /* QVGA format */
+#define   COM7_FMT_QCIF	  0x08	  /* QCIF format */
+#define	  COM7_RGB	  0x04	  /* bits 0 and 2 - RGB format */
+#define	  COM7_YUV	  0x00	  /* YUV */
+#define	  COM7_BAYER	  0x01	  /* Bayer format */
+#define	  COM7_PBAYER	  0x05	  /* "Processed bayer" */
+#define REG_COM8	0x13	/* Control 8 */
+#define   COM8_FASTAEC	  0x80	  /* Enable fast AGC/AEC */
+#define   COM8_AECSTEP	  0x40	  /* Unlimited AEC step size */
+#define   COM8_BFILT	  0x20	  /* Band filter enable */
+#define   COM8_AGC	  0x04	  /* Auto gain enable */
+#define   COM8_AWB	  0x02	  /* White balance enable */
+#define   COM8_AEC	  0x01	  /* Auto exposure enable */
+#define REG_COM9	0x14	/* Control 9  - gain ceiling */
+#define REG_COM10	0x15	/* Control 10 */
+#define   COM10_HSYNC	  0x40	  /* HSYNC instead of HREF */
+#define   COM10_PCLK_HB	  0x20	  /* Suppress PCLK on horiz blank */
+#define   COM10_HREF_REV  0x08	  /* Reverse HREF */
+#define   COM10_VS_LEAD	  0x04	  /* VSYNC on clock leading edge */
+#define   COM10_VS_NEG	  0x02	  /* VSYNC negative */
+#define   COM10_HS_NEG	  0x01	  /* HSYNC negative */
+#define REG_HSTART	0x17	/* Horiz start high bits */
+#define REG_HSTOP	0x18	/* Horiz stop high bits */
+#define REG_VSTART	0x19	/* Vert start high bits */
+#define REG_VSTOP	0x1a	/* Vert stop high bits */
+#define REG_PSHFT	0x1b	/* Pixel delay after HREF */
+#define REG_MIDH	0x1c	/* Manuf. ID high */
+#define REG_MIDL	0x1d	/* Manuf. ID low */
+#define REG_MVFP	0x1e	/* Mirror / vflip */
+#define   MVFP_MIRROR	  0x20	  /* Mirror image */
+#define   MVFP_FLIP	  0x10	  /* Vertical flip */
+
+#define REG_AEW		0x24	/* AGC upper limit */
+#define REG_AEB		0x25	/* AGC lower limit */
+#define REG_VPT		0x26	/* AGC/AEC fast mode op region */
+#define REG_HSYST	0x30	/* HSYNC rising edge delay */
+#define REG_HSYEN	0x31	/* HSYNC falling edge delay */
+#define REG_HREF	0x32	/* HREF pieces */
+#define REG_TSLB	0x3a	/* lots of stuff */
+#define   TSLB_YLAST	  0x04	  /* UYVY or VYUY - see com13 */
+#define REG_COM11	0x3b	/* Control 11 */
+#define   COM11_NIGHT	  0x80	  /* NIght mode enable */
+#define   COM11_NMFR	  0x60	  /* Two bit NM frame rate */
+#define   COM11_HZAUTO	  0x10	  /* Auto detect 50/60 Hz */
+#define	  COM11_50HZ	  0x08	  /* Manual 50Hz select */
+#define   COM11_EXP	  0x02
+#define REG_COM12	0x3c	/* Control 12 */
+#define   COM12_HREF	  0x80	  /* HREF always */
+#define REG_COM13	0x3d	/* Control 13 */
+#define   COM13_GAMMA	  0x80	  /* Gamma enable */
+#define	  COM13_UVSAT	  0x40	  /* UV saturation auto adjustment */
+#define   COM13_UVSWAP	  0x01	  /* V before U - w/TSLB */
+#define REG_COM14	0x3e	/* Control 14 */
+#define   COM14_DCWEN	  0x10	  /* DCW/PCLK-scale enable */
+#define REG_EDGE	0x3f	/* Edge enhancement factor */
+#define REG_COM15	0x40	/* Control 15 */
+#define   COM15_R10F0	  0x00	  /* Data range 10 to F0 */
+#define	  COM15_R01FE	  0x80	  /*            01 to FE */
+#define   COM15_R00FF	  0xc0	  /*            00 to FF */
+#define   COM15_RGB565	  0x10	  /* RGB565 output */
+#define   COM15_RGB555	  0x30	  /* RGB555 output */
+#define REG_COM16	0x41	/* Control 16 */
+#define   COM16_AWBGAIN   0x08	  /* AWB gain enable */
+#define REG_COM17	0x42	/* Control 17 */
+#define   COM17_AECWIN	  0xc0	  /* AEC window - must match COM4 */
+#define   COM17_CBAR	  0x08	  /* DSP Color bar */
+
+/*
+ * This matrix defines how the colors are generated, must be
+ * tweaked to adjust hue and saturation.
+ *
+ * Order: v-red, v-green, v-blue, u-red, u-green, u-blue
+ *
+ * They are nine-bit signed quantities, with the sign bit
+ * stored in 0x58.  Sign for v-red is bit 0, and up from there.
+ */
+#define	REG_CMATRIX_BASE 0x4f
+//#define   CMATRIX_LEN 6
+#define REG_CMATRIX_SIGN 0x58
+
+
+#define REG_BRIGHT	0x55	/* Brightness */
+#define REG_CONTRAS	0x56	/* Contrast control */
+
+#define REG_GFIX	0x69	/* Fix gain control */
+
+#define REG_REG76	0x76	/* OV's name */
+#define   R76_BLKPCOR	  0x80	  /* Black pixel correction enable */
+#define   R76_WHTPCOR	  0x40	  /* White pixel correction enable */
+
+#define REG_RGB444	0x8c	/* RGB 444 control */
+#define   R444_ENABLE	  0x02	  /* Turn on RGB444, overrides 5x5 */
+#define   R444_RGBX	  0x01	  /* Empty nibble at end */
+
+#define REG_HAECC1	0x9f	/* Hist AEC/AGC control 1 */
+#define REG_HAECC2	0xa0	/* Hist AEC/AGC control 2 */
+
+#define REG_BD50MAX	0xa5	/* 50hz banding step limit */
+#define REG_HAECC3	0xa6	/* Hist AEC/AGC control 3 */
+#define REG_HAECC4	0xa7	/* Hist AEC/AGC control 4 */
+#define REG_HAECC5	0xa8	/* Hist AEC/AGC control 5 */
+#define REG_HAECC6	0xa9	/* Hist AEC/AGC control 6 */
+#define REG_HAECC7	0xaa	/* Hist AEC/AGC control 7 */
+#define REG_BD60MAX	0xab	/* 60hz banding step limit */
+
+using std::string;
+using namespace flair::core;
+
+namespace flair { namespace sensor {
+
+ParrotCamV::ParrotCamV(const FrameworkManager* parent,string name,uint8_t priority) : V4LCamera(parent,name,2,320,240,cvimage::Type::Format::UYVY,priority) {
+    //i2cport
+    //we assume there is no other device on this bus
+    i2cport=new Unix_I2cPort((Thread*)this,"camv-i2c","/dev/i2c-2");
+    i2cport->SetSlave(0x42>>1);
+}
+
+ParrotCamV::~ParrotCamV() {
+
+}
+
+void ParrotCamV::SetGain(float value) {
+	if(ov7670_write(REG_GAIN, (int)(value*255) & 0xff)!=2) {
+	    Thread::Err("error writting gain\n");
+	}
+}
+
+void ParrotCamV::SetAutoGain(bool value) {
+    unsigned char com8;
+
+    if(ov7670_read(REG_COM8, &com8)!=1) {
+        Thread::Err("error reading com8\n");
+        return;
+    }
+
+    if (value) com8 |= COM8_AGC;
+    else com8 &= ~COM8_AGC;
+
+    if(ov7670_write(REG_COM8, com8)!=2) {
+	    Thread::Err("error writting com8\n");
+	}
+}
+
+void ParrotCamV::SetAutoExposure(bool value) {
+    unsigned char com8;
+
+    if(ov7670_read(REG_COM8, &com8)!=1) {
+        Thread::Err("error reading com8\n");
+        return;
+    }
+
+    if (value) com8 |= COM8_AEC;
+    else com8 &= ~COM8_AEC;
+
+    if(ov7670_write(REG_COM8, com8)!=2) {
+	    Thread::Err("error writting com8\n");
+	}
+}
+
+void ParrotCamV::SetExposure(float value) {
+	unsigned char com1,aech, aechh;
+	uint16_t value16=value*255.;//16 bit register but values after 255 does not change anything...
+
+    if(ov7670_read(REG_COM1, &com1)!=1) {
+        Thread::Err("error reading com1\n");
+        return;
+    }
+
+    if(ov7670_read(REG_AECHH, &aechh)!=1) {
+        Thread::Err("error reading aechh\n");
+        return;
+    }
+
+	com1 = (com1 & 0xfc) | (value16 & 0x03);
+	aech = (value16 >> 2) & 0xff;
+	aechh = (aechh & 0xc0) | ((value16 >> 10) & 0x3f);
+
+	if(ov7670_write(REG_COM1, com1)!=2) {
+	    Thread::Err("error writting com1\n");
+	}
+	if(ov7670_write(REG_AECH, aech)!=2) {
+	    Thread::Err("error writting aech\n");
+	}
+	if(ov7670_write(REG_AECHH, aechh)!=2) {
+	    Thread::Err("error writting aechh\n");
+	}
+}
+
+void ParrotCamV::SetBrightness(float value) {
+    uint8_t v;
+	v = ov7670_abs_to_sm(value*255.);
+
+	if(ov7670_write(REG_BRIGHT, v)!=2) {
+	    Thread::Err("error writting REG_BRIGHT\n");
+	}
+}
+
+void ParrotCamV::SetSaturation(float value) {
+	int matrix[CMATRIX_LEN];
+
+	sat=value;
+
+	ov7670_calc_cmatrix(matrix);
+	ov7670_store_cmatrix(matrix);
+}
+
+void ParrotCamV::SetHue(float value) {
+	int matrix[CMATRIX_LEN];
+
+	hue = value*2*PI-PI;
+
+	ov7670_calc_cmatrix(matrix);
+	ov7670_store_cmatrix(matrix);
+}
+
+void ParrotCamV::SetContrast(float value) {
+	ov7670_write(REG_CONTRAS, (unsigned char)(value*255));
+}
+
+unsigned char ParrotCamV::ov7670_abs_to_sm(uint8_t v) {
+    if(v==0) v=1;//0 non gére sinon...
+	if (v > 127) return v & 0x7f;
+	return (128 - v) | 0x80;
+}
+
+int ParrotCamV::ov7670_store_cmatrix(int matrix[CMATRIX_LEN]) {
+	int i, ret;
+	unsigned char signbits = 0;
+
+	/*
+	 * Weird crap seems to exist in the upper part of
+	 * the sign bits register, so let's preserve it.
+	 */
+	ret = ov7670_read(REG_CMATRIX_SIGN, &signbits);
+	signbits &= 0xc0;
+
+	for (i = 0; i < CMATRIX_LEN; i++) {
+		unsigned char raw;
+
+		if (matrix[i] < 0) {
+			signbits |= (1 << i);
+			if (matrix[i] < -255)
+				raw = 0xff;
+			else
+				raw = (-1 * matrix[i]) & 0xff;
+		}
+		else {
+			if (matrix[i] > 255)
+				raw = 0xff;
+			else
+				raw = matrix[i] & 0xff;
+		}
+		ret += ov7670_write(REG_CMATRIX_BASE + i, raw);
+	}
+	ret += ov7670_write(REG_CMATRIX_SIGN, signbits);
+	return ret;
+}
+
+void ParrotCamV::ov7670_calc_cmatrix(int matrix[CMATRIX_LEN]) {
+	int i;
+	/*
+	 * Apply the current saturation setting first.
+	 */
+	for (i = 0; i < CMATRIX_LEN; i++)
+		matrix[i] = (cmatrix[i]*(int)(sat*255)) >> 7;
+	/*
+	 * Then, if need be, rotate the hue value.
+	 */
+	if (hue != 0) {
+		float sinth, costh;
+
+		sinth = sinf(hue);
+		costh = cosf(hue);
+
+		matrix[0] = matrix[3]*sinth + matrix[0]*costh;
+		matrix[1] = matrix[4]*sinth + matrix[1]*costh;
+		matrix[2] = matrix[5]*sinth + matrix[2]*costh;
+		matrix[3] = matrix[3]*costh - matrix[0]*sinth;
+		matrix[4] = matrix[4]*costh - matrix[1]*sinth;
+		matrix[5] = matrix[5]*costh - matrix[2]*sinth;
+	}
+}
+
+int ParrotCamV::ov7670_write(uint8_t reg,uint8_t value) {
+    uint8_t tx[2];
+
+    tx[0]=reg;
+    tx[1]=value;
+
+    return i2cport->Write(tx, 2);
+}
+
+int ParrotCamV::ov7670_read(uint8_t reg,uint8_t *value) {
+    uint8_t tx=reg;
+
+    if(i2cport->Write(&tx,1)!=1) {
+        Thread::Err("error writing register address\n");
+    }
+    if(i2cport->Read(value,1)!=1) {
+        Thread::Err("error reading register\n");
+    }
+
+	return 1;
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/ParrotCamV.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamV.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotCamV.h	(revision 3)
@@ -0,0 +1,139 @@
+/*!
+ * \file ParrotCamV.h
+ * \brief Class for ardrone2 horizontal camera
+ * \author Guillaume Sanahuja
+ * \date 2014/07/16
+ * \version 4.0
+ */
+
+#ifndef PARROTCAMV_H
+#define PARROTCAMV_H
+
+#define   CMATRIX_LEN 6
+
+#include "V4LCamera.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class I2cPort;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class ParrotCamV
+    *
+    * \brief Class for ardrone2 horizontal cameraH
+    */
+    class ParrotCamV : public V4LCamera
+    {
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a ParrotCamV.
+            *
+            * \param parent parent
+            * \param name name
+            * \param priority priority of the Thread
+            */
+            ParrotCamV(const core::FrameworkManager* parent,std::string name,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~ParrotCamV();
+
+        private:
+            /*!
+            * \brief Set Gain
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetGain(float value);
+
+            /*!
+            * \brief Set Auto Gain
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value
+            */
+            void SetAutoGain(bool value);
+
+            /*!
+            * \brief Set Exposure
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetExposure(float value);
+
+            /*!
+            * \brief Set Auto Exposure
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value
+            */
+            void SetAutoExposure(bool value);
+
+            /*!
+            * \brief Set Brightness
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetBrightness(float value);
+
+            /*!
+            * \brief Set Saturation
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetSaturation(float value);
+
+            /*!
+            * \brief Set Hue
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetHue(float value);
+
+            /*!
+            * \brief Set Contrast
+            *
+            * Reimplemented from V4LCamera class
+            *
+            * \param value value between 0 and 1
+            */
+            void SetContrast(float value);
+
+            int ov7670_write(uint8_t reg,uint8_t value);
+            int ov7670_read(uint8_t reg,uint8_t* value);
+            unsigned char ov7670_abs_to_sm(uint8_t v);
+            int ov7670_store_cmatrix(int matrix[CMATRIX_LEN]);
+            void ov7670_calc_cmatrix(int matrix[CMATRIX_LEN]);
+            float hue,sat;
+            core::I2cPort* i2cport;
+            //cmatrix for V4L2_MBUS_FMT_UYVY8_2X8
+            int cmatrix[CMATRIX_LEN]= { 128, -128, 0, -34, -94, 128 };
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // PARROTCAMV_H
Index: /trunk/lib/FlairSensorActuator/src/ParrotGps.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotGps.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotGps.cpp	(revision 3)
@@ -0,0 +1,93 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/06/04
+//  filename:   ParrotGps.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant le recepteur gps de l'ardrone2
+//
+//
+/*********************************************************************/
+
+#include "ParrotGps.h"
+#include <SerialPort.h>
+#include <FrameworkManager.h>
+#include <string.h>
+
+using std::string;
+using namespace flair::core;
+
+namespace flair
+{
+namespace sensor
+{
+
+ParrotGps::ParrotGps(const FrameworkManager* parent,string name,SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Thread(parent,name,priority), Gps(parent,name,NMEAFlags)
+{
+    this->serialport=serialport;
+
+    serialport->SetBaudrate(4800);
+    serialport->SetRxTimeout(100000000);
+}
+
+ParrotGps::~ParrotGps()
+{
+    SafeStop();
+    Join();
+}
+
+void ParrotGps::Run(void)
+{
+    char response[200] = {0};
+    int size;
+
+    Sync();
+
+    WarnUponSwitches(true);
+
+    while(!ToBeStopped())
+    {
+        SleepMS(10);
+        size=0;
+        while(!ToBeStopped())
+        {
+            ssize_t read = serialport->Read(&response[size],1);
+            if(read<0)
+            {
+                Thread::Err("erreur Read (%s)\n",strerror(-read));
+            }
+            if(response[size]==0x0a) break;
+            size++;
+        }
+        size++;
+        parseFrame(response, size);
+
+    }
+
+    WarnUponSwitches(false);
+}
+
+void ParrotGps::Sync(void)
+{
+    char data=0;
+    ssize_t read = 0;
+
+    //attente fin trame
+    while(data!=0x0a)
+    {
+        read = serialport->Read(&data,1);
+        if(read<0)
+        {
+            Thread::Err("erreur Read (%s)\n",strerror(-read));
+        }
+    }
+}
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/ParrotGps.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotGps.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotGps.h	(revision 3)
@@ -0,0 +1,81 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file ParrotGps.h
+ * \brief Class for Ardrone 2 gps receiver
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/06/04
+ * \version 4.0
+ */
+
+#ifndef PARROTGPS_H
+#define PARROTGPS_H
+
+#include <Thread.h>
+#include <Gps.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class SerialPort;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class ParrotGps
+    *
+    * \brief Class for Ardrone 2 gps receiver
+    */
+    class ParrotGps : public core::Thread, public Gps
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Novatel.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport serialport
+            * \param NMEAFlags NMEA sentances to enable
+            * \param priority priority of the Thread
+            */
+            ParrotGps(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~ParrotGps();
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            core::SerialPort *serialport;
+            void Sync(void);
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // PARROTGPS_H
Index: /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.cpp	(revision 3)
@@ -0,0 +1,235 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/12/19
+//  filename:   ParrotNavBoard.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les capteurs ardrone2
+//
+//
+/*********************************************************************/
+
+#include "ParrotNavBoard.h"
+#include <FrameworkManager.h>
+#include <cvmatrix.h>
+#include <Imu.h>
+#include <ImuData.h>
+#include <UsRangeFinder.h>
+#include <SerialPort.h>
+
+#define G 9.81
+
+using std::string;
+using namespace flair::core;
+using namespace flair::sensor;
+
+namespace {
+
+//from /data/config.ini
+const float gyros_gains[3] = { 1.0679195e-03 ,-1.0669589e-03 ,-1.0683396e-03 };
+const float acc_gains[3] = { 2.0327346e+00 ,-1.9626701e+00 ,-1.9494846e+00 };
+
+class ParrotImu : public Imu {
+public:
+    ParrotImu(const FrameworkManager* parent,string name) :Imu(parent,name) {
+        //AddDataToLog(imudata);
+    }
+
+    ~ParrotImu() {}
+
+    void SetDatas(const Vector3D &rawAcc,const Vector3D &rawMag,const Vector3D &rawGyr,Time time) {
+        ImuData* imuData;
+        GetDatas(&imuData);
+        imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
+        imuData->SetDataTime(time);
+        UpdateImu();
+        ProcessUpdate(imuData);
+    }
+
+private:
+    void UpdateFrom(const io_data *data){};
+};
+
+class ParrotUs : public UsRangeFinder {
+public:
+    ParrotUs(const FrameworkManager* parent,string name) :UsRangeFinder(parent,name) {}
+
+    ~ParrotUs() {}
+
+    void SetData(float value,Time time) {
+        output->SetValue(0,0,value);
+        output->SetDataTime(time);
+        ProcessUpdate(output);
+    }
+
+private:
+    void UpdateFrom(const io_data *data){};
+};
+
+}
+
+namespace flair { namespace sensor {
+
+ParrotNavBoard::ParrotNavBoard(const FrameworkManager* parent,string name,SerialPort* serialport,uint8_t priority) : Thread(parent,name,priority) {
+    this->serialport=serialport;
+
+    us=new ParrotUs(parent,"parrot-us");
+    imu=new ParrotImu(parent,"parrot-imu");
+
+    serialport->SetBaudrate(460800);
+}
+
+ParrotNavBoard::~ParrotNavBoard() {
+    SafeStop();
+    Join();
+
+    delete imu;
+    delete us;
+}
+
+Imu* ParrotNavBoard::GetImu(void) const {
+    return imu;
+}
+
+UsRangeFinder* ParrotNavBoard::GetUsRangeFinder(void) const {
+    return us;
+}
+
+void ParrotNavBoard::UseDefaultPlot(void) {
+    us->UseDefaultPlot();
+    imu->UseDefaultPlot();
+}
+
+void ParrotNavBoard::Run(void) {
+    uint8_t data[60];
+    nav_struct* nav=(nav_struct*)data;
+    ssize_t lu=0;
+
+    // stop acquisition
+    uint8_t cmd=0x02;
+    serialport->Write(&cmd, 1);
+
+    SleepMS(20);
+    serialport->FlushInput();
+
+    // start acquisition
+    cmd=0x01;
+    serialport->Write(&cmd, 1);
+
+    while(!ToBeStopped()) {
+        lu+=serialport->Read(data+lu,sizeof(data)-lu);
+        if(lu!=sizeof(data)) continue;
+
+        if(data[0]==0x3a) {
+            Time time=GetTime();
+
+            if(nav->checksum == Checksum(data)) {
+                computeDatas(nav,time);
+                lu=0;
+            } else {
+                Warn("Wrong checksum: got %x, expected %x, time %lld\n",nav->checksum,Checksum(data),time);
+                for(int i=0;i<lu;i++) Printf("%x ",data[i]);
+                Printf("\n");
+                int i;
+                for(i=1;i<lu;i++) {//skeep first
+                    if(data[i]==0x3a) break;
+                }
+                printf("ok en %i\n",i);
+                int k=0;
+                for(int j=i;j<lu;j++) {
+                        data[k]=data[j];
+                        k++;
+                }
+                lu=k;
+                for(int i=0;i<lu;i++) Printf("%x ",data[i]);
+                Printf("\n");
+            }
+        } else {
+            Warn("Wrong header: got %x, expected 0x3a, time %lld\n",data[0],GetTime());
+            for(int i=0;i<lu;i++) Printf("%x ",data[i]);
+            Printf("\n");
+            int i;
+            for(i=1;i<lu;i++) {//skeep first
+                if(data[i]==0x3a) break;
+            }
+            printf("ok en %i\n",i);
+            int k=0;
+            for(int j=i;j<lu;j++) {
+                    data[k]=data[j];
+                    k++;
+            }
+            lu=k;
+            for(int i=0;i<lu;i++) Printf("%x ",data[i]);
+            Printf("\n");
+        }
+    }
+}
+
+void ParrotNavBoard::computeDatas(nav_struct* nav,Time time) {
+    static float accs_offset[3]={0,0,0};
+    static float gyrs_offset[3]={0,0,0};
+    static int cpt=0;
+
+    float ax,ay,az;
+    float gx,gy,gz;
+    float mx,my,mz;
+
+    if(cpt<2000) { //init offsets
+        for(int i=0;i<3;i++) accs_offset[i]+=nav->acc[i];
+        for(int i=0;i<3;i++) gyrs_offset[i]+=nav->gyro[i];
+        cpt++;
+        ax=0;
+        ay=0;
+        az=-G;
+        gx=0;
+        gy=0;
+        gz=0;
+        mx=0;
+        my=0;
+        mz=0;
+    } else {
+        //ax=-G*(nav->acc[0]-accs_offset[0]/cpt)/512.;
+        //ay=G*(nav->acc[1]-accs_offset[1]/cpt)/512.;
+        //az=G*(-(nav->acc[2]-accs_offset[2]/cpt)/512.-1);
+        ax=acc_gains[0]*(nav->acc[0]-accs_offset[0]/cpt)/100.;//gains from parrot are in cm.s-2
+        ay=acc_gains[1]*(nav->acc[1]-accs_offset[1]/cpt)/100.;
+        az=acc_gains[2]*((nav->acc[2]-accs_offset[2]/cpt)/100.)-G;
+
+        gx=(nav->gyro[0]-gyrs_offset[0]/cpt)*gyros_gains[0];
+        gy=(nav->gyro[1]-gyrs_offset[1]/cpt)*gyros_gains[1];
+        gz=(nav->gyro[2]-gyrs_offset[2]/cpt)*gyros_gains[2];
+
+        mx=nav->mag[0];
+        my=nav->mag[1];
+        mz=nav->mag[2];
+    }
+
+    ((ParrotImu*)imu)->SetDatas(Vector3D(ax,ay,az),
+                                Vector3D(mx,my,mz),
+                                Vector3D(gx,gy,gz),
+                                time
+                                );
+
+
+     if(((nav->us_echo)&0x8000)==0x8000) {
+        float h  = (float)(((nav->us_echo)&0x7fff)) * 0.0340/100.;
+        ((ParrotUs*)us)->SetData(h,time);
+    }
+}
+uint16_t ParrotNavBoard::Checksum(const uint8_t *data) const{
+    uint16_t checksum = 0;
+    for(int i = 2; i < 60-2; i += 2) {
+        checksum += data[i] + (data[i+1] << 8);
+    }
+    return checksum;
+}
+
+} // end namespace sensor
+} // end namespace flair
+
Index: /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.h	(revision 3)
@@ -0,0 +1,122 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file ParrotNavBoard.h
+ * \brief Class for ArDrone 2 sensors
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/12/19
+ * \version 4.0
+ */
+
+//thanks to Paparazzi (https://wiki.paparazziuav.org) for the struct nav_struct
+
+#ifndef PARROTNAVBOARD_H
+#define PARROTNAVBOARD_H
+
+#include <Thread.h>
+
+namespace flair {
+    namespace core {
+        class FrameworkManager;
+        class cvmatrix;
+        class ImuData;
+        class SerialPort;
+    }
+}
+
+namespace flair { namespace sensor {
+    class Imu;
+    class UsRangeFinder;
+
+    /*! \class ParrotNavBoard
+    *
+    * \brief Class for ArDrone 2 sensors
+    */
+    class ParrotNavBoard : public core::Thread {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a ParrotNavBoard.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport serialport
+            * \param priority priority of the Thread
+            */
+            ParrotNavBoard(const core::FrameworkManager* parent,std::string name,core::SerialPort* serialport,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~ParrotNavBoard();
+
+            /*!
+            * \brief Use default plot
+            *
+            */
+            void UseDefaultPlot(void);
+
+            /*!
+            * \brief Get Imu
+            *
+            * \return Associated Imu
+            */
+            Imu* GetImu(void) const;
+
+            /*!
+            * \brief Get UsRangeFinder
+            *
+            * \return Associated UsRangeFinder
+            */
+            UsRangeFinder* GetUsRangeFinder(void) const;
+
+        private:
+            typedef struct nav_struct
+            {
+                uint16_t size;                // +0x00 Size of the following data (always 0x3a)
+                uint16_t seq;                 // +0x02 Sequence number, increases every update
+                uint16_t acc[3];              // +0x04 Raw data (10-bit) of the accelerometers multiplied by 4
+                int16_t gyro[3];             // +0x0A Raw data for the gyros, 12-bit A/D converted voltage of the gyros. X,Y=IDG, Z=Epson
+                uint16_t acc_temp;            // +0x10 Accs temperature -- startup value 120 @ 25C, rising to 143
+                uint16_t gyro_temp;           // +0x12 XYGyro temperature (IDG), 12-bit A/D converted voltage of IDG's temperature sensor -- startup value 1532 @ 25C, rising to 1572
+                uint16_t us_echo;             // +0x14 bit15=1 echo pulse transmitted, bit14-0 first echo. Value 30 = 1cm. min value: 784 = 26cm
+                uint16_t us_echo_start;       // +0x16 Array with starts of echos (8 array values @ 25Hz, 9 values @ 22.22Hz)
+                uint16_t us_echo_end;         // +0x18 Array with ends of echos   (8 array values @ 25Hz, 9 values @ 22.22Hz)
+                uint16_t us_association_echo; // +0x1a Ultrasonic parameter -- echo number starting with 0. max value 3758. examples: 0,1,2,3,4,5,6,7  ; 0,1,2,3,4,86,6,9
+                uint16_t us_distance_echo;    // +0x1c Ultrasonic parameter -- no clear pattern
+                uint16_t us_courbe_temps;     // +0x1e Ultrasonic parameter -- counts up from 0 to approx 24346 in 192 sample cycles of which 12 cylces have value 0
+                uint16_t us_courbe_valeur;    // +0x20 Ultrasonic parameter -- value between 0 and 4000, no clear pattern. 192 sample cycles of which 12 cylces have value 0
+                uint16_t us_courbe_ref;       // +0x22 Ultrasonic parameter -- coutns down from 4000 to 0 in 192 sample cycles of which 12 cylces have value 0
+                uint16_t nb_echo;
+                uint32_t sum_echo;
+                int16_t gradient;
+                uint16_t flag_echo_ini;
+                int32_t pressure;
+                int16_t temprature_pressure;
+                int16_t mag[3];
+                uint16_t checksum;
+            } __attribute__ ((packed)) nav_struct;
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            uint16_t Checksum(const uint8_t *data) const;
+            void computeDatas(nav_struct* nav,core::Time time);
+
+            Imu* imu;
+            UsRangeFinder* us;
+            core::SerialPort* serialport;
+    };
+} // end namespace sensor
+} // end namespace flair
+
+#endif // PARROTNAVBOARD_H
Index: /trunk/lib/FlairSensorActuator/src/Ps3Eye.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Ps3Eye.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Ps3Eye.cpp	(revision 3)
@@ -0,0 +1,45 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2012/01/19
+//  filename:   Ps3Eye.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant la camera Ps3Eye
+//
+//
+/*********************************************************************/
+
+#include "Ps3Eye.h"
+#include <GroupBox.h>
+#include <DoubleSpinBox.h>
+#include <CheckBox.h>
+#include <GridLayout.h>
+#include <cvimage.h>
+#include <FrameworkManager.h>
+#include <stdio.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+Ps3Eye::Ps3Eye(const FrameworkManager* parent,string name,int camera_index,uint8_t priority)
+    : V4LCamera(parent,name,camera_index,320,240,cvimage::Type::Format::YUYV,priority) {
+
+}
+
+Ps3Eye::~Ps3Eye() {
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Ps3Eye.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Ps3Eye.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Ps3Eye.h	(revision 3)
@@ -0,0 +1,68 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Ps3Eye.h
+ * \brief Class for Ps3Eye camera
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2012/01/19
+ * \version 4.0
+ */
+
+#ifndef PS3EYE_H
+#define PS3EYE_H
+
+#include "V4LCamera.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class cvimage;
+        class FrameworkManager;
+    }
+    namespace gui
+    {
+        class GridLayout;
+        class DoubleSpinBox;
+        class CheckBox;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class Ps3Eye
+    *
+    * \brief Class for Ps3Eye camera
+    */
+    class Ps3Eye : public V4LCamera
+    {
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Ps3Eye.
+            *
+            * \param parent parent
+            * \param name name
+            * \param camera_index index of the camera, ie /dev/videox
+            * \param priority priority of the Thread
+            */
+            Ps3Eye(const core::FrameworkManager* parent,std::string name,int camera_index,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Ps3Eye();
+
+        private:
+    };
+} // end namespace sensor
+} // end namespace flair
+
+#endif // PS3EYE_H
Index: /trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp	(revision 3)
@@ -0,0 +1,87 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/07/08
+//  filename:   RadioReceiver.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for radio receiver
+//
+//
+/*********************************************************************/
+
+#include "RadioReceiver.h"
+#include <Tab.h>
+#include <TabWidget.h>
+#include <FrameworkManager.h>
+#include <cvmatrix.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+RadioReceiver::RadioReceiver(const FrameworkManager* parent,string name,unsigned int nb_channels) : IODevice(parent,name)
+{
+    is_connected=false;
+    this->nb_channels=nb_channels;
+
+    //init matrix
+    cvmatrix_descriptor* desc=new cvmatrix_descriptor(nb_channels,1);
+    for(int i=0;i<nb_channels;i++)
+    {
+        ostringstream channel_name;
+        channel_name << "channel " << i;
+        desc->SetElementName(i,0,channel_name.str());
+    }
+    output=new cvmatrix(this,desc,floatType,name);
+
+     //station sol
+    main_tab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+    setup_tab=new Tab(tab,"Setup");
+
+    AddDataToLog(output);
+}
+
+RadioReceiver::~RadioReceiver()
+{
+    delete main_tab;
+}
+
+Layout* RadioReceiver::GetLayout(void) const
+{
+    return setup_tab;
+}
+
+float RadioReceiver::ChannelValue(unsigned int id) const
+{
+    if(id<nb_channels)
+    {
+        return output->Value(id,0);
+    }
+    else
+    {
+        Err("channel %i/%i is out of bound\n",id,nb_channels);
+        return -1;
+    }
+}
+
+bool RadioReceiver::IsConnected(void) const
+{
+    return is_connected;
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/RadioReceiver.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/RadioReceiver.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/RadioReceiver.h	(revision 3)
@@ -0,0 +1,104 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file RadioReceiver.h
+ * \brief Base class for radio receiver
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/07/08
+ * \version 4.0
+ */
+
+#ifndef RADIORECEIVER_H
+#define RADIORECEIVER_H
+
+#include <IODevice.h>
+#include <stdint.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class cvmatrix;
+    }
+    namespace gui
+    {
+        class Tab;
+        class TabWidget;
+        class Layout;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class RadioReceiver
+    *
+    * \brief Base class for radio receiver
+    */
+    class RadioReceiver : public core::IODevice
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a RadioReceiver.
+            *
+            * \param parent parent
+            * \param name name
+            * \param nb_channels number of supported channels
+            */
+            RadioReceiver(const core::FrameworkManager* parent,std::string name,unsigned int nb_channels);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~RadioReceiver();
+
+            /*!
+            * \brief get channel value
+            *
+            * \param id channel id
+            * \return value of the channel, between 0 and 1.
+            *  Returns -1 if channel is out of bound
+            */
+            float ChannelValue(unsigned int id) const;
+
+            /*!
+            * \brief Is transmitter connected?
+            *
+            * \return true if transmitter is connected
+            */
+            bool IsConnected(void) const;
+
+            /*!
+            * \brief Setup Layout
+            *
+            * \return a Layout available
+            */
+            gui::Layout* GetLayout(void) const;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            core::cvmatrix *output;
+            bool is_connected;
+            unsigned int nb_channels;
+            gui::Tab* main_tab;
+            gui::TabWidget* tab;
+            gui::Tab* setup_tab;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // RADIORECEIVER_H
Index: /trunk/lib/FlairSensorActuator/src/SimuBldc.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuBldc.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuBldc.cpp	(revision 3)
@@ -0,0 +1,82 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/02/07
+//  filename:   SimuBldc.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation bldc
+//
+//
+/*********************************************************************/
+#include "SimuBldc.h"
+#include <FrameworkManager.h>
+#include <GridLayout.h>
+#include <DoubleSpinBox.h>
+#include <GroupBox.h>
+#include <SharedMem.h>
+#include <cvmatrix.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace actuator
+{
+
+SimuBldc::SimuBldc(const IODevice* parent,Layout* layout,string name,uint8_t motors_count,uint32_t dev_id) :Bldc(parent,layout,name,motors_count)
+{
+    ostringstream dev_name;
+    dev_name << "simu_bldc_" << dev_id;
+    shmem=new SharedMem(this,dev_name.str().c_str(),motors_count*sizeof(float));
+
+    GroupBox *groupbox=new GroupBox(layout->NewRow(),"simubldc");
+    k=new DoubleSpinBox(groupbox->NewRow(),"k driver:",0,10000,1);
+}
+
+SimuBldc::SimuBldc(const Object* parent,string name,uint8_t motors_count,uint32_t dev_id) :Bldc(parent,name,motors_count) {
+    ostringstream dev_name;
+    dev_name << "simu_bldc_" << dev_id;
+    shmem=new SharedMem(this,dev_name.str().c_str(),motors_count*sizeof(float));
+
+    //reset values
+    float values[motors_count];
+    for(int i=0;i<motors_count;i++) values[i]=0;
+
+    shmem->Write((char*)&values,motors_count*sizeof(float));
+}
+
+SimuBldc::~SimuBldc() {
+}
+
+void SimuBldc::SetMotors(float* value) {
+    float values[MotorsCount()];
+
+    for(int i=0;i<MotorsCount();i++) values[i]=k->Value()*value[i];
+
+    shmem->Write((char*)&values,MotorsCount()*sizeof(float));
+
+    //on prend une fois pour toute le mutex et on fait des accès directs
+    output->GetMutex();
+    for(int i=0;i<MotorsCount();i++) output->SetValueNoMutex(i,0,values[i]);
+    output->ReleaseMutex();
+}
+
+void SimuBldc::GetSpeeds(float* value) const {
+    float values[MotorsCount()];
+    shmem->Read((char*)&values,MotorsCount()*sizeof(float));
+
+    for(int i=0;i<MotorsCount();i++) value[i]=values[i];
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/SimuBldc.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuBldc.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuBldc.h	(revision 3)
@@ -0,0 +1,119 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuBldc.h
+ * \brief Class for a simulation bldc
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/02/07
+ * \version 4.0
+ */
+
+#ifndef SIMUBLDC_H
+#define SIMUBLDC_H
+
+#include <Bldc.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class SharedMem;
+        class IODevice;
+        class cvmatrix;
+    }
+    namespace gui
+    {
+        class DoubleSpinBox;
+        class Layout;
+    }
+}
+
+namespace flair
+{
+namespace actuator
+{
+    /*! \class SimuBldc
+    *
+    * \brief Class for a simulation bldc
+    *
+    */
+    class SimuBldc : public Bldc
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuBldc. Control part.
+            *
+            * \param parent parent
+            * \param layout layout
+            * \param name name
+            * \param motors_count number of motors
+            * \param dev_id device id
+            */
+            SimuBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count,uint32_t dev_id);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuBldc. Simulation part.
+            *
+            * \param parent parent
+            * \param name name
+            * \param motors_count number of motors
+            * \param dev_id device id
+            */
+            SimuBldc(const core::Object* parent,std::string name,uint8_t motors_count,uint32_t dev_id);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~SimuBldc();
+
+            /*!
+            * \brief Get motors speeds.
+            *
+            * This function should only be used for the simulation part.
+            *
+            * \param value array to store motors speeds
+            */
+            void GetSpeeds(float* value) const;
+
+            /*!
+            * \brief Has speed measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has speed measurement
+            */
+            bool HasSpeedMeasurement(void) const{return false;};
+
+            /*!
+            * \brief Has current measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has current measurement
+            */
+            bool HasCurrentMeasurement(void) const{return false;};
+
+        private:
+            /*!
+            * \brief Set motors values
+            *
+            * Reimplemented from Bldc. \n
+            * Values size must be the same as MotorsCount()
+            *
+            * \param values motor values
+            */
+            void SetMotors(float* value);
+
+            core::SharedMem *shmem;
+            gui::DoubleSpinBox *k;
+    };
+} // end namespace actuator
+} // end namespace flair
+#endif // SIMUBLDC_H
Index: /trunk/lib/FlairSensorActuator/src/SimuCamera.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuCamera.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuCamera.cpp	(revision 3)
@@ -0,0 +1,85 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/03/06
+//  filename:   SimuCamera.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation camera
+//
+//
+/*********************************************************************/
+
+#include "SimuCamera.h"
+#include <FrameworkManager.h>
+#include <SpinBox.h>
+#include <GroupBox.h>
+#include <cvimage.h>
+#include <SharedMem.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace sensor {
+
+SimuCamera::SimuCamera(const FrameworkManager* parent,string name,uint16_t width,uint16_t height,uint8_t channels,uint32_t dev_id,uint8_t priority) :Thread(parent,name,priority),Camera(parent,name,width,height,cvimage::Type::Format::BGR) {
+    data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,100,1,50);
+
+    buf_size=width*height*channels;
+
+    img=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,3);
+    output->img->imageData=img->imageData;
+
+    ostringstream dev_name;
+    dev_name << "simu_cam_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),buf_size);
+}
+
+SimuCamera::SimuCamera(const IODevice* parent,string name,uint16_t width,uint16_t height,uint8_t channels,uint32_t dev_id) : Thread(parent,name,0),Camera(parent,name) {
+    data_rate=NULL;
+
+    ostringstream dev_name;
+    dev_name << "simu_cam_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),width*height*channels);
+}
+
+SimuCamera::~SimuCamera() {
+    SafeStop();
+    Join();
+}
+
+void SimuCamera::Run(void) {
+    if(data_rate==NULL) {
+        Thread::Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+
+    while(!ToBeStopped()) {
+        WaitPeriod();
+
+        if(data_rate->ValueChanged()==true) {
+            SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+        }
+
+        output->GetMutex();
+        shmem->Read((char*)img->imageData,buf_size); // remplacer copie par échange de pointeur sur double buffering
+        output->ReleaseMutex();
+
+        output->SetDataTime(GetTime());
+
+        ProcessUpdate(output);
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/SimuCamera.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuCamera.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuCamera.h	(revision 3)
@@ -0,0 +1,110 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuCamera.h
+ * \brief Class for a simulation camera
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/03/06
+ * \version 4.0
+ */
+
+#ifndef SIMUCAMERA_H
+#define SIMUCAMERA_H
+
+#include <Camera.h>
+#include <Thread.h>
+#include <cxcore.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class SharedMem;
+    }
+    namespace gui
+    {
+        class SpinBox;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class SimuCamera
+    *
+    * \brief Class for a simulation camera
+    */
+    class SimuCamera : public core::Thread, public Camera
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuCamera. Control part.
+            *
+            * \param parent parent
+            * \param name name
+            * \param width width
+            * \param height height
+            * \param channels number of channels
+            * \param dev_id device id
+            * \param priority priority of the Thread
+            */
+            SimuCamera(const core::FrameworkManager* parent,std::string name,uint16_t width,uint16_t height,uint8_t channels,uint32_t dev_id,uint8_t priority);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuCamera. Simulation part.\n
+            * The Thread of this class should not be run.
+            *
+            * \param parent parent
+            * \param name name
+            * \param width width
+            * \param height height
+            * \param channels number of channels
+            * \param dev_id device id
+            */
+            SimuCamera(const core::IODevice* parent,std::string name,uint16_t width,uint16_t height,uint8_t channels,uint32_t dev_id);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~SimuCamera();
+
+        protected:
+            /*!
+            * \brief SharedMem to access datas
+            *
+            */
+            core::SharedMem *shmem;
+
+        private:
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            gui::SpinBox *data_rate;
+            size_t buf_size;
+            IplImage* img;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // SIMUCAMERA_H
Index: /trunk/lib/FlairSensorActuator/src/SimuGps.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuGps.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuGps.cpp	(revision 3)
@@ -0,0 +1,62 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/05/26
+//  filename:   SimuGps.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation GPS
+//
+//
+/*********************************************************************/
+
+#include "SimuGps.h"
+#include <FrameworkManager.h>
+#include <string.h>
+#include <GeoCoordinate.h>
+
+using std::string;
+using namespace flair::core;
+
+namespace flair
+{
+namespace sensor
+{
+
+SimuGps::SimuGps(const FrameworkManager* parent,string name,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Thread(parent,name,priority), Gps(parent,name,NMEAFlags)
+{
+}
+
+SimuGps::~SimuGps()
+{
+    SafeStop();
+    Join();
+}
+
+void SimuGps::Run(void)
+{
+    char response[200] = {0};
+    int size,result;
+//double lat=0;
+    SetPeriodMS(500);
+    WarnUponSwitches(true);
+
+    while(!ToBeStopped())
+    {
+        WaitPeriod();
+        position->SetCoordinates(49.402313, 2.795463,0);
+  //      lat+=.5;
+    }
+
+
+    WarnUponSwitches(false);
+}
+
+
+} // end namespace sensor
+} // end namespace framewor
Index: /trunk/lib/FlairSensorActuator/src/SimuGps.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuGps.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuGps.h	(revision 3)
@@ -0,0 +1,76 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuGps.h
+ * \brief Class for a simulation GPS
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/08/23
+ * \version 4.0
+ */
+
+#ifndef SIMUGPS_H
+#define SIMUGPS_H
+
+#include <Thread.h>
+#include <Gps.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class SimuGps
+    *
+    * \brief Class for a simulation GPS
+    */
+    class SimuGps : public core::Thread, public Gps
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Novatel.
+            *
+            * \param parent parent
+            * \param name name
+            * \param NMEAFlags NMEA sentances to enable
+            * \param priority priority of the Thread
+            */
+            SimuGps(const core::FrameworkManager* parent,std::string name,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~SimuGps();
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+    };
+} // end namespace sensor
+} // end namespace framewor
+#endif // SIMUGPS_H
Index: /trunk/lib/FlairSensorActuator/src/SimuImu.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 3)
@@ -0,0 +1,108 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/02/07
+//  filename:   SimuImu.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation imu
+//
+//
+/*********************************************************************/
+
+#include "SimuImu.h"
+#include <FrameworkManager.h>
+#include <ImuData.h>
+#include <SpinBox.h>
+#include <GroupBox.h>
+#include <cvmatrix.h>
+#include <SharedMem.h>
+#include <AhrsData.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair { namespace sensor {
+
+SimuImu::SimuImu(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) :Imu(parent,name),Thread(parent,name,priority) {
+    data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,200);
+    ahrsData=new AhrsData((Imu*)this);
+
+    ostringstream dev_name;
+    dev_name << "simu_imu_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(imu_states_t));
+}
+
+SimuImu::SimuImu(const IODevice* parent,string name,uint32_t dev_id) :Imu(parent,name),Thread(parent,name,0) {
+    data_rate=NULL;
+
+    ostringstream dev_name;
+    dev_name << "simu_imu_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(imu_states_t));
+}
+
+SimuImu::~SimuImu() {
+    SafeStop();
+    Join();
+}
+
+void SimuImu::UpdateFrom(const io_data *data) {
+    if(data!=NULL) {
+        cvmatrix *input=(cvmatrix*)data;
+        imu_states_t state;
+
+        input->GetMutex();
+        state.q0=input->ValueNoMutex(0,0);
+        state.q1=input->ValueNoMutex(1,0);
+        state.q2=input->ValueNoMutex(2,0);
+        state.q3=input->ValueNoMutex(3,0);
+        state.wx=input->ValueNoMutex(7,0);
+        state.wy=input->ValueNoMutex(8,0);
+        state.wz=input->ValueNoMutex(9,0);
+        input->ReleaseMutex();
+
+        shmem->Write((char*)&state,sizeof(imu_states_t));
+    }
+}
+
+void SimuImu::Run(void) {
+    imu_states_t state;
+    ImuData* imuData;
+    GetDatas(&imuData);
+
+    if(data_rate==NULL) {
+        Thread::Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+
+    while(!ToBeStopped()) {
+        WaitPeriod();
+
+        if(data_rate->ValueChanged()==true) {
+            SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+        }
+
+        shmem->Read((char*)&state,sizeof(imu_states_t));
+        ahrsData->SetQuaternionAndAngularRates(Quaternion(state.q0,state.q1,state.q2,state.q3)
+                                               ,Vector3D(state.wx,state.wy,state.wz));
+
+        imuData->SetDataTime(GetTime());
+        ahrsData->SetDataTime(GetTime());
+
+        UpdateImu();
+        ProcessUpdate(ahrsData);
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/SimuImu.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuImu.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuImu.h	(revision 3)
@@ -0,0 +1,99 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuImu.h
+ * \brief Class for a simulation Imu
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/02/07
+ * \version 4.0
+ */
+
+#ifndef SIMUIMU_H
+#define SIMUIMU_H
+
+#include <Imu.h>
+#include <Thread.h>
+
+namespace flair {
+    namespace core     {
+        class SharedMem;
+        class AhrsData;
+    }
+    namespace gui {
+        class SpinBox;
+    }
+}
+
+namespace flair { namespace sensor {
+    /*! \class SimuImu
+    *
+    * \brief Class for a simulation Imu
+    */
+    class SimuImu : public Imu, public core::Thread {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuImu. Control part.
+            *
+            * \param parent parent
+            * \param name name
+            * \param dev_id device id
+            * \param priority priority of the Thread
+            */
+            SimuImu(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuImu. Simulation part.\n
+            * The Thread of this class should not be run.
+            *
+            * \param parent parent
+            * \param name name
+            * \param dev_id device id
+            */
+            SimuImu(const core::IODevice* parent,std::string name,uint32_t dev_id);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~SimuImu();
+
+        private:
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data);
+
+            typedef struct {
+                float q0;
+                float q1;
+                float q2;
+                float q3;
+                float wx;
+                float wy;
+                float wz;
+            } imu_states_t;
+            gui::SpinBox *data_rate;
+            core::SharedMem *shmem;
+            core::AhrsData *ahrsData;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // SIMUIMU_H
Index: /trunk/lib/FlairSensorActuator/src/SimuLaser.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuLaser.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuLaser.cpp	(revision 3)
@@ -0,0 +1,92 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/02/07
+//  filename:   SimuUs.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation us
+//
+//
+/*********************************************************************/
+#include "SimuLaser.h"
+#include <FrameworkManager.h>
+#include <ImuData.h>
+#include <SpinBox.h>
+#include <GroupBox.h>
+#include <cvmatrix.h>
+#include <SharedMem.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+SimuLaser::SimuLaser(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) : Thread(parent,name,priority), LaserRangeFinder(parent,name)
+{
+    data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,50);
+
+    ostringstream dev_name;
+    dev_name << "simu_Laser_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),360*sizeof(float)); //****
+}
+
+SimuLaser::SimuLaser(const IODevice* parent,string name,uint32_t dev_id) : Thread(parent,name,0), LaserRangeFinder(parent,name)
+{
+    data_rate=NULL;
+
+    ostringstream dev_name;
+    dev_name << "simu_Laser_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),360*sizeof(float));
+}
+
+SimuLaser::~SimuLaser()
+{
+    SafeStop();
+    Join();
+}
+
+void SimuLaser::Run(void)
+{
+    float z[360];
+
+    if(data_rate==NULL)
+    {
+        Thread::Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+
+    while(!ToBeStopped())
+    {
+        WaitPeriod();
+
+        shmem->Read((char*)z,360*sizeof(float));
+
+        if(data_rate->ValueChanged()==true)
+        {
+            SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+        }
+        for(int i=0; i<360; i++)
+        {
+            output->SetValue(i,0,z[i]); //*******
+        }
+        output->SetDataTime(GetTime());
+        ProcessUpdate(output);
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/SimuLaser.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuLaser.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuLaser.h	(revision 3)
@@ -0,0 +1,101 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuUs.h
+ * \brief Class for a simulation UsRangeFinder
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/02/07
+ * \version 4.0
+ */
+
+#ifndef SIMULASER_H
+#define SIMULASER_H
+
+#include <LaserRangeFinder.h>
+#include <Thread.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class SharedMem;
+    }
+    namespace gui
+    {
+        class SpinBox;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class SimuUs
+    *
+    * \brief Class for a simulation UsRangeFinder
+    */
+    class SimuLaser : public core::Thread, public LaserRangeFinder
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuUs. Control part.
+            *
+            * \param parent parent
+            * \param name name
+            * \param dev_id device id
+            * \param priority priority of the Thread
+            */
+            SimuLaser(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuUs. Simulation part.\n
+            * The Thread of this class should not be run.
+            *
+            * \param parent parent
+            * \param name name
+            * \param dev_id device id
+            */
+            SimuLaser(const core::IODevice* parent,std::string name,uint32_t dev_id);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~SimuLaser();
+
+        protected:
+            /*!
+            * \brief SharedMem to access datas
+            *
+            */
+            core::SharedMem *shmem;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            gui::SpinBox *data_rate;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // SIMULASER_H
Index: /trunk/lib/FlairSensorActuator/src/SimuUs.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuUs.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuUs.cpp	(revision 3)
@@ -0,0 +1,90 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/02/07
+//  filename:   SimuUs.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation us
+//
+//
+/*********************************************************************/
+#include "SimuUs.h"
+#include <FrameworkManager.h>
+#include <ImuData.h>
+#include <SpinBox.h>
+#include <GroupBox.h>
+#include <cvmatrix.h>
+#include <SharedMem.h>
+#include <sstream>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+SimuUs::SimuUs(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) : Thread(parent,name,priority), UsRangeFinder(parent,name)
+{
+    data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,50);
+
+    ostringstream dev_name;
+    dev_name << "simu_us_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(float));
+}
+
+SimuUs::SimuUs(const IODevice* parent,string name,uint32_t dev_id) : Thread(parent,name,0), UsRangeFinder(parent,name)
+{
+    data_rate=NULL;
+
+    ostringstream dev_name;
+    dev_name << "simu_us_" << dev_id;
+    shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(float));
+}
+
+SimuUs::~SimuUs()
+{
+    SafeStop();
+    Join();
+}
+
+void SimuUs::Run(void)
+{
+    float z;
+
+    if(data_rate==NULL)
+    {
+        Thread::Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+
+    while(!ToBeStopped())
+    {
+        WaitPeriod();
+
+        shmem->Read((char*)&z,sizeof(float));
+
+        if(data_rate->ValueChanged()==true)
+        {
+            SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
+        }
+
+        output->SetValue(0,0,z);
+        output->SetDataTime(GetTime());
+        ProcessUpdate(output);
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/SimuUs.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/SimuUs.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/SimuUs.h	(revision 3)
@@ -0,0 +1,101 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuUs.h
+ * \brief Class for a simulation UsRangeFinder
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/02/07
+ * \version 4.0
+ */
+
+#ifndef SIMUUS_H
+#define SIMUUS_H
+
+#include <UsRangeFinder.h>
+#include <Thread.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class SharedMem;
+    }
+    namespace gui
+    {
+        class SpinBox;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class SimuUs
+    *
+    * \brief Class for a simulation UsRangeFinder
+    */
+    class SimuUs : public core::Thread, public UsRangeFinder
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuUs. Control part.
+            *
+            * \param parent parent
+            * \param name name
+            * \param dev_id device id
+            * \param priority priority of the Thread
+            */
+            SimuUs(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuUs. Simulation part.\n
+            * The Thread of this class should not be run.
+            *
+            * \param parent parent
+            * \param name name
+            * \param dev_id device id
+            */
+            SimuUs(const core::IODevice* parent,std::string name,uint32_t dev_id);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~SimuUs();
+
+        protected:
+            /*!
+            * \brief SharedMem to access datas
+            *
+            */
+            core::SharedMem *shmem;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            gui::SpinBox *data_rate;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // SIMUUS_H
Index: /trunk/lib/FlairSensorActuator/src/Srf08.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Srf08.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Srf08.cpp	(revision 3)
@@ -0,0 +1,246 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2011/05/01
+//  filename:   Srf08.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for ultra sonic SRF08
+//
+//
+/*********************************************************************/
+
+#include "Srf08.h"
+#include <I2cPort.h>
+#include <FrameworkManager.h>
+#include <GroupBox.h>
+#include <SpinBox.h>
+#include <cvmatrix.h>
+#include <string.h>
+#include <errno.h>
+#include <math.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+Srf08::Srf08(const FrameworkManager* parent,string name,I2cPort* i2cport,uint16_t address,uint8_t priority) : Thread(parent,name,priority), UsRangeFinder(parent,name)
+{
+    is_init=false;
+
+    //default values
+    //range 46*43+43=2021mm max (2m)
+    //7:gain=118
+    //range=46;
+    //gain=7;
+
+    this->i2cport=i2cport;
+	this->address=address;
+
+    //station sol
+    gain=new SpinBox(GetGroupBox()->NewRow(),"gain:",0,255,1,8);
+    range=new SpinBox(GetGroupBox()->LastRowLastCol(),"range:",0,255,1,46);
+
+
+    SetRange();
+    SetMaxGain();
+}
+
+Srf08::~Srf08()
+{
+    SafeStop();
+    Join();
+}
+
+void Srf08::Run(void)
+{
+    WarnUponSwitches(true);
+
+    //init srf
+    SendEcho();
+
+    SetPeriodMS(20);
+
+    while(!ToBeStopped())
+    {
+        WaitPeriod();
+
+        if(range->ValueChanged()==true) SetRange();
+        if(gain->ValueChanged()==true) SetMaxGain();
+
+        GetEcho();
+        SendEcho();
+    }
+
+    WarnUponSwitches(false);
+}
+
+void Srf08::SendEcho(void)
+{
+    ssize_t written;
+    uint8_t tx[2];
+
+    tx[0]=0;//command register
+    tx[1]=82;//ranging mode in usec
+
+    i2cport->GetMutex();
+
+    i2cport->SetSlave(address);
+    written = i2cport->Write(tx, 2);
+    echo_time=GetTime();
+
+    i2cport->ReleaseMutex();
+
+    if(written<0)
+    {
+        Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
+    }
+    else if (written != 2)
+    {
+        Thread::Err("erreur rt_dev_write %i/2\n",written);
+    }
+
+}
+
+void Srf08::GetEcho(void)
+{
+    float z=0;
+    ssize_t written,read;
+    uint8_t tx,rx[2];
+    tx=2;
+    uint8_t nb_err=0;
+
+    //si l'us est bloqué, on attend 1ms de plus
+    while(1)
+    {
+        i2cport->GetMutex();
+        i2cport->SetSlave(address);
+        written = i2cport->Write(&tx, 1);
+
+        if(written<0)
+        {
+            i2cport->ReleaseMutex();
+            Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
+            nb_err++;
+            if(nb_err==20)
+            {
+                Thread::Err("erreur rt_dev_write (%s), too many errors\n",strerror(-written));
+                return;
+            }
+            SleepMS(1);
+        }
+        else
+        {
+            read = i2cport->Read(rx, 2);
+            i2cport->ReleaseMutex();
+        //rt_printf("%i %i\n",rx[0],rx[1]);
+            if(read<0)
+            {
+                if(read!=-ETIMEDOUT) Thread::Err("erreur rt_dev_read (%s) %i\n",strerror(-written),read);
+                nb_err++;
+                if(nb_err==20)
+                {
+                    Thread::Err("erreur rt_dev_read (%s), too many errors\n",strerror(-written));
+                    return;
+                }
+                SleepMS(1);
+            }
+            else if (read != 2)
+            {
+                Thread::Err("erreur rt_dev_read %i/2\n",read);
+                return;
+            }
+            else if(read==2)
+            {
+                break;
+            }
+        }
+    }
+
+    //if(z!=-1)
+    //revoir ce filtrage!!!
+    {
+        z=0.000001*(float)(rx[0]*256+rx[1])*344/2;//d=v*t; v=344m/s, t=t usec/2 (aller retour)
+//if(z>1) rt_printf("%i %i %f\n",rx[0],rx[1],z);
+        //on ne permet pas 2 mesures consecutives + grandes de 10cm
+        if(fabs(z-z_1)>0.5 && is_init==true)
+        {
+            Printf("z %f (anc %f) %lld\n",z,z_1,echo_time);
+            Printf("a revoir on suppose le sol plan\n");
+            z=z_1+(z_1-z_2);//on suppose que l'on continue a la meme vitesse
+        }
+        output->SetValue(0,0,z);
+        output->SetDataTime(echo_time+(rx[0]*256+rx[1])*1000);
+        ProcessUpdate(output);
+        z_2=z_1;
+        z_1=z;
+        is_init=true;
+    }
+}
+
+
+void Srf08::SetRange(void)
+{
+    ssize_t written;
+    uint8_t tx[2];
+
+    tx[0]=2;//range register
+    tx[1]=range->Value();//range*43+43=dist max en mm
+
+    i2cport->GetMutex();
+
+    i2cport->SetSlave(address);
+    written = i2cport->Write(tx, 2);
+
+    i2cport->ReleaseMutex();
+
+    if(written<0)
+    {
+        Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
+    }
+    else if (written != 2)
+    {
+        Thread::Err("erreur rt_dev_write %i/2\n",written);
+    }
+
+}
+
+void Srf08::SetMaxGain(void)
+{
+    ssize_t written;
+    uint8_t tx[2];
+
+    //rt_printf("Srf08::SetMaxGain: %s ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
+
+    tx[0]=1;//max gain register
+    tx[1]=gain->Value();
+
+    i2cport->GetMutex();
+
+    i2cport->SetSlave(address);
+    written = i2cport->Write(tx, 2);
+
+    i2cport->ReleaseMutex();
+
+    if(written<0)
+    {
+        Thread::Err("erreur write (%s)\n",strerror(-written));
+    }
+    else if (written != 2)
+    {
+        Thread::Err("erreur write %i/2\n",written);
+    }
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/Srf08.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/Srf08.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/Srf08.h	(revision 3)
@@ -0,0 +1,107 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Srf08.h
+ * \brief Class for ultra sonic SRF08
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2011/05/01
+ * \version 4.0
+ */
+
+#ifndef SRF08_H
+#define SRF08_H
+
+#include <Thread.h>
+#include <UsRangeFinder.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class I2cPort;
+    }
+    namespace gui
+    {
+        class SpinBox;;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class Srf08
+    *
+    * \brief Class for ultra sonic SRF08
+    */
+    class Srf08 : public core::Thread, public UsRangeFinder
+    {
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a SimuUs. Control part.
+            *
+            * \param parent parent
+            * \param name name
+            * \param i2cport i2c port
+            * \param address i2c slave address
+            * \param priority priority of the Thread
+            */
+            Srf08(const core::FrameworkManager* parent,std::string name,core::I2cPort* i2cport,uint16_t address,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~Srf08();
+
+            /*!
+            * \brief Set Range
+            *
+            * check datasheet for values
+            */
+            void SetRange(void);
+
+            /*!
+            * \brief Set Max Gain
+            *
+            * check datasheet for values
+            */
+            void SetMaxGain(void);
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            void SendEcho(void);
+            void GetEcho(void);
+
+            bool is_init;
+            core::Time echo_time;
+            float z_1,z_2;
+            gui::SpinBox *gain,*range;
+            uint16_t address;
+            core::I2cPort* i2cport;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // SRF08_H
Index: /trunk/lib/FlairSensorActuator/src/TargetController.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 3)
@@ -0,0 +1,154 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/03/30
+//  filename:   TargetController.cpp
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for target side remote controls
+//
+//
+/*********************************************************************/
+#include "TargetController.h"
+#include "Controller.h"
+#include <TabWidget.h>
+#include <Tab.h>
+#include <FrameworkManager.h>
+#include <cvmatrix.h>
+
+#include <cstring>
+#include <string>
+
+#include <unistd.h> //for usleep
+
+using namespace flair::core;
+using namespace flair::gui;
+using std::string;
+
+namespace flair
+{
+namespace sensor
+{
+
+TargetController::TargetController(const FrameworkManager* parent,string name,uint8_t priority) :
+    Thread(parent,name,priority),IODevice(parent,name) {
+    main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
+    TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
+    setup_tab=new Tab(tab,"Reglages");
+
+}
+
+TargetController::~TargetController() {
+    SafeStop();
+    Join();
+}
+
+std::string TargetController::GetAxisName(unsigned int axisId) const {
+    return string("axis")+std::to_string(axisId);
+}
+
+std::string TargetController::GetButtonName(unsigned int buttonId) const {
+    return string("button")+std::to_string(buttonId);
+}
+
+bool TargetController::SetLedOn(unsigned int ledId) {
+    if (!IsControllerActionSupported(ControllerAction::SetLedOn)) return false;
+    SwitchLedMessage *msgLed=new SwitchLedMessage(true,ledId);
+    changeStateQueue.push(msgLed);
+    return true;
+}
+
+bool TargetController::SetLedOff(unsigned int ledId) {
+    if (!IsControllerActionSupported(ControllerAction::SetLedOn)) return false;
+    SwitchLedMessage *msgLed=new SwitchLedMessage(false,ledId);
+    changeStateQueue.push(msgLed);
+    return true;
+}
+
+bool TargetController::Rumble(unsigned int leftForce,unsigned int leftTimeout,unsigned int rightForce,unsigned int rightTimeout) {
+    if (!IsControllerActionSupported(ControllerAction::Rumble)) return false;
+    RumbleMessage *msgRumble=new RumbleMessage(leftForce,leftTimeout,rightForce,rightTimeout);
+    changeStateQueue.push(msgRumble);
+    return true;
+}
+
+bool TargetController::FlashLed(unsigned int ledId,unsigned int onTimeout,unsigned int offTimeout){
+    if (!IsControllerActionSupported(ControllerAction::FlashLed)) return false;
+    FlashLedMessage *msgFlashLed=new FlashLedMessage(ledId,onTimeout,offTimeout);
+    changeStateQueue.push(msgFlashLed);
+    return true;
+}
+
+float TargetController::GetAxisValue(unsigned int axisId) const{
+    //TODO we'd better throw an exception here
+    if (axis==NULL) return 0;
+
+    axis->GetMutex();
+    float axisValue=axis->Value(axisId, 0);
+    axis->ReleaseMutex();
+    return axisValue;
+}
+
+bool TargetController::IsButtonPressed(unsigned int buttonId) const{
+    //TODO we'd better throw an exception here
+    if (button==NULL) return false;
+
+    button->GetMutex();
+    bool buttonValue=button->Value(buttonId, 0);
+    button->ReleaseMutex();
+    return buttonValue;
+}
+
+void TargetController::Run() {
+    Message *message;
+
+    if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
+        SafeStop();
+        Thread::Err("An error occured, we don't proceed with the loop.\n");
+    } else {
+
+        axis=new cvmatrix((IODevice *)this,axisNumber,1,floatType);
+        button=new cvmatrix((IODevice *)this,buttonNumber,1,SignedIntegerType(8));
+
+        while(!ToBeStopped()) {
+            //Thread::Info("Debug: entering acquisition loop\n");
+            if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
+
+            if (IsDataFrameReady()) {
+                //Thread::Info("Debug: data frame is ready\n");
+
+                AcquireAxisData(*axis);
+                AcquireButtonData(*button);
+
+                //send the data
+                axis->SetDataTime(GetTime());
+                ProcessUpdate(axis);
+
+                //send pending controller state change request(s)
+
+                while(changeStateQueue.size()!=0) {
+                    message=changeStateQueue.front();
+                    if (ProcessMessage(message)) {
+                        changeStateQueue.pop();
+                        delete message;
+                    }
+                }
+            } else {
+                //Thread::Info("Debug: relax...\n");
+                usleep(20000); //20ms
+            }
+        }
+    }
+}
+
+Tab* TargetController::GetTab() const {
+    return setup_tab;
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/TargetController.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/TargetController.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/TargetController.h	(revision 3)
@@ -0,0 +1,96 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/04/14
+//  filename:   TargetController.h
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for target side remote controls
+//
+//
+/*********************************************************************/
+
+#ifndef TARGETCONTROLLER_H
+#define TARGETCONTROLLER_H
+
+#include <IODevice.h>
+#include <Thread.h>
+#include <stdint.h>
+#include <queue>
+
+namespace flair {
+    namespace core {
+        class FrameworkManager;
+        class cvmatrix;
+        class Socket;
+        class io_data;
+    }
+    namespace gui {
+        class Tab;
+        class TabWidget;
+        class DataPlot1D;
+    }
+}
+
+namespace flair { namespace sensor {
+    enum class ControllerAction;
+
+    /*! \class TargetController
+    *
+    * \brief Base Class for target side remote controls
+    *
+    */
+    class TargetController : public core::Thread, public core::IODevice {
+    public:
+        TargetController(const core::FrameworkManager* parent,std::string name,uint8_t priority=0);
+        ~TargetController();
+        //void DrawUserInterface();
+        virtual bool IsConnected() const=0;
+        virtual bool IsDataFrameReady()=0;
+        //axis stuff
+        unsigned int GetAxisNumber() const;
+        virtual std::string GetAxisName(unsigned int axisId) const;
+        float GetAxisValue(unsigned int axisId) const;// always in the range [-1.0,1.0]
+        //button stuff
+        unsigned int GetButtonNumber() const;
+        bool IsButtonPressed(unsigned int buttonId) const;
+        virtual std::string GetButtonName(unsigned int axisId) const;
+        //controller state stuff
+        virtual bool IsControllerActionSupported(ControllerAction action) const {return false;};
+        bool SetLedOn(unsigned int ledId);
+        bool SetLedOff(unsigned int ledId);
+        bool Rumble(unsigned int left_force,unsigned int left_timeout,unsigned int right_force,unsigned int right_timeout);
+        bool FlashLed(unsigned int ledId,unsigned int on_timeout,unsigned int off_timeout);
+        void UpdateFrom(const core::io_data *data){}; //TODO
+        gui::Tab* GetTab(void) const;
+
+    protected:
+        virtual bool ProcessMessage(core::Message *msg)=0;
+        void QueueMessage(core::Message msg);
+        virtual bool ControllerInitialization()=0;// {return true;};
+        //axis stuff
+        unsigned int axisNumber;
+        core::cvmatrix* axis=NULL;
+        virtual void AcquireAxisData(core::cvmatrix &axis)=0; //responsible for getting the axis data from the hardware
+        uint16_t bitsPerAxis;
+        //button stuff
+        unsigned int buttonNumber;
+        core::cvmatrix* button=NULL;
+        virtual void AcquireButtonData(core::cvmatrix &button)=0; //responsible for getting the button data from the hardware
+        //controller state stuff
+        unsigned int ledNumber;
+    private:
+        void Run();
+        std::queue<core::Message *> changeStateQueue;
+        flair::gui::Tab* main_tab;
+        flair::gui::Tab* setup_tab;
+    };
+
+}}
+
+#endif // TARGETCONTROLLER_H
Index: /trunk/lib/FlairSensorActuator/src/TargetEthController.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/TargetEthController.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/TargetEthController.cpp	(revision 3)
@@ -0,0 +1,333 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/03/30
+//  filename:   TargetEthController.cpp
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    class that gets remote controls through an ethernet connection.
+//              Typical use case: a remote control is plugged in a workstation and sends remote control
+//              data to a distant target (this class) through Wifi//
+//
+/*********************************************************************/
+#include "TargetEthController.h"
+#include <Controller.h>
+#include <FrameworkManager.h>
+#include <TcpSocket.h>
+#include <Socket.h>
+#include <cstring>
+#include <string>
+#include <cvmatrix.h>
+#include <stdexcept>
+
+#include <unistd.h> // for sleep (debug)
+
+using namespace flair::core;
+using namespace flair::gui;
+using std::string;
+
+namespace flair
+{
+namespace sensor
+{
+
+TargetEthController::TargetEthController(const FrameworkManager* parent,string name,uint16_t _port,uint8_t priority) :
+    TargetController(parent,name,priority),listeningPort(_port),receiveCurrentPosition(0) {
+    const bool blocking=true;
+    listeningSocket=new TcpSocket(parent,"TEC_listening_socket",blocking,blocking);
+    dataSocket=new Socket(parent,"TEC_data_socket",_port+1); //receiving side
+}
+
+TargetEthController::~TargetEthController() {
+    //We are (currently) the server side. We must ask the client side to initiate tcp connexion closing to avoid the server socket
+    //to get stuck in TIME_WAIT state
+    Message msg(32);
+    if (controlSocket) {
+        Message cancelAcquisition(sizeof(ControllerAction));
+        ControllerAction exit=ControllerAction::Exit;
+        memcpy(cancelAcquisition.buffer,&exit,sizeof(ControllerAction));
+        controlSocket->SendMessage(cancelAcquisition.buffer,cancelAcquisition.bufferSize);
+        //We don't expect any more data from the client, we're just waiting for the socket to be closed by the client
+        controlSocket->RecvMessage(msg.buffer,msg.bufferSize);
+    }
+
+    //TargetController calls TargetEthController methods in its run
+    //we must stop the thread now
+    SafeStop();
+    Join();
+}
+
+bool TargetEthController::IsConnected() const {
+    //TODO
+}
+
+bool TargetEthController::IsDataFrameReady(){
+    //read up to the last data
+    ssize_t received;
+    size_t bytesToReceive=dataFrameSize-receiveCurrentPosition;
+    bool fullDatagramReceived=false;
+
+    do {
+        received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
+        if (received>0) {
+            bytesToReceive-=received;
+            if (bytesToReceive==0) {
+                //a full datagram has been read in receiveFrameBuffer
+                fullDatagramReceived=true;
+                receiveCurrentPosition=0;
+                //we swap the data and reception buffers to avoid copy
+                char *swapFrameBuffer=dataFrameBuffer;
+                dataFrameBuffer=receiveFrameBuffer;
+                receiveFrameBuffer=swapFrameBuffer;
+            }
+        }
+    } while (!received<0);
+
+    return fullDatagramReceived;
+}
+
+uint8_t TargetEthController::getByteOrNull(char *buffer,int byte,size_t bufferSize) {
+    if (byte<bufferSize) return buffer[byte];
+    else return 0;
+}
+
+uint32_t TargetEthController::charBufferToUint32(char *buffer, size_t bufferSize) {
+    union {
+        uint32_t int32;
+        char byte[4];
+    } bitField;
+    if (!IsBigEndian()) {
+        bitField.byte[0]=getByteOrNull(buffer,3,bufferSize);
+        bitField.byte[1]=getByteOrNull(buffer,2,bufferSize);
+        bitField.byte[2]=getByteOrNull(buffer,1,bufferSize);
+        bitField.byte[3]=getByteOrNull(buffer,0,bufferSize);
+    } else {
+        bitField.byte[0]=getByteOrNull(buffer,0,bufferSize);
+        bitField.byte[1]=getByteOrNull(buffer,1,bufferSize);
+        bitField.byte[2]=getByteOrNull(buffer,2,bufferSize);
+        bitField.byte[3]=getByteOrNull(buffer,3,bufferSize);
+    }
+    return bitField.int32;
+}
+
+//read _up_to_16_bits_ in a buffer
+uint16_t TargetEthController::readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize) {
+    //parameters check
+    if (valueSizeInBits>16) throw std::range_error("bitfield should be at max 16bits wide");
+    size_t minBufferSize=(offsetInBits+valueSizeInBits)/8;
+    if ((offsetInBits+valueSizeInBits)%8!=0) minBufferSize++;
+    if (bufferSize<minBufferSize) throw std::range_error("buffer too small");
+    //skip first bytes
+    size_t bytesToSkip=offsetInBits/8;
+    buffer+=bytesToSkip;
+    bufferSize-=bytesToSkip;
+    offsetInBits-=bytesToSkip*8;
+    //take care of endianness
+    uint32_t value=charBufferToUint32(buffer,bufferSize);
+    value>>=32-offsetInBits-valueSizeInBits;
+    value&=(1<<valueSizeInBits)-1;
+    return (uint16_t)value;
+}
+
+void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
+    axis.GetMutex();
+//char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
+    for (unsigned int i=0;i<axisNumber;i++) {
+        uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,dataFrameBuffer,dataFrameSize);
+//        uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
+        uint16_t scale=1<<(bitsPerAxis-1);
+//Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
+        axis.SetValueNoMutex(i,0,(rawAxisValue-scale)/(float)scale);
+    }
+    axis.ReleaseMutex();
+}
+
+void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
+    uint8_t buttonValue;
+    int currentButton=0;
+    button.GetMutex();
+    /*
+    for (unsigned int i=0;i<buttonNumber;i++) {
+        memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
+        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
+        button.SetValueNoMutex(i,0,buttonValue);
+//        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n", GetButtonName(i).c_str());
+    }*/
+    for (unsigned int i=0;i<buttonNumber/8;i++) {
+        memcpy(&buttonValue,dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),sizeof(uint8_t));
+//        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
+        for(unsigned int j=0;j<8;j++) {
+            button.SetValueNoMutex(currentButton,0,(buttonValue>>j)&0x01);
+            currentButton++;
+        }
+    }
+    button.ReleaseMutex();
+}
+
+string TargetEthController::GetAxisName(unsigned int axisId) const{
+    //TODO: should throw an exception if axisName==NULL or axisId>axisNumber
+    return axisName[axisId];
+}
+
+string TargetEthController::GetButtonName(unsigned int buttonId) const{
+    //TODO: should throw an exception if buttonName==NULL or buttonId>buttonNumber
+    return buttonName[buttonId];
+}
+
+bool TargetEthController::ProcessMessage(Message *msg) {
+    return !(controlSocket->SendMessage(msg->buffer,msg->bufferSize,0)<0);
+}
+
+bool TargetEthController::IsControllerActionSupported(ControllerAction action) const {
+    //TODO: here we should ask the remote side (host). Probably through the control socket
+    switch(action) {
+    case ControllerAction::SetLedOn:
+        return true;
+    case ControllerAction::SetLedOff:
+        return true;
+    case ControllerAction::Rumble:
+        return true;
+    case ControllerAction::FlashLed:
+        return true;
+    default:
+        return false;
+    }
+}
+
+bool TargetEthController::ControllerInitialization() {
+    char message[1024];
+    ssize_t received;
+    bool connected=false;
+    bool mustReadMore;
+
+    listeningSocket->Listen(listeningPort);
+    Thread::Info("Debug: Listening to port %d\n",listeningPort);
+                 //accept incoming connection
+    bool connectionAccepted=false;
+    while (!connectionAccepted) {
+        controlSocket=listeningSocket->Accept(10);
+        if (controlSocket==nullptr) {
+            //Timeout (or error btw)
+            if (ToBeStopped()) return false;
+        } else connectionAccepted=true;
+    }
+    Thread::Info("Debug: Connexion accepted\n");
+
+    //get axis stuff
+    bool axisNumberRead=false;
+    while (!axisNumberRead) {
+        try {
+            axisNumber=controlSocket->ReadUInt32(10);
+            //Thread::Info("Debug: axisNumber %d\n", axisNumber);
+            axisNumberRead=true;
+        }
+        catch (std::runtime_error e) {
+            //timeout
+            if (ToBeStopped()) return false;
+        }
+    }
+    bool bitsPerAxisRead=false;
+    while (!bitsPerAxisRead) {
+        try {
+            bitsPerAxis=controlSocket->ReadUInt32(10);
+            //Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
+            bitsPerAxisRead=true;
+        }
+        catch (std::runtime_error e) {
+            //timeout
+            if (ToBeStopped()) return false;
+        }
+    }
+    axisName=new string[axisNumber];
+    for (unsigned int i=0;i<axisNumber;i++) {
+        //read string size
+        int stringSize;
+        bool stringSizeRead=false;
+        while (!stringSizeRead) {
+            try {
+                stringSize=controlSocket->ReadUInt32(10);
+                stringSizeRead=true;
+            }
+            catch (std::runtime_error e) {
+                //timeout
+                if (ToBeStopped()) return false;
+            }
+        }
+        //read string
+        bool axisNameRead=false;
+        while (!axisNameRead) {
+            try {
+                axisName[i]=controlSocket->ReadString(stringSize,10);
+                axisNameRead=true;
+            }
+            catch (std::runtime_error e) {
+                //timeout
+                if (ToBeStopped()) return false;
+            }
+        }
+        //Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
+    }
+
+    //get button stuff
+    bool buttonNumberRead=false;
+    while (!buttonNumberRead) {
+        try {
+            buttonNumber=controlSocket->ReadUInt32(10);
+            buttonNumberRead=true;
+        }
+        catch (std::runtime_error e) {
+            //timeout
+            if (ToBeStopped()) return false;
+        }
+    }
+    //Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
+    buttonName=new string[buttonNumber];
+    for (unsigned int i=0;i<buttonNumber;i++) {
+        //read string size
+        int stringSize;
+        bool stringSizeRead=false;
+        while (!stringSizeRead) {
+            try {
+                stringSize=controlSocket->ReadUInt32(10);
+                stringSizeRead=true;
+            }
+            catch (std::runtime_error e) {
+                //timeout
+                if (ToBeStopped()) return false;
+            }
+        }
+        //read string
+        bool buttonNameRead=false;
+        while (!buttonNameRead) {
+            try {
+                buttonName[i]=controlSocket->ReadString(stringSize,10);
+                buttonNameRead=true;
+            }
+            catch (std::runtime_error e) {
+                //timeout
+                if (ToBeStopped()) return false;
+            }
+        }
+        //Thread::Info("Debug: buttonName for button %d %s\n", i, buttonName[i].c_str());
+    }
+
+//    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
+    buttonOffset=(axisNumber*bitsPerAxis)/8;
+    if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
+    dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
+    if ((buttonNumber%8)!=0) dataFrameSize++;
+    dataFrameBuffer=new char[dataFrameSize];
+    receiveFrameBuffer=new char[dataFrameSize];
+
+    Thread::Info("Controller connected with host side\n");
+    if(buttonNumber%8!=0) Thread::Err("Button number is not multiple of 8\n");
+    return true;
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/TargetEthController.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/TargetEthController.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/TargetEthController.h	(revision 3)
@@ -0,0 +1,85 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/04/14
+//  filename:   TargetEthController.h
+//
+//  author:     Gildas Bayard
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    class that gets remote controls through an ethernet connection.
+//              Typical use case: a remote control is plugged in a workstation and sends remote control
+//              data to a distant target (this class) through Wifi
+//
+//
+/*********************************************************************/
+
+#ifndef TARGETETHCONTROLLER_H
+#define TARGETETHCONTROLLER_H
+
+#include "TargetController.h"
+
+namespace flair {
+    namespace core {
+        class FrameworkManager;
+        class cvmatrix;
+        class TcpSocket;
+        class Socket;
+    }
+    namespace gui {
+        class Tab;
+        class TabWidget;
+        class DataPlot1D;
+    }
+}
+
+namespace flair { namespace sensor {
+    /*! \class TargetController
+    *
+    * \brief Base Class for target side remote controls
+    *
+    */
+    class TargetEthController : public TargetController {
+    public:
+        TargetEthController(const core::FrameworkManager* parent,std::string name,uint16_t port,uint8_t priority=0);
+        ~TargetEthController();
+        //void DrawUserInterface();
+    protected:
+        bool IsConnected() const;
+        //axis stuff
+        std::string GetAxisName(unsigned int axisId) const;
+        //button stuff
+        std::string GetButtonName(unsigned int axisId) const;
+        //controller state stuff
+        bool ProcessMessage(core::Message *msg);
+        bool IsControllerActionSupported(ControllerAction action) const;
+
+        bool IsDataFrameReady();
+        void AcquireAxisData(core::cvmatrix &axis); //responsible for getting the axis data from the hardware
+        void AcquireButtonData(core::cvmatrix &button); //responsible for getting the button data from the hardware
+
+        bool ControllerInitialization();
+
+    private:
+        uint16_t readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize);
+        uint8_t getByteOrNull(char *buffer,int byte,size_t bufferSize);
+        uint32_t charBufferToUint32(char *buffer, size_t bufferSize);
+        core::TcpSocket *listeningSocket;
+        int listeningPort;
+        core::TcpSocket *controlSocket=NULL;
+        core::Socket *dataSocket;
+        std::string *axisName=NULL;
+        std::string *buttonName=NULL;
+        size_t dataFrameSize;
+        char *dataFrameBuffer;
+        char *receiveFrameBuffer;
+        size_t receiveCurrentPosition;
+        uint8_t buttonOffset;
+    };
+
+}}
+
+#endif // TARGETCONTROLLER_H
Index: /trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp	(revision 3)
@@ -0,0 +1,117 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/01/22
+//  filename:   UsRangeFinder.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Base class for UsRangeFinder
+//
+//
+/*********************************************************************/
+
+#include "UsRangeFinder.h"
+#include <FrameworkManager.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <GroupBox.h>
+#include <Layout.h>
+#include <DataPlot1D.h>
+#include <cvmatrix.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+UsRangeFinder::UsRangeFinder(const FrameworkManager* parent,string name) : IODevice(parent,name)
+{
+    plot_tab=NULL;
+
+    cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
+    desc->SetElementName(0,0,name);
+    output=new cvmatrix(this,desc,floatType);
+    AddDataToLog(output);
+
+    //station sol
+    main_tab=new Tab(parent->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+	sensor_tab=new Tab(tab,"Reglages");
+	setup_groupbox=new GroupBox(sensor_tab->NewRow(),name);
+}
+
+UsRangeFinder::UsRangeFinder(const IODevice* parent,std::string name) : IODevice(parent,name)
+{
+    plot_tab=NULL;
+    main_tab=NULL;
+    tab=NULL;
+	sensor_tab=NULL;
+	setup_groupbox=NULL;
+
+    output=NULL;
+}
+
+UsRangeFinder::~UsRangeFinder()
+{
+    if(main_tab!=NULL) delete main_tab;
+}
+
+GroupBox* UsRangeFinder::GetGroupBox(void) const
+{
+    return setup_groupbox;
+}
+
+Layout* UsRangeFinder::GetLayout(void) const
+{
+    return sensor_tab;
+}
+
+DataPlot1D* UsRangeFinder::GetPlot(void) const
+{
+    return plot;
+}
+
+Tab* UsRangeFinder::GetPlotTab(void) const
+{
+    return plot_tab;
+}
+
+void UsRangeFinder::UseDefaultPlot(void)
+{
+    if(tab==NULL)
+    {
+        Err("not applicable for simulation part.\n");
+        return;
+    }
+
+    plot_tab=new Tab(tab,"Mesures");
+        plot=new DataPlot1D(plot_tab->NewRow(),ObjectName(),0,2);
+            plot->AddCurve(output->Element(0));
+}
+
+void UsRangeFinder::LockUserInterface(void) const
+{
+    setup_groupbox->setEnabled(false);
+}
+
+void UsRangeFinder::UnlockUserInterface(void) const
+{
+    setup_groupbox->setEnabled(true);
+}
+
+float UsRangeFinder::Value(void) const
+{
+    return output->Value(0,0);
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/UsRangeFinder.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/UsRangeFinder.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/UsRangeFinder.h	(revision 3)
@@ -0,0 +1,155 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file UsRangeFinder.h
+ * \brief Base class for UsRangeFinder
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/01/22
+ * \version 4.0
+ */
+
+#ifndef USRANGEFINDER_H
+#define USRANGEFINDER_H
+
+#include <IODevice.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class cvmatrix;
+    }
+    namespace gui
+    {
+        class Tab;
+        class TabWidget;
+        class GroupBox;
+        class Layout;
+        class DataPlot1D;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class UsRangeFinder
+    *
+    * \brief Base class for UsRangeFinder
+    *
+    * Use this class to define a custom UsRangeFinder.
+    *
+    */
+    class UsRangeFinder : public core::IODevice
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a UsRangeFinder. Control part.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            UsRangeFinder(const core::FrameworkManager* parent,std::string name);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a UsRangeFinder. Simulation part.
+            *
+            * \param parent parent
+            * \param name name
+            */
+            UsRangeFinder(const core::IODevice* parent,std::string name);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~UsRangeFinder();
+
+            /*!
+            * \brief Lock user interface
+            *
+            */
+            void LockUserInterface(void) const;
+
+            /*!
+            * \brief Unlock user interface
+            *
+            */
+            void UnlockUserInterface(void) const;
+
+            /*!
+            * \brief Use default plot
+            *
+            * Should no be used for the simulation part.
+            */
+            void UseDefaultPlot(void);
+
+            /*!
+            * \brief Plot
+            *
+            * \return DataPlot1D
+            */
+            gui::DataPlot1D* GetPlot(void) const;
+
+            /*!
+            * \brief Setup Layout
+            *
+            * \return a Layout available
+            */
+            gui::Layout* GetLayout(void) const;
+
+            /*!
+            * \brief Plot tab
+            *
+            * \return plot Tab
+            */
+            gui::Tab* GetPlotTab(void) const;
+
+            /*!
+            * \brief Value
+            *
+            * \return output value
+            */
+            float Value(void) const;
+
+        protected:
+            /*!
+            * \brief Output matrix
+            *
+            * \return output matrix
+            */
+            core::cvmatrix *output;
+
+            /*!
+            * \brief Setup GroupBox
+            *
+            * \return a GroupBox available
+            */
+            gui::GroupBox* GetGroupBox(void) const;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            gui::Tab* main_tab;
+            gui::TabWidget *tab;
+            gui::GroupBox* setup_groupbox;
+            gui::Tab* sensor_tab;
+            gui::DataPlot1D* plot;
+            gui::Tab* plot_tab;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // USRANGEFINDER_H
Index: /trunk/lib/FlairSensorActuator/src/V4LCamera.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 3)
@@ -0,0 +1,203 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2014/07/17
+//  filename:   V4LCamera.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    base class for V4l camera
+//
+//
+/*********************************************************************/
+
+#include "V4LCamera.h"
+#include <GroupBox.h>
+#include <DoubleSpinBox.h>
+#include <CheckBox.h>
+#include <GridLayout.h>
+#include <Label.h>
+#include <cvimage.h>
+#include <FrameworkManager.h>
+#include <linux/videodev2.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+V4LCamera::V4LCamera(const FrameworkManager* parent,string name,uint8_t camera_index,uint16_t width,uint16_t height,cvimage::Type::Format format,uint8_t priority) : Thread(parent,name,priority), Camera(parent,name,width,height,format)
+{
+    capture = cvCaptureFromCAM(camera_index);//a mettre apres l'init dsp
+
+    if(capture<0) Thread::Err("cvCaptureFromCAM error\n");
+
+    if(cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,width)<0) Thread::Err("cvSetCaptureProperty error\n");
+    if(cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,height)<0) Thread::Err("cvSetCaptureProperty error\n");
+
+    if(format==cvimage::Type::Format::UYVY)
+    {
+        if(cvSetCaptureProperty(capture,CV_CAP_PROP_FORMAT,V4L2_PIX_FMT_UYVY)<0) Thread::Err("cvSetCaptureProperty error\n");
+    }
+    else if(format==cvimage::Type::Format::YUYV)
+    {
+        if(cvSetCaptureProperty(capture,CV_CAP_PROP_FORMAT,V4L2_PIX_FMT_YUYV)<0) Thread::Err("cvSetCaptureProperty error\n");
+    }
+    else
+    {
+        Thread::Err("format not supported\n");
+    }
+
+    //station sol
+    gain=new DoubleSpinBox(GetGroupBox()->NewRow(),"gain:",0,1,0.1);
+    exposure=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"exposure:",0,1,0.1);
+    bright=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"bright:",0,1,0.1);
+    contrast=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"contrast:",0,1,0.1);
+    hue=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"hue:",0,1,0.1);
+    sharpness=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"sharpness:",0,1,0.1);
+    sat=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"saturation:",0,1,0.1);
+    autogain=new CheckBox(GetGroupBox()->NewRow(),"autogain:");
+    autoexposure=new CheckBox(GetGroupBox()->LastRowLastCol(),"autoexposure:");
+    awb=new CheckBox(GetGroupBox()->LastRowLastCol(),"awb:");
+    fps=new Label(GetGroupBox()->NewRow(),"fps");
+}
+
+V4LCamera::~V4LCamera()
+{
+    SafeStop();
+    Join();
+}
+
+void V4LCamera::Run(void)
+{
+    Time cam_time,new_time,fpsNow,fpsPrev;
+    IplImage* img;//raw image
+    int fpsCounter=0;
+
+    //init image old
+    if(!cvGrabFrame(capture))
+    {
+        Printf("Could not grab a frame\n");
+    }
+    cam_time=GetTime();
+    fpsPrev = cam_time;
+
+    while(!ToBeStopped())
+    {
+        //fps counter
+        fpsCounter++;
+        if(fpsCounter==100) {
+            fpsNow = GetTime();
+            fps->SetText("fps: %.1f",fpsCounter/((float)(fpsNow - fpsPrev)/1000000000.));
+            fpsCounter=0;
+            fpsPrev=fpsNow;
+        }
+
+        //cam properties
+        if(gain->ValueChanged()==true && autogain->Value()==false) SetGain(gain->Value());
+        if(exposure->ValueChanged()==true && autoexposure->Value()==false) SetExposure(exposure->Value());
+        if(bright->ValueChanged()==true) SetBrightness(bright->Value());
+        if(sat->ValueChanged()==true) SetSaturation(sat->Value());
+        if(contrast->ValueChanged()==true) SetContrast(contrast->Value());
+        if(hue->ValueChanged()==true) SetHue(hue->Value());
+        if(sharpness->ValueChanged()==true) cvSetCaptureProperty(capture,CV_CAP_PROP_SHARPNESS,sharpness->Value());
+        if(autogain->ValueChanged()==true)
+        {
+            if(autogain->Value()==true)
+            {
+                gain->setEnabled(false);
+            }
+            else
+            {
+                gain->setEnabled(true);
+                SetGain(gain->Value());
+            }
+            SetAutoGain(autogain->Value());
+        }
+        if(autoexposure->ValueChanged()==true)
+        {
+            if(autoexposure->Value()==true)
+            {
+                exposure->setEnabled(false);
+            }
+            else
+            {
+                exposure->setEnabled(true);
+                SetExposure(exposure->Value());
+            }
+            SetAutoExposure(autoexposure->Value());
+        }
+        if(awb->ValueChanged()==true) cvSetCaptureProperty(capture,CV_CAP_PROP_AWB,awb->Value());
+
+
+        //cam pictures
+        img=cvRetrieveRawFrame(capture);
+        if(!cvGrabFrame(capture))
+        {
+            Printf("Could not grab a frame\n");
+        }
+        new_time=GetTime();
+
+        output->GetMutex();
+        output->img->imageData=img->imageData;
+        output->ReleaseMutex();
+
+        output->SetDataTime(cam_time);
+        ProcessUpdate(output);
+
+        cam_time=new_time;
+    }
+
+    cvReleaseCapture(&capture);
+}
+
+void V4LCamera::SetAutoGain(bool value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_AUTOGAIN,value);
+}
+
+void V4LCamera::SetAutoExposure(bool value)
+{
+    Thread::Warn("not implemented in opencv\n");
+}
+
+void V4LCamera::SetGain(float value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_GAIN,value);
+}
+
+void V4LCamera::SetExposure(float value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_EXPOSURE,value);
+}
+
+void V4LCamera::SetBrightness(float value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_BRIGHTNESS,value);
+}
+
+void V4LCamera::SetSaturation(float value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_SATURATION,value);
+}
+
+void V4LCamera::SetHue(float value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_HUE,value);
+}
+
+void V4LCamera::SetContrast(float value)
+{
+    cvSetCaptureProperty(capture,CV_CAP_PROP_CONTRAST,value);
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/V4LCamera.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/V4LCamera.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/V4LCamera.h	(revision 3)
@@ -0,0 +1,152 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file V4LCamera.h
+ * \brief Base class for V4l camera
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2014/07/17
+ * \version 4.0
+ */
+
+#ifndef V4LCamera_H
+#define V4LCamera_H
+
+#include <Thread.h>
+#include "Camera.h"
+#include <highgui.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class cvimage;
+        class FrameworkManager;
+    }
+    namespace gui
+    {
+        class GridLayout;
+        class DoubleSpinBox;
+        class CheckBox;
+        class Label;
+    }
+}
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class V4LCamera
+    *
+    * \brief Base class for V4l camera
+    */
+    class V4LCamera : public core::Thread, public Camera
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a Camera.
+            *
+            * \param parent parent
+            * \param name name
+            * \param camera_index camera index
+            * \param width width
+            * \param height height
+            * \param format image format
+            * \param priority priority of the Thread
+            */
+            V4LCamera(const core::FrameworkManager* parent,std::string name,uint8_t camera_index,uint16_t width,uint16_t height,core::cvimage::Type::Format format,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~V4LCamera();
+
+        protected:
+            /*!
+            * \brief Set Gain
+            *
+            * \param value value between 0 and 1
+            */
+            virtual void SetGain(float value);
+
+            /*!
+            * \brief Set Auto Gain
+            *
+            * \param value value
+            */
+            virtual void SetAutoGain(bool value);
+
+            /*!
+            * \brief Set Exposure
+            *
+            * \param value value between 0 and 1
+            */
+            virtual void SetExposure(float value);
+
+            /*!
+            * \brief Set Auto Exposure
+            *
+            * \param value value
+            */
+            virtual void SetAutoExposure(bool value);
+
+            /*!
+            * \brief Set Brightness
+            *
+            * \param value value between 0 and 1
+            */
+            virtual void SetBrightness(float value);
+
+            /*!
+            * \brief Set Saturation
+            *
+            * \param value value between 0 and 1
+            */
+            virtual void SetSaturation(float value);
+
+            /*!
+            * \brief Set Hue
+            *
+            * \param value value between 0 and 1
+            */
+            virtual void SetHue(float value);
+
+            /*!
+            * \brief Set Contrast
+            *
+            * \param value value between 0 and 1
+            */
+            virtual void SetContrast(float value);
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            CvCapture* capture;
+
+            gui::Tab* sensor_tab;
+            gui::DoubleSpinBox *bright,*exposure,*gain,*contrast,*hue,*sharpness,*sat;
+            gui::CheckBox *autogain,*awb,*autoexposure;
+            gui::Label *fps;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // V4LCamera_H
Index: /trunk/lib/FlairSensorActuator/src/VrpnClient.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/VrpnClient.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/VrpnClient.cpp	(revision 3)
@@ -0,0 +1,72 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/03
+//  filename:   VrpnClient.cpp
+//
+//  author:     César Richard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class to connect to a Vrpn server
+//
+/*********************************************************************/
+
+#include "VrpnClient.h"
+#include "VrpnClient_impl.h"
+#include <FrameworkManager.h>
+#include <TabWidget.h>
+#include <Layout.h>
+#include <string.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+VrpnClient::VrpnClient(const FrameworkManager* parent,string name,string address,uint16_t us_period,uint8_t priority): Thread(parent,name,priority)
+{
+    pimpl_=new VrpnClient_impl(this,name,address,us_period);
+}
+
+VrpnClient::VrpnClient(const FrameworkManager* parent,string name,SerialPort* serialport,uint16_t us_period,uint8_t priority): Thread(parent,name,priority)
+{
+    pimpl_=new VrpnClient_impl(this,name,serialport,us_period);
+}
+
+VrpnClient::~VrpnClient()
+{
+    SafeStop();
+    Join();
+
+    delete pimpl_;
+}
+
+Layout* VrpnClient::GetLayout(void) const
+{
+    return (Layout*)(pimpl_->setup_tab);
+}
+
+TabWidget* VrpnClient::GetTabWidget(void) const
+{
+    return pimpl_->tab;
+}
+
+bool VrpnClient::UseXbee(void) const
+{
+    return pimpl_->UseXbee();
+}
+
+void VrpnClient::Run(void)
+{
+    pimpl_->Run();
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/VrpnClient.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/VrpnClient.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/VrpnClient.h	(revision 3)
@@ -0,0 +1,114 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file VrpnClient.h
+ * \brief Class to connect to a Vrpn server
+ * \author César Richard, Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/04/03
+ * \version 4.0
+ */
+
+#ifndef VRPNCLIENT_H
+#define VRPNCLIENT_H
+
+#include <Thread.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class FrameworkManager;
+        class SerialPort;
+    }
+    namespace gui
+    {
+        class TabWidget;
+        class Layout;
+    }
+}
+
+class VrpnClient_impl;
+class VrpnObject_impl;
+
+namespace flair
+{
+namespace sensor
+{
+    /*! \class VrpnClient
+    *
+    * \brief Class to connect to a Vrpn server
+    */
+    class VrpnClient:public core::Thread
+    {
+        friend class ::VrpnObject_impl;
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a VrpnClient. Connection is done by IP.
+            *
+            * \param parent parent
+            * \param name name
+            * \param address server address
+            * \param us_period Thread period in us
+            * \param priority priority of the Thread
+            */
+            VrpnClient(const core::FrameworkManager* parent,std::string name,std::string address,uint16_t us_period,uint8_t priority);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a VrpnClient. Connection is done by XBee modem.
+            *
+            * \param parent parent
+            * \param name name
+            * \param serialport SerialPort for XBee modem
+            * \param us_period Xbee RX timeout in us
+            * \param priority priority of the Thread
+            */
+            VrpnClient(const core::FrameworkManager* parent,std::string name,core::SerialPort* serialport,uint16_t us_period,uint8_t priority);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~VrpnClient();
+
+            /*!
+            * \brief Setup Layout
+            *
+            * \return a Layout available
+            */
+            gui::Layout* GetLayout(void) const;
+
+            /*!
+            * \brief Setup Tab
+            *
+            * \return a Tab available
+            */
+            gui::TabWidget* GetTabWidget(void) const;
+
+            /*!
+            * \brief Is XBee used?
+            *
+            * \return true if connection is based on XBee modem
+            */
+            bool UseXbee(void) const;
+
+        private:
+            /*!
+            * \brief Run function
+            *
+            * Reimplemented from Thread.
+            *
+            */
+            void Run(void);
+
+            class VrpnClient_impl* pimpl_;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // VRPNCLIENT_H
Index: /trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp	(revision 3)
@@ -0,0 +1,246 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/03
+//  filename:   VrpnClient_impl.cpp
+//
+//  author:     César Richard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet se connectant au serveur vrpn
+//
+//
+/*********************************************************************/
+
+#include "VrpnClient.h"
+#include "VrpnClient_impl.h"
+#include "VrpnObject.h"
+#include "VrpnObject_impl.h"
+#include <SerialPort.h>
+#include <vrpn_Connection.h>
+#include <FrameworkManager.h>
+#include <TabWidget.h>
+#include <Tab.h>
+#include <GridLayout.h>
+#include <OneAxisRotation.h>
+#include <Vector3D.h>
+#include <Quaternion.h>
+#include <Mutex.h>
+#include <string.h>
+#include <unistd.h>
+#include <limits.h>
+#include <math.h>
+
+using std::string;
+using std::vector;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+
+VrpnClient_impl::VrpnClient_impl(VrpnClient* self,std::string name,std::string address,uint16_t us_period)
+{
+    this->us_period=us_period;
+    this->self=self;
+    serialport=NULL;
+
+    mutex=new Mutex(self,name);
+
+    connection = vrpn_get_connection_by_name(address.c_str());
+
+    //station sol
+    main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+    setup_tab=new Tab(tab,"Reglages");
+
+    rotation_1=new OneAxisRotation(setup_tab->NewRow(),"post rotation 1");
+    rotation_2=new OneAxisRotation(setup_tab->NewRow(),"post rotation 2");
+}
+
+VrpnClient_impl::VrpnClient_impl(VrpnClient* self,std::string name,SerialPort* serialport,uint16_t us_period)
+{
+    this->us_period=us_period;
+    this->self=self;
+    this->serialport=serialport;
+    connection=NULL;
+    mutex=new Mutex(self,name);
+
+    serialport->SetBaudrate(111111);
+    serialport->SetRxTimeout(us_period*1000);
+
+    //station sol
+    main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
+    tab=new TabWidget(main_tab->NewRow(),name);
+    setup_tab=new Tab(tab,"Reglages");
+
+    rotation_1=new OneAxisRotation(setup_tab->NewRow(),"post rotation 1");
+    rotation_2=new OneAxisRotation(setup_tab->NewRow(),"post rotation 2");
+}
+
+VrpnClient_impl::~VrpnClient_impl()
+{
+    if(!UseXbee())
+    {
+        //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
+        vector<VrpnObject*> trackables_copy=trackables;
+        for(unsigned int i=0;i<trackables_copy.size();i++) delete trackables_copy.at(i);
+        //trackables.clear();
+    }
+    else
+    {
+        //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
+        vector<xbee_object> xbee_objects_copy=xbee_objects;
+        for(unsigned int i=0;i<xbee_objects_copy.size();i++) delete xbee_objects_copy.at(i).vrpnobject->self;
+    }
+
+    delete main_tab;
+
+    if(!UseXbee())
+    {
+        //it will automatically delete connection
+        connection->removeReference();
+    }
+}
+
+void VrpnClient_impl::ComputeRotations(Vector3D& point)
+{
+    rotation_1->ComputeRotation(point);
+    rotation_2->ComputeRotation(point);
+}
+
+void VrpnClient_impl::ComputeRotations(Quaternion& quat)
+{
+    rotation_1->ComputeRotation(quat);
+    rotation_2->ComputeRotation(quat);
+}
+
+void VrpnClient_impl::AddTrackable(VrpnObject* obj)
+{
+    mutex->GetMutex();
+    trackables.push_back(obj);
+    mutex->ReleaseMutex();
+}
+
+void VrpnClient_impl::RemoveTrackable(VrpnObject* obj)
+{
+    mutex->GetMutex();
+    for(vector<VrpnObject*>::iterator it=trackables.begin() ; it < trackables.end(); it++ )
+    {
+        if(*it==obj)
+        {
+            trackables.erase (it);
+            break;
+        }
+    }
+    mutex->ReleaseMutex();
+}
+
+void VrpnClient_impl::AddTrackable(VrpnObject_impl* obj,uint8_t id)
+{
+    xbee_object tmp;
+    tmp.vrpnobject=obj;
+    tmp.id=id;
+    mutex->GetMutex();
+    xbee_objects.push_back(tmp);
+    mutex->ReleaseMutex();
+}
+
+void VrpnClient_impl::RemoveTrackable(VrpnObject_impl* obj)
+{
+    mutex->GetMutex();
+    for(vector<xbee_object>::iterator it=xbee_objects.begin() ; it < xbee_objects.end(); it++ )
+    {
+        if((*it).vrpnobject==obj)
+        {
+            xbee_objects.erase (it);
+            break;
+        }
+    }
+    mutex->ReleaseMutex();
+}
+
+bool VrpnClient_impl::UseXbee(void)
+{
+    if(connection==NULL)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void VrpnClient_impl::Run(void)
+{
+    while(!self->ToBeStopped())
+    {
+        if(UseXbee())
+        {
+            ssize_t read = 0;
+            uint8_t response[38] = {0};
+
+            read = serialport->Read(response,sizeof(response));
+            if(read>0 && read!=sizeof(response)) read += serialport->Read(&response[read],sizeof(response)-read);
+            //int temps=(float)self->GetTime()/(1000*1000);
+//self->Printf("%i %i %i\n",temps-last,temps,last);
+//last=temps;
+            if(read<0)
+            {
+                //self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
+            }
+            else if (read != sizeof(response))
+            {
+                self->Warn("erreur rt_dev_read %i/%i\n",read,sizeof(response));
+            }
+            else
+            {
+                //for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
+                //printf("\n");
+                uint8_t checksum=0;
+                for(ssize_t i=3;i<read;i++) checksum+=response[i];
+                if(checksum!=255)
+                {
+                    self->Err("checksum error\n");
+                }
+                else
+                {
+                    vrpn_TRACKERCB t;
+                    float pos[3];
+                    float quat[4];
+                    uint8_t id=response[8];
+
+                    mutex->GetMutex();
+                    if(id<xbee_objects.size())
+                    {
+                        memcpy(pos,&response[9],sizeof(pos));
+                        memcpy(quat,&response[9]+sizeof(pos),sizeof(quat));
+                        for(int i=0;i<3;i++) t.pos[i]=pos[i];
+                        for(int i=0;i<4;i++) t.quat[i]=quat[i];
+                        if(fabs(pos[0]>10) || fabs(pos[1]>10) || fabs(pos[2]>10))
+                        {
+                            printf("prob pos %f %f %f\n",pos[0],pos[1],pos[2]);
+                        }
+                        else
+                        {
+                            //self->Printf("%i %f %f %f %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
+                            VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject,t);
+                        }
+                    }
+                    mutex->ReleaseMutex();
+                }
+            }
+        }
+        else
+        {
+            connection->mainloop();
+            mutex->GetMutex();
+            for(unsigned int i=0;i<trackables.size();i++) trackables.at(i)->mainloop();
+            mutex->ReleaseMutex();
+
+            self->SleepUS(us_period);
+        }
+    }
+}
Index: /trunk/lib/FlairSensorActuator/src/VrpnObject.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/VrpnObject.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/VrpnObject.cpp	(revision 3)
@@ -0,0 +1,112 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/03
+//  filename:   VrpnObject.cpp
+//
+//  author:     César Richard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for VRPN object
+//
+//
+/*********************************************************************/
+
+#include "VrpnObject.h"
+#include "VrpnObject_impl.h"
+#include "VrpnClient.h"
+#include <string.h>
+
+#include <cvmatrix.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace sensor
+{
+
+VrpnObject::VrpnObject(const VrpnClient *parent,string name,const TabWidget* tab): IODevice(parent,name)
+{
+    pimpl_=new VrpnObject_impl(this,parent,name,-1,tab);
+    AddDataToLog(pimpl_->output);
+}
+
+VrpnObject::VrpnObject(const VrpnClient *parent,string name,uint8_t id,const TabWidget* tab): IODevice(parent,name)
+{
+    Warn("Creation of object %s with id %i\n",name.c_str(),id);
+    pimpl_=new VrpnObject_impl(this,parent,name,id,tab);
+    AddDataToLog(pimpl_->output);
+}
+
+VrpnObject::~VrpnObject(void)
+{
+	delete pimpl_;
+}
+
+cvmatrix *VrpnObject::Output(void) const
+{
+    return pimpl_->output;
+}
+
+cvmatrix *VrpnObject::State(void) const
+{
+    return pimpl_->state;
+}
+
+Tab* VrpnObject::GetPlotTab(void) const
+{
+    return pimpl_->plot_tab;
+}
+
+DataPlot1D* VrpnObject::xPlot(void) const
+{
+    return pimpl_->x_plot;
+}
+
+DataPlot1D* VrpnObject::yPlot(void) const
+{
+    return pimpl_->y_plot;
+}
+
+DataPlot1D* VrpnObject::zPlot(void) const
+{
+    return pimpl_->z_plot;
+}
+
+Time VrpnObject::GetLastPacketTime(void) const
+{
+	return pimpl_->output->DataTime();
+}
+
+bool VrpnObject::IsTracked(unsigned int timeout_ms) const
+{
+    return pimpl_->IsTracked(timeout_ms);
+}
+
+void VrpnObject::GetEuler(Euler &euler) const
+{
+    pimpl_->GetEuler(euler);
+}
+
+void VrpnObject::GetQuaternion(Quaternion &quaternion) const {
+    pimpl_->GetQuaternion(quaternion);
+}
+
+void VrpnObject::GetPosition(Vector3D &point) const
+{
+    pimpl_->GetPosition(point);
+}
+
+void VrpnObject::mainloop(void)
+{
+    pimpl_->mainloop();
+}
+
+} // end namespace sensor
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/VrpnObject.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/VrpnObject.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/VrpnObject.h	(revision 3)
@@ -0,0 +1,189 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file VrpnObject.h
+ * \brief Class for VRPN object
+ * \author César Richard, Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/04/03
+ * \version 4.0
+ */
+
+#ifndef VRPNOBJECT_H
+#define VRPNOBJECT_H
+
+#include <IODevice.h>
+#include <stdint.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class cvmatrix;
+        class Vector3D;
+        class Euler;
+        class Quaternion;
+    }
+    namespace gui
+    {
+        class TabWidget;
+        class Tab;
+        class DataPlot1D;
+    }
+}
+
+class VrpnObject_impl;
+class VrpnClient_impl;
+
+namespace flair
+{
+namespace sensor
+{
+    class VrpnClient;
+
+    /*! \class VrpnObject
+    *
+    * \brief Class for VRPN object
+    */
+    class VrpnObject: public core::IODevice
+    {
+        friend class ::VrpnObject_impl;
+        friend class ::VrpnClient_impl;
+
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a VrpnObject. Connection is done by IP.
+            *
+            * \param parent parent
+            * \param name VRPN object name, should be the same as defined in the server
+            * \param tab Tab for the user interface
+            */
+            VrpnObject(const VrpnClient *parent,std::string name,const gui::TabWidget* tab);
+
+            /*!
+            * \brief Constructor
+            *
+            * Construct a VrpnObject. Connection is done by IP.
+            *
+            * \param parent parent
+            * \param name name
+            * \param id VRPN object id, should be the same as defined in the server
+            * \param tab Tab for the user interface
+            */
+            VrpnObject(const VrpnClient *parent,std::string name,uint8_t id,const gui::TabWidget* tab);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~VrpnObject(void);
+
+            /*!
+            * \brief Plot tab
+            *
+            * \return plot Tab
+            */
+            gui::Tab* GetPlotTab(void) const;
+
+            /*!
+            * \brief Get Last Packet Time
+            *
+            * \return Time of last received packe
+            */
+            core::Time GetLastPacketTime(void) const;
+
+            /*!
+            * \brief Is object tracked?
+            *
+            * \param timeout_ms timeout
+            * \return true if object is tracked during timeout_ms time
+            */
+            bool IsTracked(unsigned int timeout_ms) const;
+
+            /*!
+            * \brief Get Euler angles
+            *
+            * \param euler output datas
+            */
+            void GetEuler(core::Euler &euler) const;
+
+            /*!
+            * \brief Get Quaternion
+            *
+            * \param quaternion output datas
+            */
+            void GetQuaternion(core::Quaternion &quaternion) const;
+            /*!
+            * \brief Get Position
+            *
+            * \param point output datas
+            */
+            void GetPosition(core::Vector3D &point) const;
+
+            /*!
+            * \brief Output matrix
+            *
+            * Matrix is of type float and as follows: \n
+            * (0,0) roll (rad) \n
+            * (1,0) pitch (rad) \n
+            * (2,0) yaw (rad) \n
+            * (3,0) x \n
+            * (4,0) y \n
+            * (5,0) z \n
+            *
+            * \return Output metrix
+            */
+            core::cvmatrix *Output(void) const;
+
+            /*!
+            * \brief State matrix
+            *
+            * Matrix is of type float and as follows: \n
+            * (0,0) roll (deg) \n
+            * (1,0) pitch (deg) \n
+            * (2,0) yaw (deg) \n
+            *
+            * \return State metrix
+            */
+            core::cvmatrix *State(void) const;
+
+            /*!
+            * \brief x plot
+            *
+            * \return x plot
+            */
+            gui::DataPlot1D* xPlot(void) const;
+
+            /*!
+            * \brief y plot
+            *
+            * \return y plot
+            */
+            gui::DataPlot1D* yPlot(void) const;
+
+            /*!
+            * \brief z plot
+            *
+            * \return z plot
+            */
+            gui::DataPlot1D* zPlot(void) const;
+
+        private:
+            /*!
+            * \brief Update using provided datas
+            *
+            * Reimplemented from IODevice.
+            *
+            * \param data data from the parent to process
+            */
+            void UpdateFrom(const core::io_data *data){};
+
+            void mainloop(void);
+            class VrpnObject_impl* pimpl_;
+    };
+} // end namespace sensor
+} // end namespace flair
+#endif // VRPNOBJECT_H
Index: /trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp	(revision 3)
@@ -0,0 +1,214 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/03
+//  filename:   VrpnObject.cpp
+//
+//  author:     César Richard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet vrpn
+//
+//
+/*********************************************************************/
+#include "VrpnObject_impl.h"
+#include "VrpnObject.h"
+#include "VrpnClient.h"
+#include "VrpnClient_impl.h"
+#include <vrpn_Tracker.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
+#include <vrpn_Connection.h>
+#include <cvmatrix.h>
+#include <Tab.h>
+#include <TabWidget.h>
+#include <DataPlot1D.h>
+#include <OneAxisRotation.h>
+#include <Vector3D.h>
+#include <Euler.h>
+#include <math.h>
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::sensor;
+
+VrpnObject_impl::VrpnObject_impl(VrpnObject* self,const VrpnClient *parent,string name, int id,const TabWidget* tab)
+{
+    this->parent=parent;
+    this->self=self;
+
+    if(id==-1 && parent->UseXbee())
+    {
+        self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
+    }
+    if(id!=-1 && !parent->UseXbee())
+    {
+        self->Warn("identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
+    }
+
+	if(parent->UseXbee())
+	{
+        parent->pimpl_->AddTrackable(this,id);
+        tracker=NULL;
+	}
+	else
+	{
+	    parent->pimpl_->AddTrackable(self);
+        tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
+        tracker->register_change_handler(this,handle_pos);
+        tracker->shutup=true;
+	}
+
+	//state
+	cvmatrix_descriptor* desc=new cvmatrix_descriptor(6,1);
+	desc->SetElementName(0,0,"roll");
+	desc->SetElementName(1,0,"pitch");
+	desc->SetElementName(2,0,"yaw");
+	desc->SetElementName(3,0,"x");
+	desc->SetElementName(4,0,"y");
+	desc->SetElementName(5,0,"z");
+	output=new cvmatrix(self,desc,floatType);
+
+	desc=new cvmatrix_descriptor(3,1);
+	desc->SetElementName(0,0,"roll");
+	desc->SetElementName(1,0,"pitch");
+	desc->SetElementName(2,0,"yaw");
+	state=new cvmatrix(self,desc,floatType);
+
+	//ui
+	plot_tab=new Tab(tab,"Mesures "+ name);
+        x_plot=new DataPlot1D(plot_tab->NewRow(),"x",-10,10);
+            x_plot->AddCurve(output->Element(3));
+        y_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"y",-10,10);
+            y_plot->AddCurve(output->Element(4));
+        z_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"z",-2,0);
+            z_plot->AddCurve(output->Element(5));
+}
+
+VrpnObject_impl::~VrpnObject_impl(void)
+{
+    if(tracker!=NULL)//normal
+	{
+	    parent->pimpl_->RemoveTrackable(self);
+        tracker->unregister_change_handler(this,handle_pos);
+        delete tracker;
+	}
+	else//xbee
+	{
+        parent->pimpl_->RemoveTrackable(this);
+	}
+	delete plot_tab;
+}
+
+void VrpnObject_impl::mainloop(void)
+{
+	tracker->mainloop();
+}
+
+bool VrpnObject_impl::IsTracked(unsigned int timeout_ms)
+{
+    output->GetMutex();
+    Time a=GetTime();
+    Time dt=a-output->DataTime();
+    output->ReleaseMutex();
+
+    if(dt>(Time)(timeout_ms*1000000))
+    {
+        //self->Printf("%lld %lld %lld %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+void VrpnObject_impl::GetEuler(Euler &euler)
+{
+    output->GetMutex();
+    euler.roll=output->ValueNoMutex(0,0);
+    euler.pitch=output->ValueNoMutex(1,0);
+    euler.yaw=output->ValueNoMutex(2,0);
+    output->ReleaseMutex();
+}
+
+void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
+    output->GetMutex();
+    quaternion.q0=this->quaternion.q0;
+    quaternion.q1=this->quaternion.q1;
+    quaternion.q2=this->quaternion.q2;
+    quaternion.q3=this->quaternion.q3;
+    output->ReleaseMutex();
+}
+
+void VrpnObject_impl::GetPosition(Vector3D &point)
+{
+    output->GetMutex();
+    point.x=output->ValueNoMutex(3,0);
+    point.y=output->ValueNoMutex(4,0);
+    point.z=output->ValueNoMutex(5,0);
+    output->ReleaseMutex();
+}
+
+void VRPN_CALLBACK VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t)
+{
+    bool is_nan=false;
+	VrpnObject_impl* caller= reinterpret_cast<VrpnObject_impl*>(userdata);
+	Time time=GetTime();
+
+    //check if something is nan
+    for(int i=0;i<3;i++)
+    {
+        if(isnan(t.pos[i])==true) is_nan=true;
+    }
+    for(int i=0;i<4;i++)
+    {
+        if(isnan(t.quat[i])==true) is_nan=true;
+    }
+    if(is_nan==true)
+    {
+        caller->self->Warn("data is nan, skipping it (time %lld)\n",time);
+        return;
+    }
+
+    //on prend une fois pour toute le mutex et on fait des accès directs
+    caller->output->GetMutex();
+
+    //warning: t.quat is defined as (qx,qy,qz,qw), which is different from flair::core::Quaternion
+    caller->quaternion.q0=t.quat[3];
+    caller->quaternion.q1=t.quat[0];
+    caller->quaternion.q2=t.quat[1];
+    caller->quaternion.q3=t.quat[2];
+    Vector3D pos((float)t.pos[0],(float)t.pos[1],(float)t.pos[2]);
+
+    //on effectue les rotation
+    caller->parent->pimpl_->ComputeRotations(pos);
+    caller->parent->pimpl_->ComputeRotations(caller->quaternion);
+
+    Euler euler;
+    caller->quaternion.ToEuler(euler);
+    caller->output->SetValueNoMutex( 0, 0,euler.roll);
+    caller->output->SetValueNoMutex( 1, 0,euler.pitch);
+    caller->output->SetValueNoMutex( 2, 0,euler.yaw);
+    caller->output->SetValueNoMutex( 3, 0,pos.x);
+    caller->output->SetValueNoMutex( 4, 0,pos.y);
+    caller->output->SetValueNoMutex( 5, 0,pos.z);
+
+    caller->output->SetDataTime(time);
+    caller->output->ReleaseMutex();
+
+    caller->state->GetMutex();
+    caller->state->SetValueNoMutex( 0, 0,Euler::ToDegree(euler.roll));
+    caller->state->SetValueNoMutex( 1, 0,Euler::ToDegree(euler.pitch));
+    caller->state->SetValueNoMutex(2, 0,Euler::ToDegree(euler.yaw));
+    caller->state->ReleaseMutex();
+
+    caller->self->ProcessUpdate(caller->output);
+}
+
Index: /trunk/lib/FlairSensorActuator/src/XBldc.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/XBldc.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/XBldc.cpp	(revision 3)
@@ -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}
+//  created:    2013/11/13
+//  filename:   XBldc.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for Xufo Bldc
+//
+//
+/*********************************************************************/
+
+#include "XBldc.h"
+#include "XBldc_impl.h"
+
+using std::string;
+using namespace flair::core;
+using namespace flair::gui;
+
+namespace flair
+{
+namespace actuator
+{
+
+XBldc::XBldc(const IODevice* parent,Layout* layout,string name,I2cPort* i2cport) : Bldc(parent,layout,name,4) {
+    pimpl_=new XBldc_impl(this,i2cport);
+}
+
+XBldc::~XBldc() {
+    delete pimpl_;
+}
+
+void XBldc::SetMotors(float* value) {
+    pimpl_->SetMotors(value);
+}
+
+} // end namespace actuator
+} // end namespace flair
Index: /trunk/lib/FlairSensorActuator/src/XBldc.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/XBldc.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/XBldc.h	(revision 3)
@@ -0,0 +1,95 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file XBldc.h
+ * \brief Class for Xufo Bldc
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2013/11/13
+ * \version 4.0
+ */
+
+#ifndef XBLDC_H
+#define XBLDC_H
+
+#include "Bldc.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class IODevice;
+        class I2cPort;
+    }
+    namespace gui
+    {
+        class Layout;
+    }
+}
+
+class XBldc_impl;
+
+namespace flair
+{
+namespace actuator
+{
+    /*! \class XBldc
+    *
+    * \brief Class for Xufo Bldc
+    */
+    class XBldc : public Bldc
+    {
+        public:
+            /*!
+            * \brief Constructor
+            *
+            * Construct a XBldc.
+            *
+            * \param parent parent
+            * \param layout layout
+            * \param name name
+            * \param i2cport i2cport
+            */
+            XBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,core::I2cPort* i2cport);
+
+            /*!
+            * \brief Destructor
+            *
+            */
+            ~XBldc();
+
+            /*!
+            * \brief Has speed measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has speed measurement
+            */
+            bool HasSpeedMeasurement(void) const{return false;};
+
+            /*!
+            * \brief Has current measurement
+            *
+            * Reimplemented from Bldc. \n
+            *
+            * \return true if it has current measurement
+            */
+            bool HasCurrentMeasurement(void) const{return false;};
+
+        private:
+            /*!
+            * \brief Set motors values
+            *
+            * Reimplemented from Bldc. \n
+            * Values size must be the same as MotorsCount()
+            *
+            * \param values motor values
+            */
+            void SetMotors(float* value);
+
+            class XBldc_impl* pimpl_;
+    };
+} // end namespace actuator
+} // end namespace flair
+#endif // XBLDC_H
Index: /trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp	(revision 3)
@@ -0,0 +1,148 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/11/13
+//  filename:   XBldc_impl.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c Xufo
+//
+//
+/*********************************************************************/
+
+#include "XBldc_impl.h"
+#include "XBldc.h"
+#include <I2cPort.h>
+#include <cvmatrix.h>
+#include <string.h>
+
+#define BASE_ADDRESS 0x01
+#define MAX_VALUE 200
+
+using namespace flair::core;
+using namespace flair::gui;
+using namespace flair::actuator;
+
+XBldc_impl::XBldc_impl(XBldc* self,I2cPort* i2cport) {
+    this->self=self;
+	this->i2cport=i2cport;
+
+    i2cport->SetTxTimeout(5000000);
+}
+
+XBldc_impl::~XBldc_impl() {
+}
+
+void XBldc_impl::SetMotors(float* value) {
+    uint8_t tosend_value[5];
+
+    for(int i=0;i<4;i++) tosend_value[i]=(uint8_t)(MAX_VALUE*value[i]);
+
+    ssize_t written;
+    tosend_value[4]=0xaa;
+    for(int i=0;i<4;i++) tosend_value[4]+=tosend_value[i];
+    self->Err("please verify motors adresses\n");
+    //todo faire la procedure changement addresse par la station sol
+/*
+    i2cport->GetMutex();
+    i2cport->SetSlave(BASE_ADDRESS);
+
+    written =i2cport->Write(tosend_value, sizeof(tosend_value));
+    if(written<0)
+    {
+        self->Err("rt_dev_write error (%s)\n",strerror(-written));
+    }
+    else if (written != sizeof(tosend_value))
+    {
+        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tosend_value));
+    }
+
+    i2cport->ReleaseMutex();*/
+
+}
+/*
+void XBldc_impl::StartTest(uint8_t moteur)
+{
+    unsigned char tx[4];
+    ssize_t written;
+
+    tx[0]=251;
+    tx[1]=moteur;
+    tx[2]=0;
+    tx[3]=231+moteur;
+
+    i2cport->GetMutex();
+    i2cport->SetSlave(BASE_ADDRESS);
+
+    written =i2cport->Write(tx, sizeof(tx));
+    if(written<0)
+    {
+        self->Err("rt_dev_write error (%s)\n",strerror(-written));
+    }
+    else if (written != sizeof(tx))
+    {
+        self->Err("rt_dev_write %i/%i error\n",written,sizeof(tx));
+    }
+
+    i2cport->ReleaseMutex();
+}
+*/
+void XBldc_impl::ChangeDirection(uint8_t moteur) {
+    unsigned char tx[4];
+    ssize_t written;
+
+    tx[0]=254;
+    tx[1]=moteur;
+    tx[2]=0;
+    tx[3]=234+moteur;
+
+    i2cport->GetMutex();
+    i2cport->SetSlave(BASE_ADDRESS);
+
+    written =i2cport->Write(tx, sizeof(tx));
+    if(written<0) {
+        self->Err("rt_dev_write (%s) error\n",strerror(-written));
+    } else if (written != sizeof(tx)) {
+        self->Err("rt_dev_write %i/%i error\n",written,sizeof(tx));
+    }
+
+    i2cport->ReleaseMutex();
+}
+
+void XBldc_impl::ChangeAdress(uint8_t old_add,uint8_t new_add) {
+    unsigned char tx[4];
+    ssize_t written;
+
+    tx[0]=250;
+    tx[1]=old_add;
+    tx[2]=new_add;
+    tx[3]=230+old_add+new_add;
+
+    i2cport->GetMutex();
+    i2cport->SetSlave(BASE_ADDRESS);
+
+    written =i2cport->Write(tx, sizeof(tx));
+    if(written<0) {
+        self->Err("rt_dev_write error (%s)\n",strerror(-written));
+    } else if (written != sizeof(tx)) {
+        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
+    }
+
+    i2cport->ReleaseMutex();
+}
+
+uint8_t XBldc_impl::Sat(float value,uint8_t min,uint8_t max) {
+  uint8_t sat_value=(uint8_t)value;
+
+  if(value>((float)sat_value+0.5)) sat_value++;
+
+  if(value<(float)min) sat_value=min;
+  if(value>(float)max) sat_value=max;
+
+  return sat_value;
+}
Index: /trunk/lib/FlairSensorActuator/src/geodesie.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/geodesie.cpp	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/geodesie.cpp	(revision 3)
@@ -0,0 +1,343 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+#include <fstream>
+
+#include "geodesie.h"
+
+#ifdef _MSC_VER
+#   pragma warning(disable:4244)
+#endif //_MSC_VER
+
+namespace Geodesie {
+/// ////////////////////////////////////////////////////////////////////
+Matrice::Matrice(const Matrice & A) {
+    c0_l0=A.c0_l0;c1_l0=A.c1_l0;c2_l0=A.c2_l0;
+    c0_l1=A.c0_l1;c1_l1=A.c1_l1;c2_l1=A.c2_l1;
+    c0_l2=A.c0_l2;c1_l2=A.c1_l2;c2_l2=A.c2_l2;
+}
+/// ////////////////////////////////////////////////////////////////////
+Matrice::Matrice() {
+    c0_l0=0.0;c1_l0=0.0;c2_l0=0.0;
+    c0_l1=0.0;c1_l1=0.0;c2_l1=0.0;
+    c0_l2=0.0;c1_l2=0.0;c2_l2=0.0;
+}
+/// ////////////////////////////////////////////////////////////////////
+void Matrice::Apply(double v0,double v1,double v2, double& Mv0,double& Mv1,double& Mv2) {
+    Mv0 = c0_l0*v0 + c1_l0*v1 + c2_l0*v2;
+    Mv1 = c0_l1*v0 + c1_l1*v1 + c2_l1*v2;
+    Mv2 = c0_l2*v0 + c1_l2*v1 + c2_l2*v2;
+}
+/// ////////////////////////////////////////////////////////////////////
+Matrice ProdMat(const Matrice A, const Matrice B) {
+    Matrice out;
+
+    out.c0_l0=A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
+    out.c1_l0=A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
+    out.c2_l0=A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
+
+    out.c0_l1=A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
+    out.c1_l1=A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
+    out.c2_l1=A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
+
+    out.c0_l2=A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
+    out.c1_l2=A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
+    out.c2_l2=A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
+    return out;
+}
+
+/// ////////////////////////////////////////////////////////////////////
+Matrice TransMat(const Matrice A) {
+    Matrice out;
+    out.c0_l0=A.c0_l0 ; out.c1_l0 = A.c0_l1 ; out.c2_l0 = A.c0_l2 ;
+    out.c0_l1=A.c1_l0 ; out.c1_l1 = A.c1_l1 ; out.c2_l1 = A.c1_l2 ;
+    out.c0_l2=A.c2_l0 ; out.c1_l2 = A.c2_l1 ; out.c2_l2 = A.c2_l2 ;
+    return out;
+}
+
+/// ////////////////////////////////////////////////////////////////////
+void Write(const Matrice A,std::ostream& out) {
+    out<< A.c0_l0<<"\t"<< A.c1_l0<<"\t"<< A.c2_l0<<"\n";
+    out<< A.c0_l1<<"\t"<< A.c1_l1<<"\t"<< A.c2_l1<<"\n";
+    out<< A.c0_l2<<"\t"<< A.c1_l2<<"\t"<< A.c2_l2<<"\n";
+}
+
+/// ////////////////////////////////////////////////////////////////////
+Raf98::~Raf98() {
+    m_dvalues.clear();
+}
+
+//-----------------------------------------------------------------------------
+bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const {
+    *Hwgs84 = 0.0;
+    if (m_dvalues.size()==0)
+        return false;
+    const double longitude_min =  -5.5;
+    const double longitude_max =  8.5;
+    if (longitude < longitude_min)
+        return false;
+    if (longitude > longitude_max)
+        return false;
+
+    const double latitude_min =  42;
+    const double latitude_max =  51.5;
+    if (latitude < latitude_min)
+        return false;
+    if (latitude > latitude_max)
+        return false;
+
+    //conversion en position
+    double longPix = (longitude - longitude_min) * 30.;
+    double latPix  = (latitude_max - latitude) * 40.;
+
+    double RestCol,RestLig;
+    double  ColIni,LigIni;
+    RestCol = modf(longPix,&ColIni);
+    RestLig = modf(latPix,&LigIni);
+
+
+    double Zbd = (1.0-RestCol) * (1.0-RestLig) * LitGrille(ColIni  , LigIni  );
+    Zbd += RestCol * (1.0-RestLig) * LitGrille(ColIni+1, LigIni  );
+    Zbd += (1.0-RestCol) *   RestLig  * LitGrille(ColIni  , LigIni+1);
+    Zbd += RestCol * RestLig * LitGrille(ColIni+1, LigIni+1);
+    *Hwgs84 = Zbd;
+
+
+    return true;
+}
+/// ////////////////////////////////////////////////////////////////////
+double Raf98::LitGrille(unsigned int c,unsigned int l) const{
+    const unsigned int w=421;
+    //    const unsigned int h=381;
+    return m_dvalues.at(c+l*w);
+}
+/// ////////////////////////////////////////////////////////////////////
+bool Raf98::Load(const std::string & sin) {
+    std::ifstream in(sin.c_str());
+    unsigned int w = 421;
+    unsigned int h = 381;
+
+    m_dvalues.reserve(w*h);
+
+    char entete[1024];//sur 3 lignes
+    in.getline(entete,1023);
+    in.getline(entete,1023);
+    in.getline(entete,1023);
+
+    char bidon[1024];
+    double val;
+    for (unsigned int i=0; i< h; ++i) {
+        for (unsigned int j=0; j< 52; ++j)  {
+            for (unsigned int k=0; k< 8; ++k) {
+                in >> val;
+                m_dvalues.push_back( val );
+            }
+            in.getline(bidon,1023);
+        }
+        for (unsigned int k=0; k< 5; ++k) {
+            in >> val;
+            m_dvalues.push_back( val );
+        }
+        in.getline(bidon,1023);
+        if (!in.good())         {
+            m_dvalues.clear();
+            return false;
+        }
+    }
+    return in.good();
+}
+
+} // namespace Geodesie
+
+/// ////////////////////////////////////////////////////////////////////
+/// ////////////////////////////////////////////////////////////////////
+
+/// ////////////////////////////////////////////////////////////////////
+//ALGO0001
+double Geodesie::LatitueIsometrique(double latitude,double e) {
+    double li;
+    li = log(tan(M_PI_4 + latitude/2.)) + e*log( (1-e*sin(latitude))/(1+e*sin(latitude)) )/2;
+    return li;
+}
+
+/// ////////////////////////////////////////////////////////////////////
+//ALGO0002
+double Geodesie::LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon) {
+    double latitude_i=2*atan(exp(latitude_iso)) - M_PI_2;
+    double latitude_ip1=latitude_i+epsilon*2;
+    while (fabs(latitude_i-latitude_ip1)>epsilon) {
+        latitude_i=latitude_ip1;
+        latitude_ip1=2*atan(
+                    exp(e*0.5*
+                        log(
+                            (1+e*sin(latitude_i))/(1-e*sin(latitude_i))
+                            )
+                        )
+                    *exp(latitude_iso)
+                    ) - M_PI_2;
+    }
+    return latitude_ip1;
+}
+/// ////////////////////////////////////////////////////////////////////
+void Geodesie::Geo2ProjLambert(
+    double lambda,double phi,
+    double n, double c,double e,
+    double lambdac,double xs,double ys,
+    double& X,double& Y)
+{
+    double lat_iso=LatitueIsometrique(phi,e);
+    X=xs+c*exp(-n*lat_iso)*sin(n*(lambda-lambdac));
+    Y=ys-c*exp(-n*lat_iso)*cos(n*(lambda-lambdac));
+}
+/// ////////////////////////////////////////////////////////////////////
+//ALGO0004
+void Geodesie::Proj2GeoLambert(
+    double X,double Y,
+    double n, double c,double e,
+    double lambdac,double xs,double ys,
+    double epsilon,
+    double& lambda,double& phi)
+{
+    double X_xs=X-xs;
+    double ys_Y=ys-Y;
+    double R=sqrt(X_xs*X_xs+ys_Y*ys_Y);
+    double gamma=atan(X_xs/ys_Y);
+    lambda=lambdac+gamma/n;
+    double lat_iso=-1/n*log(fabs(R/c));
+    phi=LatitueIsometrique2Lat(lat_iso,e,epsilon);
+}
+/// ////////////////////////////////////////////////////////////////////
+double Geodesie::ConvMerApp(double longitude) {
+    double phi0_Lambert93 = Deg2Rad(46.5);
+    double lambda0_Lambert93 = Deg2Rad(3.0);
+    double conv=-sin(phi0_Lambert93)*(longitude-lambda0_Lambert93);
+    return  conv;
+}
+
+////////////////////////////////////////////////////////////////////
+void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out) {
+    Matrice passage;
+    double conv=Geodesie::ConvMerApp(lambda);
+    double c_=cos(conv);
+    double s_=sin(conv);
+
+    passage.c0_l0 = c_;
+    passage.c0_l1 = s_;
+    passage.c0_l2 =  0.0;
+
+    passage.c1_l0 = -s_;
+    passage.c1_l1 = c_;
+    passage.c1_l2 =  0.0;
+
+    passage.c2_l0 =  0.0;
+    passage.c2_l1 =  0.0;
+    passage.c2_l2 =  1.0;
+
+    out=ProdMat(passage,in);
+    double diff_h;
+    raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
+    h=he-diff_h;
+
+    Geodesie::Geo2ProjLambert(
+                lambda,phi,
+                n_Lambert93,c_Lambert93,e_Lambert93,
+                lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
+                E,N);
+}
+////////////////////////////////////////////////////////////////////////
+void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h) {
+    Geodesie::Geo2ProjLambert(
+                lambda,phi,
+                n_Lambert93,c_Lambert93,e_Lambert93,
+                lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
+                E,N);
+
+    double diff_h;
+    raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
+    h=he-diff_h;
+}
+/**
+    Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height)
+*/
+void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he) {
+    Geodesie::Proj2GeoLambert(
+                E,N,
+                n_Lambert93,c_Lambert93,e_Lambert93,
+                lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
+                0.0000000000000001,
+                lambda,phi);
+
+    double diff_h;
+    raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
+    he=h+diff_h;
+}
+////////////////////////////////////////////////////////////////////////
+void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out) {
+    Geodesie::Proj2GeoLambert(
+                E,N,
+                n_Lambert93,c_Lambert93,e_Lambert93,
+                lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
+                0.0000000000000001,
+                lambda,phi);
+
+    Matrice passage;
+    double conv=Geodesie::ConvMerApp(lambda);
+    double c_=cos(conv);
+    double s_=sin(conv);
+
+    passage.c0_l0 = c_;
+    passage.c0_l1 = -s_;
+    passage.c0_l2 =  0.0;
+
+    passage.c1_l0 = s_;
+    passage.c1_l1 = c_;
+    passage.c1_l2 =  0.0;
+
+    passage.c2_l0 =  0.0;
+    passage.c2_l1 =  0.0;
+    passage.c2_l2 =  1.0;
+
+    out=ProdMat(passage,in);
+
+    double diff_h;
+    raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
+    he=h+diff_h;
+}
+
+////////////////////////////////////////////////////////////////////////
+void Geodesie::Geographique_2_ECEF(double longitude,double latitude,double he,double& x,double& y,double& z) {
+    const double n = GRS_a / sqrt(1.0 - pow(GRS_e,2) * pow(sin(latitude),2));
+    x = (n + he) * cos(latitude) * cos(longitude);
+    y = (n + he) * cos(latitude) * sin(longitude);
+    z = (n * (1.0 - pow(GRS_e,2)) + he) * sin(latitude);
+}
+
+////////////////////////////////////////////////////////////////////////
+void Geodesie::ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0) {
+    double slat = std::sin(lat0);
+    double clat = std::cos(lat0);
+    double slon = std::sin(lon0);
+    double clon = std::cos(lon0);
+
+    Geodesie::Matrice C;
+    C.c0_l0 = -slon;
+    C.c1_l0 = clon;
+
+    C.c0_l1 = -clon * slat;
+    C.c1_l1 = -slon * slat;
+    C.c2_l1 = clat;
+
+    C.c0_l2 = clon * clat;
+    C.c1_l2 = slon * clat;
+    C.c2_l2 = slat;
+
+    double x0, y0, z0;
+    Geographique_2_ECEF(lon0,lat0,he0, x0,y0,z0);
+
+    x -= x0;
+    y -= y0;
+    z -= z0;
+
+    C.Apply(x,y,z, e,n,u);
+}
Index: /trunk/lib/FlairSensorActuator/src/groups.dox
===================================================================
--- /trunk/lib/FlairSensorActuator/src/groups.dox	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/groups.dox	(revision 3)
@@ -0,0 +1,18 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*! \namespace framework::sensor
+ * 
+ * \brief Sensor classes
+ */
+
+/*! \namespace framework::actuator
+ * 
+ * \brief Actuator classes
+ */
+
+/*! \namespace framework::meta
+ * 
+ * \brief High level classes
+ */
Index: /trunk/lib/FlairSensorActuator/src/unexported/AfroBldc_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/AfroBldc_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/AfroBldc_impl.h	(revision 3)
@@ -0,0 +1,63 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2015/06/15
+//  filename:   AfroBldc_impl.h
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c
+//
+//
+/*********************************************************************/
+
+
+#ifndef AFROBLDC_IMPL_H
+#define AFROBLDC_IMPL_H
+
+#include <stdint.h>
+#include <Object.h>
+
+#define MAX_MOTORS 8
+
+namespace flair
+{
+    namespace core
+    {
+        class I2cPort;
+    }
+    namespace gui
+    {
+        class SpinBox;
+        class GroupBox;
+        class Layout;
+    }
+    namespace actuator
+    {
+        class AfroBldc;
+    }
+    namespace sensor
+    {
+        class BatteryMonitor;
+    }
+}
+
+class AfroBldc_impl
+{
+    public:
+        AfroBldc_impl(flair::actuator::AfroBldc* self,flair::gui::Layout *layout,flair::core::I2cPort* i2cport);
+        ~AfroBldc_impl();
+        void SetMotors(float* value);
+
+    private:
+        void WriteValue(uint16_t value);//I2cPort mutex must be taken before calling this function
+        int nb_mot;
+        flair::core::I2cPort* i2cport;
+        flair::actuator::AfroBldc* self;
+};
+
+#endif // AFROBLDC_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/BlCtrlV2_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/BlCtrlV2_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/BlCtrlV2_impl.h	(revision 3)
@@ -0,0 +1,69 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2011/09/13
+//  filename:   BlCtrlV2_impl.h
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c
+//
+//
+/*********************************************************************/
+
+
+#ifndef BLCTRLV2_IMPL_H
+#define BLCTRLV2_IMPL_H
+
+#include <stdint.h>
+#include <Object.h>
+
+#define MAX_MOTORS 8
+
+namespace flair
+{
+    namespace core
+    {
+        class I2cPort;
+    }
+    namespace gui
+    {
+        class SpinBox;
+        class GroupBox;
+        class Layout;
+    }
+    namespace actuator
+    {
+        class BlCtrlV2;
+    }
+    namespace sensor
+    {
+        class BatteryMonitor;
+    }
+}
+
+class BlCtrlV2_impl
+{
+    public:
+        BlCtrlV2_impl(flair::actuator::BlCtrlV2* self,flair::gui::Layout *layout,flair::core::I2cPort* i2cport);
+        ~BlCtrlV2_impl();
+        void SetMotors(float* value);
+        flair::sensor::BatteryMonitor *battery;
+        flair::gui::SpinBox *poles;
+
+    private:
+        void WriteValue(uint16_t value);//I2cPort mutex must be taken before calling this function
+        void DetectMotors(void);
+        void GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage);//I2cPort mutex must be taken before calling this function
+        void GetCurrentAndSpeed(float &current,float &speed);//I2cPort mutex must be taken before calling this function
+        flair::core::Time last_voltage_time;
+        int nb_mot;
+        flair::core::I2cPort* i2cport;
+        flair::actuator::BlCtrlV2* self;
+};
+
+#endif // BLCTRLV2_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/Bldc_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/Bldc_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/Bldc_impl.h	(revision 3)
@@ -0,0 +1,71 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/11/14
+//  filename:   Bldc_impl.h
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Virtual class for brushless drivers
+//
+//
+/*********************************************************************/
+
+#ifndef BLDC_IMPL_H
+#define BLDC_IMPL_H
+
+#include <IODevice.h>
+#include <stdint.h>
+
+namespace flair
+{
+    namespace gui
+    {
+        class DoubleSpinBox;
+        class Layout;
+        class Label;
+        class DataPlot1D;
+        class TabWidget;
+        class PushButton;
+    }
+    namespace actuator
+    {
+        class Bldc;
+    }
+}
+
+class Bldc_impl
+{
+    public:
+        Bldc_impl(flair::actuator::Bldc* self,flair::gui::Layout* layout,std::string name,uint8_t motors_count);
+        Bldc_impl(flair::actuator::Bldc* self,uint8_t motors_count);//simulation
+        ~Bldc_impl();
+        void UpdateFrom(const flair::core::io_data *data);
+        void LockUserInterface(void) const;
+        void UnlockUserInterface(void) const;
+        bool are_enabled;
+        float* power;
+        void UseDefaultPlot(flair::gui::TabWidget* tab);
+        uint8_t motors_count;
+        flair::gui::Layout* layout;
+
+    private:
+        float *values;
+        float Sat(float value);
+        flair::actuator::Bldc* self;
+        flair::gui::DoubleSpinBox *min_value,*max_value,*test_value;
+        flair::gui::Label *flight_time;
+        flair::gui::DataPlot1D *plots;
+        flair::core::Time flight_start_time;
+        flair::gui::PushButton **button_test;
+        int time_sec;
+        bool is_running;
+        flair::core::Time test_start_time;
+        int tested_motor;//=-1 if no motor is tested
+};
+
+#endif // BLDC_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/Gx3_25_imu_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/Gx3_25_imu_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/Gx3_25_imu_impl.h	(revision 3)
@@ -0,0 +1,71 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file Gx3_25_imu_impl.h
+ * \brief Classe intégrant la centrale 3dmgx3-25
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2011/08/19
+ * \version 4.0
+ */
+
+#ifndef GX3_25_IMU_IMPL_H
+#define GX3_25_IMU_IMPL_H
+
+#include "Gx3_25_imu.h"
+#include <unistd.h> //ssize_t
+
+namespace flair {
+    namespace core {
+        class FrameworkManager;
+        class SerialPort;
+        class AhrsData;
+    }
+    namespace gui {
+        class SpinBox;
+        class CheckBox;
+        class PushButton;
+        class Label;
+    }
+}
+
+/*! \class Gx3_25_imu_impl
+*
+* \brief Classe intégrant la centrale 3dmgx3-25
+*/
+
+class Gx3_25_imu_impl {
+
+    public:
+        Gx3_25_imu_impl(flair::sensor::Gx3_25_imu* self,std::string name,flair::core::SerialPort *serialport,flair::sensor::Gx3_25_imu::Command_t command,flair::gui::GroupBox* setupgroupbox);
+        ~Gx3_25_imu_impl();
+        void Run(void);
+
+    private:
+        void DeviceReset(void);
+        void GetData(uint8_t* buf,ssize_t buf_size,flair::core::Time *time);
+        float Dequeue(uint8_t** buf);
+        void GyrosBias(void);
+        void SamplingSettings(void);
+        void SetBaudrate(int value);
+        bool CalcChecksum(uint8_t *buf,int size);
+        int FirmwareNumber(void);
+        void PrintModelInfo(void);
+        void RealignUpNorth(bool realign_up,bool realign_north);
+
+        void SetContinuousMode(uint8_t continuous_command);
+
+        flair::gui::GroupBox *setupgroupbox;
+        flair::gui::SpinBox *data_rate,*gyro_acc_size,*mag_size,*up_comp,*north_comp;
+        flair::gui::CheckBox *coning,*disable_magn,*disable_north_comp,*disable_grav_comp;
+        flair::gui::PushButton *button_bias;
+        flair::gui::Label *data_rate_label;
+
+        flair::core::SerialPort *serialport;
+        uint8_t command;
+        flair::sensor::Gx3_25_imu *self;
+        flair::core::AhrsData *ahrsData;
+};
+
+#endif // GX3_25_IMU_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/ParrotBldc_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/ParrotBldc_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/ParrotBldc_impl.h	(revision 3)
@@ -0,0 +1,73 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/12/19
+//  filename:   ParrotBldc_impl.h
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Classe integrant les moteurs ardrone
+//
+//
+/*********************************************************************/
+
+
+#ifndef PARROTBLDC_IMPL_H
+#define PARROTBLDC_IMPL_H
+
+#include <IODevice.h>
+#include <stdint.h>
+
+namespace flair {
+    namespace core {
+        class SerialPort;
+    }
+    namespace actuator {
+        class ParrotBldc;
+    }
+}
+
+class ParrotBldc_impl {
+
+    public:
+        ParrotBldc_impl(flair::actuator::ParrotBldc* self,flair::core::SerialPort* serialport);
+        ~ParrotBldc_impl();
+        void UpdateFrom(flair::core::io_data *data);
+        void SetMotors(float* value);
+
+    private:
+        typedef struct GpioData_t {
+            int gpio;
+            int value;
+        } GpioData_t;
+
+        enum class GpioMode_t {
+            GpioInput = 0, // Pin configured for input
+            GpioOutputLow, // Pin configured for output with low level
+            GpioOutputHigh, // Pin configured for output with high level
+        };
+
+        typedef struct GpioDirection_t {
+            int gpio;
+            GpioMode_t mode;
+        } GpioDirection_t;
+
+        void StartTest(void);
+        void StopTest(void);
+        void ResetBldc(void);
+        void SetGPIODirection(int gpio,GpioMode_t mode);
+        void SetGPIOValue(int gpio,bool value);
+        int WriteCmd(uint8_t cmd, uint8_t *reply, int replylen);
+        void SetLeds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3);
+
+        flair::actuator::ParrotBldc* self;
+        flair::core::SerialPort* serialport;
+        bool is_reseted;
+        int fd;
+};
+
+#endif // PARROTBLDC_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h	(revision 3)
@@ -0,0 +1,84 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/03
+//  filename:   VrpnClient_impl.h
+//
+//  author:     César Richard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet se connectant au serveur vrpn
+//
+//
+/*********************************************************************/
+
+#ifndef VRPNCLIENT_IMPL_H
+#define VRPNCLIENT_IMPL_H
+
+#include <string>
+#include <vector>
+
+namespace flair
+{
+    namespace core
+    {
+        class OneAxisRotation;
+        class Vector3D;
+        class Quaternion;
+        class Mutex;
+        class SerialPort;
+    }
+    namespace gui
+    {
+        class TabWidget;
+        class Tab;
+        class Layout;
+    }
+    namespace sensor
+    {
+        class VrpnClient;
+        class VrpnObject;
+    }
+}
+
+class VrpnObject_impl;
+class vrpn_Connection;
+
+class VrpnClient_impl
+{
+    public:
+        VrpnClient_impl(flair::sensor::VrpnClient* self,std::string name,std::string address,uint16_t us_period);
+        VrpnClient_impl(flair::sensor::VrpnClient* self,std::string name,flair::core::SerialPort* serialport,uint16_t us_period);
+        ~VrpnClient_impl();
+        void AddTrackable(flair::sensor::VrpnObject* obj);//normal
+        void RemoveTrackable(flair::sensor::VrpnObject* obj);//normal
+        void AddTrackable(VrpnObject_impl* obj,uint8_t id);//xbee
+        void RemoveTrackable(VrpnObject_impl* obj);//xbee
+        void ComputeRotations(flair::core::Vector3D& point);
+        void ComputeRotations(flair::core::Quaternion& quat);
+        bool UseXbee(void);
+        void Run(void);
+        flair::gui::Tab* setup_tab;
+        flair::gui::TabWidget* tab;
+        vrpn_Connection *connection;
+
+    private:
+        flair::sensor::VrpnClient* self;
+        flair::core::Mutex* mutex;
+        uint16_t us_period;
+        std::vector<flair::sensor::VrpnObject*> trackables;
+        typedef struct xbee_object{
+            VrpnObject_impl* vrpnobject;
+            uint8_t id;
+        } xbee_object;
+
+        std::vector<xbee_object> xbee_objects;
+        flair::gui::Tab* main_tab;
+        flair::core::OneAxisRotation *rotation_1,*rotation_2;
+        flair::core::SerialPort* serialport;
+};
+
+#endif // VRPNCLIENT_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h	(revision 3)
@@ -0,0 +1,78 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/04/03
+//  filename:   VrpnObject_impl.h
+//
+//  author:     César Richard, Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet vrpn
+//
+//
+/*********************************************************************/
+
+#ifndef VRPNOBJECT_IMPL_H
+#define VRPNOBJECT_IMPL_H
+
+#include <IODevice.h>
+#include <string>
+#include <stdint.h>
+#include <vrpn_Tracker.h>
+#include "Quaternion.h"
+
+namespace flair
+{
+    namespace core
+    {
+        class cvmatrix;
+        class Vector3D;
+        class Euler;
+    }
+    namespace gui
+    {
+        class TabWidget;
+        class Tab;
+        class DataPlot1D;
+    }
+    namespace sensor
+    {
+        class VrpnClient;
+        class VrpnObject;
+    }
+}
+
+class VrpnObject_impl
+{
+    friend class VrpnClient_impl;
+
+    public:
+        VrpnObject_impl(flair::sensor::VrpnObject* self,const flair::sensor::VrpnClient *parent,std::string name,int id,const flair::gui::TabWidget* tab);
+        ~VrpnObject_impl(void);
+
+        void mainloop(void);
+        void GetEuler(flair::core::Euler &euler);
+        void GetQuaternion(flair::core::Quaternion &quaternion);
+        void GetPosition(flair::core::Vector3D &point);
+        bool IsTracked(unsigned int timeout_ms);
+
+        flair::gui::Tab* plot_tab;
+        flair::gui::DataPlot1D* x_plot;
+        flair::gui::DataPlot1D* y_plot;
+        flair::gui::DataPlot1D* z_plot;
+        flair::core::cvmatrix *output,*state;
+
+        static void	VRPN_CALLBACK handle_pos(void *userdata, const vrpn_TRACKERCB t);
+
+    private:
+        flair::sensor::VrpnObject* self;
+        const flair::sensor::VrpnClient *parent;
+        vrpn_Tracker_Remote* tracker;
+        flair::core::Quaternion quaternion;//todo: quaternion should be included in the output to replace euler angles
+        void Update(void);
+};
+
+#endif // VRPNOBJECT_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/XBldc_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/XBldc_impl.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/XBldc_impl.h	(revision 3)
@@ -0,0 +1,56 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2013/11/13
+//  filename:   XBldc_impl.h
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    objet integrant les moteurs i2c Xufo
+//
+//
+/*********************************************************************/
+
+
+#ifndef XBLDC_IMPL_H
+#define XBLDC_IMPL_H
+
+#include <IODevice.h>
+#include <stdint.h>
+
+namespace flair
+{
+    namespace core
+    {
+        class I2cPort;
+    }
+    namespace actuator
+    {
+        class XBldc;
+    }
+}
+
+
+class XBldc_impl
+{
+
+    public:
+        XBldc_impl(flair::actuator::XBldc* self,flair::core::I2cPort* i2cport);
+        ~XBldc_impl();
+        void UpdateFrom(flair::core::io_data *data);
+        void SetMotors(float* value);
+
+    private:
+        uint8_t Sat(float value,uint8_t min,uint8_t max);
+        //void StartTest(uint8_t moteur);
+        void ChangeDirection(uint8_t moteur);
+        void ChangeAdress(uint8_t old_add,uint8_t new_add);
+        flair::actuator::XBldc* self;
+        flair::core::I2cPort* i2cport;
+};
+
+#endif // XBLDC_IMPL_H
Index: /trunk/lib/FlairSensorActuator/src/unexported/geodesie.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/geodesie.h	(revision 3)
+++ /trunk/lib/FlairSensorActuator/src/unexported/geodesie.h	(revision 3)
@@ -0,0 +1,158 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+#ifndef GEODESIE_H
+#define GEODESIE_H
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+namespace Geodesie {
+
+#ifndef M_PI
+#   define M_PI 3.14159265358979323846
+#endif
+#ifndef M_PI_2
+#   define M_PI_2 1.57079632679489661923
+#endif
+#ifndef M_PI_4
+#   define M_PI_4 0.78539816339744830962
+#endif
+
+////////////////////////////////////////////////////////////////////////
+struct Matrice {
+    Matrice(const Matrice & A);
+    Matrice();
+    void Apply(double v0, double v1, double v2, double & Mv0, double & Mv1, double & Mv2);
+    double c0_l0;double c1_l0;double c2_l0;
+    double c0_l1;double c1_l1;double c2_l1;
+    double c0_l2;double c1_l2;double c2_l2;
+}; // class
+
+Matrice TransMat(const Matrice A);
+
+Matrice ProdMat(const Matrice A,const Matrice B);
+void Write(const Matrice A,std::ostream& out);
+
+////////////////////////////////////////////////////////////////////////
+class Raf98 {
+private :
+    std::vector<double> m_dvalues;
+    double LitGrille(unsigned int c,unsigned int l) const;
+public :
+    ~Raf98();
+    Raf98() {}
+    bool Load(const std::string & s);
+    bool Interpol(double longitude/*deg*/, double latitude/*deg*/, double* Hwgs84) const;
+}; // class
+////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////
+inline double Deg2Rad(double deg) {return deg*M_PI/180.0;}
+inline double Rad2Deg(double rad) {return rad*180.0/M_PI;}
+////////////////////////////////////////////////////////////////////////
+
+const double a_Lambert93=6378137;
+const double f_Lambert93=1 / 298.257222101;
+const double e_Lambert93=sqrt(f_Lambert93*(2-f_Lambert93));
+const double lambda0_Lambert93=Deg2Rad(3.0);//degres
+const double phi0_Lambert93=Deg2Rad(46.5);
+const double phi1_Lambert93=Deg2Rad(44.0);
+const double phi2_Lambert93=Deg2Rad(49.0);//degres
+const double X0_Lambert93=700000;//
+const double Y0_Lambert93=6600000;//
+const double n_Lambert93 = 0.7256077650;
+const double c_Lambert93 = 11754255.426;
+const double xs_Lambert93 = 700000;
+const double ys_Lambert93 = 12655612.050;
+
+const double GRS_a = 6378137;
+const double GRS_f = 1/298.257222101;
+const double GRS_b = GRS_a*(1-GRS_f);
+const double GRS_e = sqrt((pow(GRS_a,2) - pow(GRS_b,2)) / pow(GRS_a,2));
+
+////////////////////////////////////////////////////////////////////////
+void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out);
+void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h);
+void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he);
+void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out);
+/** Convert from geographique to ECEF.
+ * @param[in] longitude Longitude in radian.
+ * @param[in] latitude Latitude in radian.
+ * @param[in] he Height in meter.
+ */
+void Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z);
+/** Convert from ECEF two ENU.
+ * @param[in] lon0 Longitude of the origin in radian.
+ * @param[in] lat0 Latitude of the origin in radian.
+ * @param[in] he0 Height of the origin in radian.
+ */
+void ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0);
+////////////////////////////////////////////////////////////////////////
+
+//ALGO0001
+double LatitueIsometrique(double latitude,double e);
+//ALGO0002
+double LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon);
+
+//ALGO0003
+void Geo2ProjLambert(
+    double lambda,double phi,
+    double n, double c,double e,
+    double lambdac,double xs,double ys,
+    double& X,double& Y);
+//ALGO0004
+void Proj2GeoLambert(
+    double X,double Y,
+    double n, double c,double e,
+    double lambdac,double xs,double ys,
+    double epsilon,
+    double& lambda,double& phi);
+
+double ConvMerApp(double longitude);
+
+/**
+Converts Cartesian (x, y) coordinates to polar coordinates (r, theta)
+*/
+template <typename _T1, typename _T2>
+void cartesianToPolar(const _T1 x, const _T1 y, _T2 & r, _T2 & theta) {
+    r = std::sqrt(x*x + y*y);
+    theta = std::atan2(x, y);
+}
+
+/**
+Converts polar coordinates (r, theta) to Cartesian (x, y) coordinates
+*/
+template <typename _T1, typename _T2>
+void polarToCartesian(const _T1 r, const _T1 theta, _T2 & x, _T2 & y) {
+    x = r * std::cos(theta);
+    y = r * std::sin(theta);
+}
+
+/**
+Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta, phi)
+Angles expressed in radians.
+*/
+template <typename _T1, typename _T2>
+void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 & r, _T2 & theta, _T2 & phi) {
+    r = std::sqrt(x*x + y*y + z*z);
+    theta = std::acos(z / r);
+    phi = std::atan2(y, x);
+}
+
+/**
+Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z) coordinates.
+Angles expressed in radians.
+*/
+template <typename _T1, typename _T2>
+void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 & x, _T2 & y, _T2 & z) {
+    x = r * std::sin(theta) * std::cos(phi);
+    y = r * std::sin(theta) * std::sin(phi);
+    z = r * std::cos(theta);
+}
+
+} // namespace Geodesie
+
+#endif // GEODESIE_H
