Index: unk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.cpp	(revision 5)
+++ 	(revision )
@@ -1,134 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBatteryMonitor.h	(revision 5)
+++ 	(revision )
@@ -1,68 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotBldc.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBldc.cpp	(revision 5)
+++ 	(revision )
@@ -1,46 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotBldc.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBldc.h	(revision 5)
+++ 	(revision )
@@ -1,96 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotBldc_impl.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotBldc_impl.cpp	(revision 5)
+++ 	(revision )
@@ -1,229 +1,0 @@
-// %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;
-
-    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: unk/lib/FlairSensorActuator/src/ParrotCamH.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamH.cpp	(revision 5)
+++ 	(revision )
@@ -1,1053 +1,0 @@
-//  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: unk/lib/FlairSensorActuator/src/ParrotCamH.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamH.h	(revision 5)
+++ 	(revision )
@@ -1,145 +1,0 @@
-/*!
- * \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: unk/lib/FlairSensorActuator/src/ParrotCamV.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamV.cpp	(revision 5)
+++ 	(revision )
@@ -1,370 +1,0 @@
-//  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: unk/lib/FlairSensorActuator/src/ParrotCamV.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotCamV.h	(revision 5)
+++ 	(revision )
@@ -1,139 +1,0 @@
-/*!
- * \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: unk/lib/FlairSensorActuator/src/ParrotGps.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotGps.cpp	(revision 5)
+++ 	(revision )
@@ -1,93 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotGps.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotGps.h	(revision 5)
+++ 	(revision )
@@ -1,81 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotNavBoard.cpp
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.cpp	(revision 5)
+++ 	(revision )
@@ -1,236 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/ParrotNavBoard.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/ParrotNavBoard.h	(revision 5)
+++ 	(revision )
@@ -1,122 +1,0 @@
-// %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: unk/lib/FlairSensorActuator/src/unexported/ParrotBldc_impl.h
===================================================================
--- /trunk/lib/FlairSensorActuator/src/unexported/ParrotBldc_impl.h	(revision 5)
+++ 	(revision )
@@ -1,74 +1,0 @@
-// %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
-//
-//
-/*********************************************************************/
-
-//see linux-ardrone2-2.6.32-9/drivers/parrot/char/gpio2_ioctl.h for ardrone2 specific gpios definitions
-
-#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
