Changeset 15 in flair-src for trunk/lib/FlairSensorActuator


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (9 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

Location:
trunk/lib/FlairSensorActuator/src
Files:
78 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/AfroBldc.cpp

    r3 r15  
    3030using namespace flair::gui;
    3131
    32 namespace flair { namespace actuator {
     32namespace flair {
     33namespace actuator {
    3334
    34 AfroBldc::AfroBldc(const IODevice* parent,Layout* layout,string name,uint8_t motors_count,I2cPort* i2cport) : Bldc(parent,layout,name,motors_count) {
    35     pimpl_=new AfroBldc_impl(this,layout,i2cport);
     35AfroBldc::AfroBldc(const IODevice *parent, Layout *layout, string name,
     36                   uint8_t motors_count, I2cPort *i2cport)
     37    : Bldc(parent, layout, name, motors_count) {
     38  pimpl_ = new AfroBldc_impl(this, layout, i2cport);
    3639}
    3740
    38 AfroBldc::~AfroBldc() {
    39     delete pimpl_;
    40 }
     41AfroBldc::~AfroBldc() { delete pimpl_; }
    4142
    42 void AfroBldc::SetMotors(float* values) {
    43     pimpl_->SetMotors(values);
    44 }
     43void AfroBldc::SetMotors(float *values) { pimpl_->SetMotors(values); }
    4544
    4645} // end namespace actuator

  • trunk/lib/FlairSensorActuator/src/AfroBldc.h

    r3 r15  
    1616#include "Bldc.h"
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class FrameworkManager;
    23         class I2cPort;
    24     }
    25     namespace sensor
    26     {
    27         class BatteryMonitor;
    28     }
     18namespace flair {
     19namespace core {
     20class FrameworkManager;
     21class I2cPort;
     22}
     23namespace sensor {
     24class BatteryMonitor;
     25}
    2926}
    3027
    3128class AfroBldc_impl;
    3229
    33 namespace flair
    34 {
    35 namespace actuator
    36 {
    37     /*! \class AfroBldc
    38     *
    39     * \brief Class for Mikrokopter's blctrlv2
    40     *
    41     * blctrlv2 drivers can also monitor the battery level. See GetBatteryMonitor().
    42     */
    43     class AfroBldc : public Bldc
    44     {
    45         friend class ::AfroBldc_impl;
     30namespace flair {
     31namespace actuator {
     32/*! \class AfroBldc
     33*
     34* \brief Class for Mikrokopter's blctrlv2
     35*
     36* blctrlv2 drivers can also monitor the battery level. See GetBatteryMonitor().
     37*/
     38class AfroBldc : public Bldc {
     39  friend class ::AfroBldc_impl;
    4640
    47         public:
    48             /*!
    49             * \brief Constructor
    50             *
    51             * Construct a AfroBldc.
    52             *
    53             * \param parent parent
    54             * \param layout layout
    55             * \param name name
    56             * \param motors_count number of motors
    57             * \param i2cport I2cPort
    58             */
    59             AfroBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count,core::I2cPort* i2cport);
     41public:
     42  /*!
     43  * \brief Constructor
     44  *
     45  * Construct a AfroBldc.
     46  *
     47  * \param parent parent
     48  * \param layout layout
     49  * \param name name
     50  * \param motors_count number of motors
     51  * \param i2cport I2cPort
     52  */
     53  AfroBldc(const core::IODevice *parent, gui::Layout *layout, std::string name,
     54           uint8_t motors_count, core::I2cPort *i2cport);
    6055
    61             /*!
    62             * \brief Destructor
    63             *
    64             */
    65             ~AfroBldc();
     56  /*!
     57  * \brief Destructor
     58  *
     59  */
     60  ~AfroBldc();
    6661
    67             /*!
    68             * \brief Has speed measurement
    69             *
    70             * Reimplemented from Bldc. \n
    71             *
    72             * \return true if it has speed measurement
    73             */
    74             bool HasSpeedMeasurement(void) const{return false;};
     62  /*!
     63  * \brief Has speed measurement
     64  *
     65  * Reimplemented from Bldc. \n
     66  *
     67  * \return true if it has speed measurement
     68  */
     69  bool HasSpeedMeasurement(void) const { return false; };
    7570
    76             /*!
    77             * \brief Has current measurement
    78             *
    79             * Reimplemented from Bldc. \n
    80             *
    81             * \return true if it has current measurement
    82             */
    83             bool HasCurrentMeasurement(void) const{return false;};
     71  /*!
     72  * \brief Has current measurement
     73  *
     74  * Reimplemented from Bldc. \n
     75  *
     76  * \return true if it has current measurement
     77  */
     78  bool HasCurrentMeasurement(void) const { return false; };
    8479
    85         private:
    86             /*!
    87             * \brief Set motors values
    88             *
    89             * Reimplemented from Bldc. \n
    90             * Values size must be the same as MotorsCount()
    91             *
    92             * \param values motor values
    93             */
    94             void SetMotors(float* values);
     80private:
     81  /*!
     82  * \brief Set motors values
     83  *
     84  * Reimplemented from Bldc. \n
     85  * Values size must be the same as MotorsCount()
     86  *
     87  * \param values motor values
     88  */
     89  void SetMotors(float *values);
    9590
    96             class AfroBldc_impl* pimpl_;
    97     };
     91  class AfroBldc_impl *pimpl_;
     92};
    9893} // end namespace actuator
    9994} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp

    r3 r15  
    3434using namespace flair::actuator;
    3535
    36 AfroBldc_impl::AfroBldc_impl(AfroBldc* self,Layout *layout,I2cPort* i2cport) {
    37     this->self=self;
    38     this->i2cport=i2cport;
    39     nb_mot=self->MotorsCount();
     36AfroBldc_impl::AfroBldc_impl(AfroBldc *self, Layout *layout, I2cPort *i2cport) {
     37  this->self = self;
     38  this->i2cport = i2cport;
     39  nb_mot = self->MotorsCount();
    4040}
    4141
    42 AfroBldc_impl::~AfroBldc_impl() {
     42AfroBldc_impl::~AfroBldc_impl() {}
    4343
     44void AfroBldc_impl::SetMotors(float *value) {
     45  uint16_t tosend_value[nb_mot];
     46
     47  for (int i = 0; i < nb_mot; i++)
     48    tosend_value[i] = (uint16_t)(MAX_VALUE * value[i]);
     49
     50  i2cport->GetMutex();
     51
     52  for (int i = 0; i < nb_mot; i++) {
     53    i2cport->SetSlave(BASE_ADDRESS + i);
     54    WriteValue(tosend_value[i]);
     55  }
     56  i2cport->ReleaseMutex();
    4457}
    4558
    46 void AfroBldc_impl::SetMotors(float* value) {
    47     uint16_t tosend_value[nb_mot];
     59// I2cPort mutex must be taken before calling this function
     60void AfroBldc_impl::WriteValue(uint16_t value) {
     61  uint8_t tx[2];
     62  ssize_t written;
    4863
    49     for(int i=0; i<nb_mot; i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
    50 
    51     i2cport->GetMutex();
    52 
    53     for(int i=0; i<nb_mot; i++) {
    54         i2cport->SetSlave(BASE_ADDRESS+i);
    55         WriteValue(tosend_value[i]);
    56     }
    57     i2cport->ReleaseMutex();
     64  tx[0] = (uint8_t)(value >> 3); // msb
     65  tx[1] = (value & 0x07);        // lsb
     66  written = i2cport->Write(tx, 2);
     67  if (written < 0) {
     68    self->Err("rt_dev_write error (%s)\n", strerror(-written));
     69  } else if (written != sizeof(tx)) {
     70    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
     71  }
    5872}
    59 
    60 //I2cPort mutex must be taken before calling this function
    61 void AfroBldc_impl::WriteValue(uint16_t value) {
    62     uint8_t tx[2];
    63     ssize_t written;
    64 
    65     tx[0]=(uint8_t)(value>>3);//msb
    66     tx[1]=(value&0x07);//lsb
    67     written =i2cport->Write(tx, 2);
    68     if(written<0) {
    69         self->Err("rt_dev_write error (%s)\n",strerror(-written));
    70     } else if (written != sizeof(tx)) {
    71         self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
    72     }
    73 }

  • trunk/lib/FlairSensorActuator/src/BatteryMonitor.cpp

    r3 r15  
    1616/*********************************************************************/
    1717
    18 
    1918#include "BatteryMonitor.h"
    2019#include <Layout.h>
    … …  
    2625using namespace flair::gui;
    2726
    28 namespace flair { namespace sensor {
     27namespace flair {
     28namespace sensor {
    2929
    30 BatteryMonitor::BatteryMonitor(const gui::LayoutPosition* position,string name) : GroupBox(position,name) {
    31     battery=new Label(this->NewRow(),"battery");
    32     battery_thresh=new DoubleSpinBox(this->LastRowLastCol(),"threshold"," V",0,24,.1,1);
     30BatteryMonitor::BatteryMonitor(const gui::LayoutPosition *position, string name)
     31    : GroupBox(position, name) {
     32  battery = new Label(this->NewRow(), "battery");
     33  battery_thresh = new DoubleSpinBox(this->LastRowLastCol(), "threshold", " V",
     34                                     0, 24, .1, 1);
    3335}
    3436
    35 BatteryMonitor::~BatteryMonitor() {
     37BatteryMonitor::~BatteryMonitor() {}
    3638
    37 }
    38 
    39 float BatteryMonitor::GetVoltage(void) const {
    40     return batteryvalue;
    41 }
     39float BatteryMonitor::GetVoltage(void) const { return batteryvalue; }
    4240
    4341bool BatteryMonitor::IsBatteryLow(void) const {
    44     if(batteryvalue<battery_thresh->Value())
    45         return true;
    46     else
    47         return false;
     42  if (batteryvalue < battery_thresh->Value())
     43    return true;
     44  else
     45    return false;
    4846}
    4947
    5048void BatteryMonitor::SetBatteryValue(float value) {
    51     batteryvalue=value;
    52     if(value>0) {
    53         battery->SetText("battery: %.1fV",value);
    54     } else {
    55         battery->SetText("battery: unreadable");
    56     }
     49  batteryvalue = value;
     50  if (value > 0) {
     51    battery->SetText("battery: %.1fV", value);
     52  } else {
     53    battery->SetText("battery: unreadable");
     54  }
    5755}
    5856

  • trunk/lib/FlairSensorActuator/src/BatteryMonitor.h

    r3 r15  
    1616#include <GroupBox.h>
    1717
    18 namespace flair
    19 {
    20     namespace gui
    21     {
    22         class LayoutPosition;
    23         class Label;
    24         class DoubleSpinBox;
    25     }
     18namespace flair {
     19namespace gui {
     20class LayoutPosition;
     21class Label;
     22class DoubleSpinBox;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30 namespace sensor
    31 {
     26namespace flair {
     27namespace sensor {
    3228
    33     /*! \class BatteryMonitor
    34     *
    35     * \brief Base class for battery monitor
    36     */
     29/*! \class BatteryMonitor
     30*
     31* \brief Base class for battery monitor
     32*/
    3733
    38     class BatteryMonitor : public gui::GroupBox
    39     {
    40         public:
    41             /*!
    42             * \brief Constructor
    43             *
    44             * Construct a BatteryMonitor at given position.
    45             *
    46             * \param position position
    47             * \param name name
    48             */
    49             BatteryMonitor(const gui::LayoutPosition* position,std::string name);
     34class BatteryMonitor : public gui::GroupBox {
     35public:
     36  /*!
     37  * \brief Constructor
     38  *
     39  * Construct a BatteryMonitor at given position.
     40  *
     41  * \param position position
     42  * \param name name
     43  */
     44  BatteryMonitor(const gui::LayoutPosition *position, std::string name);
    5045
    51             /*!
    52             * \brief Destructor
    53             *
    54             */
    55             ~BatteryMonitor();
     46  /*!
     47  * \brief Destructor
     48  *
     49  */
     50  ~BatteryMonitor();
    5651
    57             /*!
    58             * \brief Is batteru low?
    59             *
    60             * \return true if battery is below threshold
    61             *
    62             */
    63             bool IsBatteryLow(void) const;
     52  /*!
     53  * \brief Is batteru low?
     54  *
     55  * \return true if battery is below threshold
     56  *
     57  */
     58  bool IsBatteryLow(void) const;
    6459
    65             /*!
    66             * \brief Set battery value
    67             *
    68             * \param battery value
    69             *
    70             */
    71             void SetBatteryValue(float value);
     60  /*!
     61  * \brief Set battery value
     62  *
     63  * \param battery value
     64  *
     65  */
     66  void SetBatteryValue(float value);
    7267
    73             /*!
    74             * \brief Get battery voltage
    75             *
    76             * \return battery voltage
    77             *
    78             */
    79             float GetVoltage(void) const;
     68  /*!
     69  * \brief Get battery voltage
     70  *
     71  * \return battery voltage
     72  *
     73  */
     74  float GetVoltage(void) const;
    8075
    81         private:
    82             float batteryvalue;
    83             gui::DoubleSpinBox *battery_thresh;
    84             gui::Label *battery;
    85     };
     76private:
     77  float batteryvalue;
     78  gui::DoubleSpinBox *battery_thresh;
     79  gui::Label *battery;
     80};
    8681} // end namespace sensor
    8782} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/BlCtrlV2.cpp

    r3 r15  
    3131using namespace flair::gui;
    3232
    33 namespace flair
    34 {
    35 namespace actuator
    36 {
     33namespace flair {
     34namespace actuator {
    3735
    38 BlCtrlV2::BlCtrlV2(const IODevice* parent,Layout* layout,string name,uint8_t motors_count,I2cPort* i2cport) : Bldc(parent,layout,name,motors_count)
    39 {
    40     pimpl_=new BlCtrlV2_impl(this,layout,i2cport);
     36BlCtrlV2::BlCtrlV2(const IODevice *parent, Layout *layout, string name,
     37                   uint8_t motors_count, I2cPort *i2cport)
     38    : Bldc(parent, layout, name, motors_count) {
     39  pimpl_ = new BlCtrlV2_impl(this, layout, i2cport);
    4140}
    4241
    43 BlCtrlV2::~BlCtrlV2()
    44 {
    45     delete pimpl_;
     42BlCtrlV2::~BlCtrlV2() { delete pimpl_; }
     43
     44BatteryMonitor *BlCtrlV2::GetBatteryMonitor(void) const {
     45  return pimpl_->battery;
    4646}
    4747
    48 BatteryMonitor* BlCtrlV2::GetBatteryMonitor(void) const
    49 {
    50     return pimpl_->battery;
    51 }
    52 
    53 void BlCtrlV2::SetMotors(float* values)
    54 {
    55     pimpl_->SetMotors(values);
    56 }
     48void BlCtrlV2::SetMotors(float *values) { pimpl_->SetMotors(values); }
    5749
    5850} // end namespace actuator

  • trunk/lib/FlairSensorActuator/src/BlCtrlV2.h

    r3 r15  
    1616#include "Bldc.h"
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class FrameworkManager;
    23         class I2cPort;
    24     }
    25     namespace sensor
    26     {
    27         class BatteryMonitor;
    28     }
     18namespace flair {
     19namespace core {
     20class FrameworkManager;
     21class I2cPort;
     22}
     23namespace sensor {
     24class BatteryMonitor;
     25}
    2926}
    3027
    3128class BlCtrlV2_impl;
    3229
    33 namespace flair
    34 {
    35 namespace actuator
    36 {
    37     /*! \class BlCtrlV2
    38     *
    39     * \brief Class for Mikrokopter's blctrlv2
    40     *
    41     * blctrlv2 drivers can also monitor the battery level. See GetBatteryMonitor().
    42     */
    43     class BlCtrlV2 : public Bldc
    44     {
    45         friend class ::BlCtrlV2_impl;
     30namespace flair {
     31namespace actuator {
     32/*! \class BlCtrlV2
     33*
     34* \brief Class for Mikrokopter's blctrlv2
     35*
     36* blctrlv2 drivers can also monitor the battery level. See GetBatteryMonitor().
     37*/
     38class BlCtrlV2 : public Bldc {
     39  friend class ::BlCtrlV2_impl;
    4640
    47         public:
    48             /*!
    49             * \brief Constructor
    50             *
    51             * Construct a BlCtrlV2.
    52             *
    53             * \param parent parent
    54             * \param layout layout
    55             * \param name name
    56             * \param motors_count number of motors
    57             * \param i2cport I2cPort
    58             */
    59             BlCtrlV2(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count,core::I2cPort* i2cport);
     41public:
     42  /*!
     43  * \brief Constructor
     44  *
     45  * Construct a BlCtrlV2.
     46  *
     47  * \param parent parent
     48  * \param layout layout
     49  * \param name name
     50  * \param motors_count number of motors
     51  * \param i2cport I2cPort
     52  */
     53  BlCtrlV2(const core::IODevice *parent, gui::Layout *layout, std::string name,
     54           uint8_t motors_count, core::I2cPort *i2cport);
    6055
    61             /*!
    62             * \brief Destructor
    63             *
    64             */
    65             ~BlCtrlV2();
     56  /*!
     57  * \brief Destructor
     58  *
     59  */
     60  ~BlCtrlV2();
    6661
    67             /*!
    68             * \brief Get battery monitor
    69             *
    70             * \return BatteryMonitor
    71             */
    72             sensor::BatteryMonitor* GetBatteryMonitor(void) const;
     62  /*!
     63  * \brief Get battery monitor
     64  *
     65  * \return BatteryMonitor
     66  */
     67  sensor::BatteryMonitor *GetBatteryMonitor(void) const;
    7368
    74             /*!
    75             * \brief Has speed measurement
    76             *
    77             * Reimplemented from Bldc. \n
    78             *
    79             * \return true if it has speed measurement
    80             */
    81             bool HasSpeedMeasurement(void) const{return true;};
     69  /*!
     70  * \brief Has speed measurement
     71  *
     72  * Reimplemented from Bldc. \n
     73  *
     74  * \return true if it has speed measurement
     75  */
     76  bool HasSpeedMeasurement(void) const { return true; };
    8277
    83             /*!
    84             * \brief Has current measurement
    85             *
    86             * Reimplemented from Bldc. \n
    87             *
    88             * \return true if it has current measurement
    89             */
    90             bool HasCurrentMeasurement(void) const{return true;};
     78  /*!
     79  * \brief Has current measurement
     80  *
     81  * Reimplemented from Bldc. \n
     82  *
     83  * \return true if it has current measurement
     84  */
     85  bool HasCurrentMeasurement(void) const { return true; };
    9186
    92         private:
    93             /*!
    94             * \brief Set motors values
    95             *
    96             * Reimplemented from Bldc. \n
    97             * Values size must be the same as MotorsCount()
    98             *
    99             * \param values motor values
    100             */
    101             void SetMotors(float* values);
     87private:
     88  /*!
     89  * \brief Set motors values
     90  *
     91  * Reimplemented from Bldc. \n
     92  * Values size must be the same as MotorsCount()
     93  *
     94  * \param values motor values
     95  */
     96  void SetMotors(float *values);
    10297
    103             class BlCtrlV2_impl* pimpl_;
    104     };
     98  class BlCtrlV2_impl *pimpl_;
     99};
    105100} // end namespace actuator
    106101} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp

    r3 r15  
    3535using namespace flair::actuator;
    3636
    37 BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2* self,Layout *layout,I2cPort* i2cport) {
    38     this->self=self;
    39         this->i2cport=i2cport;
    40         last_voltage_time=0;
    41 
    42     DetectMotors();
    43     //if(nb_mot!=self->MotorsCount()) self->Err("motors count different from multiplex count\n");
    44 
    45     //BatteryMonitor
    46     battery=new BatteryMonitor(layout->NewRow(),"battery");
    47 
    48     //user interface
    49     GroupBox *setupgroupbox=new GroupBox(layout->NewRow(),"Motors");
    50     poles=new SpinBox(setupgroupbox->NewRow(),"nb poles:",0,255,1,14);
    51 }
    52 
    53 BlCtrlV2_impl::~BlCtrlV2_impl() {
    54 
    55 }
    56 
    57 void BlCtrlV2_impl::SetMotors(float* value) {
    58     uint16_t tosend_value[nb_mot];
    59     //stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas bloquer sur le mutex de l'output)
    60     float speeds[nb_mot];
    61     float currents[nb_mot];
    62 
    63     for(int i=0;i<nb_mot;i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
    64 
    65     i2cport->GetMutex();
    66 
    67     for(int i=0;i<nb_mot;i++) {
    68         i2cport->SetSlave(BASE_ADDRESS+i);
    69         WriteValue(tosend_value[i]);
     37BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2 *self, Layout *layout, I2cPort *i2cport) {
     38  this->self = self;
     39  this->i2cport = i2cport;
     40  last_voltage_time = 0;
     41
     42  DetectMotors();
     43  // if(nb_mot!=self->MotorsCount()) self->Err("motors count different from
     44  // multiplex count\n");
     45
     46  // BatteryMonitor
     47  battery = new BatteryMonitor(layout->NewRow(), "battery");
     48
     49  // user interface
     50  GroupBox *setupgroupbox = new GroupBox(layout->NewRow(), "Motors");
     51  poles = new SpinBox(setupgroupbox->NewRow(), "nb poles:", 0, 255, 1, 14);
     52}
     53
     54BlCtrlV2_impl::~BlCtrlV2_impl() {}
     55
     56void BlCtrlV2_impl::SetMotors(float *value) {
     57  uint16_t tosend_value[nb_mot];
     58  // stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas
     59  // bloquer sur le mutex de l'output)
     60  float speeds[nb_mot];
     61  float currents[nb_mot];
     62
     63  for (int i = 0; i < nb_mot; i++)
     64    tosend_value[i] = (uint16_t)(MAX_VALUE * value[i]);
     65
     66  i2cport->GetMutex();
     67
     68  for (int i = 0; i < nb_mot; i++) {
     69    i2cport->SetSlave(BASE_ADDRESS + i);
     70    WriteValue(tosend_value[i]);
     71  }
     72
     73  for (int i = 0; i < nb_mot; i++) {
     74    i2cport->SetSlave(BASE_ADDRESS + i);
     75
     76    if (i == 0 && GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
     77      // toute les 5 secondes sur moteur 0
     78      float voltage;
     79      GetCurrentSpeedAndVoltage(currents[i], speeds[i], voltage);
     80      battery->SetBatteryValue(voltage);
     81      last_voltage_time = GetTime();
     82    } else {
     83      GetCurrentAndSpeed(currents[i], speeds[i]);
    7084    }
    71 
    72     for(int i=0;i<nb_mot;i++) {
    73         i2cport->SetSlave(BASE_ADDRESS+i);
    74 
    75         if(i==0 && GetTime()>(last_voltage_time+5*(Time)1000000000)) {
    76             //toute les 5 secondes sur moteur 0
    77             float voltage;
    78             GetCurrentSpeedAndVoltage(currents[i],speeds[i],voltage);
    79             battery->SetBatteryValue(voltage);
    80             last_voltage_time=GetTime();
    81         } else {
    82             GetCurrentAndSpeed(currents[i],speeds[i]);
    83         }
     85  }
     86  // printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
     87  /*
     88      if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
     89      {
     90          i2cport->SetSlave(BASE_ADDRESS);
     91          battery->SetBatteryValue(ReadVoltage());
     92          last_voltage_time=GetTime();
     93      }
     94  */
     95  i2cport->ReleaseMutex();
     96
     97  // on prend une fois pour toute le mutex et on fait des accès directs
     98  cvmatrix *output = self->output;
     99  output->GetMutex();
     100  for (int i = 0; i < nb_mot; i++)
     101    output->SetValueNoMutex(i, 0, speeds[i]);
     102  for (int i = 0; i < nb_mot; i++)
     103    output->SetValueNoMutex(i, 1, currents[i]);
     104  output->ReleaseMutex();
     105}
     106
     107// I2cPort mutex must be taken before calling this function
     108void BlCtrlV2_impl::WriteValue(uint16_t value) {
     109  uint8_t tx[2];
     110  ssize_t written;
     111
     112  tx[0] = (uint8_t)(value >> 3); // msb
     113  tx[1] = (value & 0x07);
     114  ; //+16+8; //pour recuperer la vitesse en premier
     115  written = i2cport->Write(tx, 2);
     116  if (written < 0) {
     117    self->Err("rt_dev_write error (%s)\n", strerror(-written));
     118  } else if (written != sizeof(tx)) {
     119    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
     120  }
     121}
     122
     123// I2cPort mutex must be taken before calling this function
     124void BlCtrlV2_impl::GetCurrentAndSpeed(float &current, float &speed) {
     125  ssize_t read;
     126  uint8_t value[4];
     127  read = i2cport->Read(value, sizeof(value));
     128
     129  if (read < 0) {
     130    self->Err("rt_dev_read error (%s)\n", strerror(-read));
     131    speed = -1;
     132    current = -1;
     133  } else if (read != sizeof(value)) {
     134    self->Err("rt_dev_read error %i/%i\n", read, sizeof(value));
     135    speed = -1;
     136    current = -1;
     137  } else {
     138    current = value[0] / 10.;
     139    speed = value[3] * 780. / poles->Value();
     140  }
     141}
     142
     143// I2cPort mutex must be taken before calling this function
     144void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current, float &speed,
     145                                              float &voltage) {
     146  ssize_t read;
     147  uint8_t value[6];
     148  read = i2cport->Read(value, sizeof(value));
     149
     150  if (read < 0) {
     151    self->Err("rt_dev_read error (%s)\n", strerror(-read));
     152    speed = -1;
     153    voltage = -1;
     154    current = -1;
     155  } else if (read != sizeof(value)) {
     156    self->Err("rt_dev_read error %i/%i\n", read, sizeof(value));
     157    speed = -1;
     158    voltage = -1;
     159    current = -1;
     160  } else {
     161    current = value[0] / 10.;
     162    voltage = value[5] / 10.;
     163    speed = value[3] * 780. / poles->Value();
     164  }
     165}
     166
     167void BlCtrlV2_impl::DetectMotors(void) {
     168  int nb = 0;
     169  ssize_t read, nb_write;
     170  uint8_t value[3];
     171  uint8_t tx[2];
     172
     173  i2cport->GetMutex();
     174
     175  for (int i = 0; i < MAX_MOTORS; i++) {
     176    // printf("test %i\n",i);
     177    i2cport->SetSlave(BASE_ADDRESS + i);
     178    tx[0] = 0;
     179    tx[1] = 16 + 8; // 16+8 pour recuperer la vitesse
     180
     181    nb_write = i2cport->Write(tx, 2);
     182
     183    if (nb_write != sizeof(tx)) {
     184      continue;
    84185    }
    85 //printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
    86 /*
    87     if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
    88     {
    89         i2cport->SetSlave(BASE_ADDRESS);
    90         battery->SetBatteryValue(ReadVoltage());
    91         last_voltage_time=GetTime();
     186    nb++;
     187  }
     188
     189  for (int i = 0; i < MAX_MOTORS; i++) {
     190    i2cport->SetSlave(BASE_ADDRESS + i);
     191    read = i2cport->Read(value, 3);
     192
     193    if (read != sizeof(value)) {
     194      continue;
    92195    }
    93 */
    94     i2cport->ReleaseMutex();
    95 
    96     //on prend une fois pour toute le mutex et on fait des accès directs
    97     cvmatrix* output=self->output;
    98     output->GetMutex();
    99     for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,speeds[i]);
    100     for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,1,currents[i]);
    101     output->ReleaseMutex();
    102 }
    103 
    104 //I2cPort mutex must be taken before calling this function
    105 void BlCtrlV2_impl::WriteValue(uint16_t value) {
    106     uint8_t tx[2];
    107     ssize_t written;
    108 
    109     tx[0]=(uint8_t)(value>>3);//msb
    110     tx[1]=(value&0x07);;//+16+8; //pour recuperer la vitesse en premier
    111     written =i2cport->Write(tx, 2);
    112     if(written<0) {
    113         self->Err("rt_dev_write error (%s)\n",strerror(-written));
    114     } else if (written != sizeof(tx)) {
    115         self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
    116     }
    117 }
    118 
    119 //I2cPort mutex must be taken before calling this function
    120 void BlCtrlV2_impl::GetCurrentAndSpeed(float &current,float &speed) {
    121     ssize_t read;
    122     uint8_t value[4];
    123     read = i2cport->Read(value, sizeof(value));
    124 
    125     if(read<0) {
    126         self->Err("rt_dev_read error (%s)\n",strerror(-read));
    127         speed=-1;
    128         current=-1;
    129     } else if (read != sizeof(value)) {
    130         self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
    131         speed=-1;
    132         current=-1;
    133     } else {
    134         current=value[0]/10.;
    135         speed= value[3]*780./poles->Value();
    136     }
    137 }
    138 
    139 //I2cPort mutex must be taken before calling this function
    140 void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage) {
    141     ssize_t read;
    142     uint8_t value[6];
    143     read = i2cport->Read(value, sizeof(value));
    144 
    145     if(read<0) {
    146         self->Err("rt_dev_read error (%s)\n",strerror(-read));
    147         speed=-1;
    148         voltage=-1;
    149         current=-1;
    150     } else if (read != sizeof(value)) {
    151         self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
    152         speed=-1;
    153         voltage=-1;
    154         current=-1;
    155     } else {
    156         current=value[0]/10.;
    157         voltage=value[5]/10.;
    158         speed= value[3]*780./poles->Value();
    159     }
    160 }
    161 
    162 void BlCtrlV2_impl::DetectMotors(void) {
    163     int nb=0;
    164     ssize_t read,nb_write;
    165     uint8_t value[3];
    166     uint8_t tx[2];
    167 
    168     i2cport->GetMutex();
    169 
    170     for(int i=0;i<MAX_MOTORS;i++) {
    171         //printf("test %i\n",i);
    172         i2cport->SetSlave(BASE_ADDRESS+i);
    173         tx[0]=0;
    174         tx[1]=16+8;//16+8 pour recuperer la vitesse
    175 
    176         nb_write = i2cport->Write(tx, 2);
    177 
    178         if (nb_write != sizeof(tx)) {
    179             continue;
    180         }
    181         nb++;
    182     }
    183 
    184     for(int i=0;i<MAX_MOTORS;i++) {
    185         i2cport->SetSlave(BASE_ADDRESS+i);
    186         read = i2cport->Read(value, 3);
    187 
    188         if (read != sizeof(value)) {
    189             continue;
    190         }
    191     }
    192 
    193     i2cport->ReleaseMutex();
    194 
    195     Printf("BlCtrlV2: Dectected motors: %i\n",nb);
    196     if(nb==4) {
    197         Printf("BlCtrlV2: Configuration X4\n");
    198     } else if(nb==8) {
    199         Printf("BlCtrlV2: Configuration X8\n");
    200     } else {
    201         //self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
    202     }
    203 
    204     nb_mot=nb;
    205 }
     196  }
     197
     198  i2cport->ReleaseMutex();
     199
     200  Printf("BlCtrlV2: Dectected motors: %i\n", nb);
     201  if (nb == 4) {
     202    Printf("BlCtrlV2: Configuration X4\n");
     203  } else if (nb == 8) {
     204    Printf("BlCtrlV2: Configuration X8\n");
     205  } else {
     206    // self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
     207  }
     208
     209  nb_mot = nb;
     210}

  • trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp

    r3 r15  
    3838using namespace flair::gui;
    3939
    40 namespace flair
    41 {
    42 namespace actuator
    43 {
    44 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)
    45 {
    46     this->i2cport=i2cport;
    47         slave_address=base_address;
    48         tested_motor=-1;
    49         enabled=false;
    50         int_av_g=0;
    51         int_av_d=0;
    52         int_ar_g=0;
    53         int_ar_d=0;
    54 
    55         //flight time
    56         FILE *file;
    57     file=fopen("/etc/flight_time","r");
    58     if (file == NULL)
    59     {
    60         Printf("fichier d'info de vol vide\n");
    61         time_sec=0;
     40namespace flair {
     41namespace actuator {
     42BlCtrlV2_x4_speed::BlCtrlV2_x4_speed(FrameworkManager *parent, string name,
     43                                     I2cPort *i2cport, uint8_t base_address,
     44                                     uint8_t priority)
     45    : Thread(parent, name, priority), IODevice(parent, name) {
     46  this->i2cport = i2cport;
     47  slave_address = base_address;
     48  tested_motor = -1;
     49  enabled = false;
     50  int_av_g = 0;
     51  int_av_d = 0;
     52  int_ar_g = 0;
     53  int_ar_d = 0;
     54
     55  // flight time
     56  FILE *file;
     57  file = fopen("/etc/flight_time", "r");
     58  if (file == NULL) {
     59    Printf("fichier d'info de vol vide\n");
     60    time_sec = 0;
     61  } else {
     62    char ligne[32];
     63    fgets(ligne, 32, file);
     64    time_sec = atoi(ligne);
     65    Printf("temps de vol total: %is = %imin = %ih\n", time_sec, time_sec / 60,
     66           time_sec / 3600);
     67    fclose(file);
     68  }
     69
     70  // station sol
     71  main_tab = new Tab(parent->GetTabWidget(), name);
     72  tab = new TabWidget(main_tab->NewRow(), name);
     73  Tab *sensor_tab = new Tab(tab, "Reglages");
     74  reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
     75  poles = new SpinBox(reglages_groupbox->NewRow(), "nb poles", 0, 255, 1);
     76  kp = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "kp", 0., 255,
     77                         0.001, 4);
     78  ki = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "ki", 0., 255,
     79                         0.001, 4);
     80  min = new SpinBox(reglages_groupbox->NewRow(), "min pwm", 0., 2048, 1);
     81  max =
     82      new SpinBox(reglages_groupbox->LastRowLastCol(), "max pwm", 0., 2048, 1);
     83  test = new SpinBox(reglages_groupbox->LastRowLastCol(), "test value", 0.,
     84                     2048, 1);
     85  start_value = new SpinBox(reglages_groupbox->NewRow(), "valeur demarrage", 0,
     86                            10000, 10);
     87  trim = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "pas decollage",
     88                           0, 1000, .1);
     89
     90  av_g = new ComboBox(reglages_groupbox->NewRow(), "avant gauche");
     91  av_g->AddItem("1");
     92  av_g->AddItem("2");
     93  av_g->AddItem("3");
     94  av_g->AddItem("4");
     95  button_avg = new PushButton(reglages_groupbox->LastRowLastCol(), "test avg");
     96
     97  av_d = new ComboBox(reglages_groupbox->LastRowLastCol(), "avant droite:");
     98  av_d->AddItem("1");
     99  av_d->AddItem("2");
     100  av_d->AddItem("3");
     101  av_d->AddItem("4");
     102  button_avd = new PushButton(reglages_groupbox->LastRowLastCol(), "test avd");
     103
     104  ar_g = new ComboBox(reglages_groupbox->NewRow(), "arriere gauche:");
     105  ar_g->AddItem("1");
     106  ar_g->AddItem("2");
     107  ar_g->AddItem("3");
     108  ar_g->AddItem("4");
     109  button_arg = new PushButton(reglages_groupbox->LastRowLastCol(), "test arg");
     110
     111  ar_d = new ComboBox(reglages_groupbox->LastRowLastCol(), "arriere droite:");
     112  ar_d->AddItem("1");
     113  ar_d->AddItem("2");
     114  ar_d->AddItem("3");
     115  ar_d->AddItem("4");
     116  button_ard = new PushButton(reglages_groupbox->LastRowLastCol(), "test ard");
     117
     118  pas = new ComboBox(reglages_groupbox->NewRow(), "pas helice avant gauche:");
     119  pas->AddItem("normal");
     120  pas->AddItem("inverse");
     121
     122  input = new cvmatrix((IODevice *)this, 8, 1, floatType);
     123
     124  cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 2);
     125  desc->SetElementName(0, 0, "avant gauche");
     126  desc->SetElementName(1, 0, "arriere droite");
     127  desc->SetElementName(2, 0, "avant droite");
     128  desc->SetElementName(3, 0, "arriere gauche");
     129
     130  desc->SetElementName(0, 1, "cons avant gauche");
     131  desc->SetElementName(1, 1, "cons arriere droite");
     132  desc->SetElementName(2, 1, "cons avant droite");
     133  desc->SetElementName(3, 1, "cons arriere gauche");
     134  output = new cvmatrix((IODevice *)this, desc, floatType);
     135
     136  /*
     137
     138  //le 3ieme lu est la tension batteire
     139          if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
     140      uint16_t pwm_moteur;
     141      pwm_moteur=0;
     142      ssize_t read;
     143      uint8_t rx[8];
     144      SetSlave(slave_address);
     145
     146      for(int j=0;j<10;j++)
     147      {
     148
     149
     150          WriteValue(pwm_moteur);
     151
     152
     153          read = rt_dev_read(i2c_fd, rx, sizeof(rx));
     154
     155          if(read<0)
     156          {
     157              rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur
     158  rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
     159          }
     160          else if (read != sizeof(rx))
     161          {
     162              rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur
     163  rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
     164
     165          }
     166          for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
     167
     168          printf("\n");
     169
     170      }
     171
     172      if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
     173}
     174
     175BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void) {
     176  SafeStop();
     177  Join();
     178  delete main_tab;
     179}
     180
     181void BlCtrlV2_x4_speed::UseDefaultPlot(void) {
     182  Tab *plot_tab = new Tab(tab, "Mesures");
     183  DataPlot1D *av_g_plot = new DataPlot1D(plot_tab->NewRow(), "avg", 0, 10000);
     184  av_g_plot->AddCurve(output->Element(0, 0));
     185  av_g_plot->AddCurve(output->Element(0, 1), DataPlot::Blue);
     186  DataPlot1D *av_d_plot =
     187      new DataPlot1D(plot_tab->LastRowLastCol(), "avd", 0, 10000);
     188  av_d_plot->AddCurve(output->Element(2, 0));
     189  av_d_plot->AddCurve(output->Element(2, 1), DataPlot::Blue);
     190  DataPlot1D *ar_g_plot = new DataPlot1D(plot_tab->NewRow(), "arg", 0, 10000);
     191  ar_g_plot->AddCurve(output->Element(3, 0));
     192  ar_g_plot->AddCurve(output->Element(3, 1), DataPlot::Blue);
     193  DataPlot1D *ar_d_plot =
     194      new DataPlot1D(plot_tab->LastRowLastCol(), "ard", 0, 10000);
     195  ar_d_plot->AddCurve(output->Element(1, 0));
     196  ar_d_plot->AddCurve(output->Element(1, 1), DataPlot::Blue);
     197}
     198
     199float BlCtrlV2_x4_speed::TrimValue(void) { return (float)trim->Value(); }
     200
     201int BlCtrlV2_x4_speed::StartValue(void) { return start_value->Value(); }
     202
     203void BlCtrlV2_x4_speed::Run(void) {
     204  WarnUponSwitches(true);
     205
     206  SetPeriodUS(TAU_US);
     207
     208  while (!ToBeStopped()) {
     209    WaitPeriod();
     210
     211    Update();
     212  }
     213
     214  WarnUponSwitches(false);
     215}
     216
     217void BlCtrlV2_x4_speed::Update(void) {
     218  float u_roll, u_pitch, u_yaw, u_gaz;
     219  float trim_roll, trim_pitch, trim_yaw;
     220  float pwm[4];
     221  uint16_t pwm_moteur[4];
     222
     223  // on prend une fois pour toute le mutex et on fait des accès directs
     224  input->GetMutex();
     225
     226  u_roll = input->ValueNoMutex(0, 0);
     227  u_pitch = input->ValueNoMutex(1, 0);
     228  u_yaw = input->ValueNoMutex(2, 0);
     229  u_gaz =
     230      input->ValueNoMutex(3, 0) +
     231      input->ValueNoMutex(7, 0) * input->ValueNoMutex(7, 0); // ugaz+trim*trim
     232  trim_roll = input->ValueNoMutex(4, 0);
     233  trim_pitch = input->ValueNoMutex(5, 0);
     234  trim_yaw = input->ValueNoMutex(6, 0);
     235
     236  input->ReleaseMutex();
     237
     238  if (pas->CurrentIndex() == 1) {
     239    trim_yaw = -trim_yaw;
     240    u_yaw = -u_yaw;
     241  }
     242
     243  // rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
     244  // if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
     245
     246  // avant gauche
     247  if (u_gaz + u_pitch + u_roll + u_yaw > 0) {
     248    pwm[0] = trim_pitch + trim_roll + trim_yaw +
     249             sqrtf(u_gaz + u_pitch + u_roll + u_yaw);
     250  } else {
     251    pwm[0] = trim_pitch + trim_roll + trim_yaw;
     252  }
     253
     254  // arriere gauche
     255  if (u_gaz - u_pitch + u_roll - u_yaw > 0) {
     256    pwm[3] = -trim_pitch + trim_roll - trim_yaw +
     257             sqrtf(u_gaz - u_pitch + u_roll - u_yaw);
     258  } else {
     259    pwm[3] = -trim_pitch + trim_roll - trim_yaw;
     260  }
     261
     262  // arriere droit
     263  if (u_gaz - u_pitch - u_roll + u_yaw > 0) {
     264    pwm[1] = -trim_pitch - trim_roll + trim_yaw +
     265             sqrtf(u_gaz - u_pitch - u_roll + u_yaw);
     266  } else {
     267    pwm[1] = -trim_pitch - trim_roll + trim_yaw;
     268  }
     269
     270  // avant droit
     271  if (u_gaz + u_pitch - u_roll - u_yaw > 0) {
     272    pwm[2] = trim_pitch - trim_roll - trim_yaw +
     273             sqrtf(u_gaz + u_pitch - u_roll - u_yaw);
     274  } else {
     275    pwm[2] = trim_pitch - trim_roll - trim_yaw;
     276  }
     277
     278  int_av_g += ki->Value() * (pwm[0] - speed_av_g);
     279  pwm[0] = kp->Value() * (pwm[0] - speed_av_g) + int_av_g;
     280
     281  int_ar_g += ki->Value() * (pwm[3] - speed_ar_g);
     282  pwm[3] = kp->Value() * (pwm[3] - speed_ar_g) + int_ar_g;
     283
     284  int_ar_d += ki->Value() * (pwm[1] - speed_ar_d);
     285  pwm[1] = kp->Value() * (pwm[1] - speed_ar_d) + int_ar_d;
     286
     287  int_av_d += ki->Value() * (pwm[2] - speed_av_d);
     288  pwm[2] = kp->Value() * (pwm[2] - speed_av_d) + int_av_d;
     289
     290  // rt_printf("%f\n",pwm[0]);
     291  for (int i = 0; i < 4; i++)
     292    pwm_moteur[i] = SatPWM(pwm[i], min->Value(), max->Value());
     293
     294  if (button_avg->Clicked() == true) {
     295    tested_motor = 0;
     296    StartTest();
     297  }
     298  if (button_avd->Clicked() == true) {
     299    tested_motor = 2;
     300    StartTest();
     301  }
     302  if (button_arg->Clicked() == true) {
     303    tested_motor = 3;
     304    StartTest();
     305  }
     306  if (button_ard->Clicked() == true) {
     307    tested_motor = 1;
     308    StartTest();
     309  }
     310
     311  if (tested_motor != -1) {
     312    for (int i = 0; i < 4; i++) {
     313      pwm_moteur[i] = 0;
    62314    }
    63     else
    64     {
    65         char ligne[32];
    66         fgets(ligne, 32, file);
    67         time_sec=atoi(ligne);
    68         Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    69         fclose(file);
     315    pwm_moteur[tested_motor] = (uint16_t)test->Value();
     316
     317    if (GetTime() > (start_time + 2 * 1000000000))
     318      StopTest();
     319  }
     320
     321  i2cport->GetMutex();
     322
     323  if (enabled == true) {
     324    i2cport->SetSlave(slave_address + av_g->CurrentIndex());
     325    WriteValue(pwm_moteur[0]);
     326
     327    i2cport->SetSlave(slave_address + av_d->CurrentIndex());
     328    WriteValue(pwm_moteur[2]);
     329
     330    i2cport->SetSlave(slave_address + ar_g->CurrentIndex());
     331    WriteValue(pwm_moteur[3]);
     332
     333    i2cport->SetSlave(slave_address + ar_d->CurrentIndex());
     334    WriteValue(pwm_moteur[1]);
     335
     336  } else {
     337    for (int i = 0; i < 4; i++) {
     338      i2cport->SetSlave(slave_address + i);
     339      WriteValue(0);
    70340    }
    71 
    72     //station sol
    73     main_tab=new Tab(parent->GetTabWidget(),name);
    74     tab=new TabWidget(main_tab->NewRow(),name);
    75         Tab *sensor_tab=new Tab(tab,"Reglages");
    76         reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
    77             poles=new SpinBox(reglages_groupbox->NewRow(),"nb poles",0,255,1);
    78             kp=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"kp",0.,255,0.001,4);
    79             ki=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"ki",0.,255,0.001,4);
    80             min=new SpinBox(reglages_groupbox->NewRow(),"min pwm",0.,2048,1);
    81             max=new SpinBox(reglages_groupbox->LastRowLastCol(),"max pwm",0.,2048,1);
    82             test=new SpinBox(reglages_groupbox->LastRowLastCol(),"test value",0.,2048,1);
    83             start_value=new SpinBox(reglages_groupbox->NewRow(),"valeur demarrage",0,10000,10);
    84             trim=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"pas decollage",0,1000,.1);
    85 
    86             av_g=new ComboBox(reglages_groupbox->NewRow(),"avant gauche");
    87             av_g->AddItem("1");
    88             av_g->AddItem("2");
    89             av_g->AddItem("3");
    90             av_g->AddItem("4");
    91             button_avg=new PushButton(reglages_groupbox->LastRowLastCol(),"test avg");
    92 
    93             av_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"avant droite:");
    94             av_d->AddItem("1");
    95             av_d->AddItem("2");
    96             av_d->AddItem("3");
    97             av_d->AddItem("4");
    98             button_avd=new PushButton(reglages_groupbox->LastRowLastCol(),"test avd");
    99 
    100             ar_g=new ComboBox(reglages_groupbox->NewRow(),"arriere gauche:");
    101             ar_g->AddItem("1");
    102             ar_g->AddItem("2");
    103             ar_g->AddItem("3");
    104             ar_g->AddItem("4");
    105             button_arg=new PushButton(reglages_groupbox->LastRowLastCol(),"test arg");
    106 
    107             ar_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"arriere droite:");
    108             ar_d->AddItem("1");
    109             ar_d->AddItem("2");
    110             ar_d->AddItem("3");
    111             ar_d->AddItem("4");
    112             button_ard=new PushButton(reglages_groupbox->LastRowLastCol(),"test ard");
    113 
    114             pas=new ComboBox(reglages_groupbox->NewRow(),"pas helice avant gauche:");
    115             pas->AddItem("normal");
    116             pas->AddItem("inverse");
    117 
    118     input=new cvmatrix((IODevice*)this,8,1,floatType);
    119 
    120     cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,2);
    121     desc->SetElementName(0,0,"avant gauche");
    122         desc->SetElementName(1,0,"arriere droite");
    123         desc->SetElementName(2,0,"avant droite");
    124         desc->SetElementName(3,0,"arriere gauche");
    125 
    126         desc->SetElementName(0,1,"cons avant gauche");
    127         desc->SetElementName(1,1,"cons arriere droite");
    128         desc->SetElementName(2,1,"cons avant droite");
    129         desc->SetElementName(3,1,"cons arriere gauche");
    130     output=new cvmatrix((IODevice*)this,desc,floatType);
    131 
    132 
    133 
    134 /*
    135 
    136 //le 3ieme lu est la tension batteire
    137         if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
    138     uint16_t pwm_moteur;
    139     pwm_moteur=0;
    140     ssize_t read;
    141     uint8_t rx[8];
    142     SetSlave(slave_address);
    143 
    144     for(int j=0;j<10;j++)
    145     {
    146 
    147 
    148         WriteValue(pwm_moteur);
    149 
    150 
    151         read = rt_dev_read(i2c_fd, rx, sizeof(rx));
    152 
    153         if(read<0)
    154         {
    155             rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
    156         }
    157         else if (read != sizeof(rx))
    158         {
    159             rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
    160 
    161         }
    162         for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
    163 
    164         printf("\n");
    165 
     341    int_av_g = 0;
     342    int_av_d = 0;
     343    int_ar_g = 0;
     344    int_ar_d = 0;
     345  }
     346
     347  i2cport->SetSlave(slave_address + av_g->CurrentIndex());
     348  speed_av_g = GetSpeed();
     349
     350  i2cport->SetSlave(slave_address + av_d->CurrentIndex());
     351  speed_av_d = GetSpeed();
     352
     353  i2cport->SetSlave(slave_address + ar_g->CurrentIndex());
     354  speed_ar_g = GetSpeed();
     355
     356  i2cport->SetSlave(slave_address + ar_d->CurrentIndex());
     357  speed_ar_d = GetSpeed();
     358
     359  i2cport->ReleaseMutex();
     360
     361  // on prend une fois pour toute le mutex et on fait des accès directs
     362  output->GetMutex();
     363  output->SetValueNoMutex(0, 0, speed_av_g);
     364  output->SetValueNoMutex(1, 0, speed_ar_d);
     365  output->SetValueNoMutex(2, 0, speed_av_d);
     366  output->SetValueNoMutex(3, 0, speed_ar_g);
     367  // rt_printf("%i %i %i
     368  // %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
     369  output->ReleaseMutex();
     370
     371  output->SetDataTime(GetTime());
     372  ProcessUpdate(output);
     373}
     374
     375void BlCtrlV2_x4_speed::StartTest(void) {
     376  start_time = GetTime();
     377  SetEnabled(true);
     378}
     379
     380void BlCtrlV2_x4_speed::StopTest(void) {
     381  SetEnabled(false);
     382  tested_motor = -1;
     383}
     384
     385uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons, uint16_t min, uint16_t max) {
     386  uint16_t sat_value = (uint16_t)vel_cons;
     387
     388  if (vel_cons > ((float)sat_value + 0.5))
     389    sat_value++;
     390
     391  if (vel_cons < (float)min)
     392    sat_value = min;
     393  if (vel_cons > (float)max)
     394    sat_value = max;
     395
     396  return sat_value;
     397}
     398
     399void BlCtrlV2_x4_speed::LockUserInterface(void) {
     400  reglages_groupbox->setEnabled(false);
     401}
     402
     403void BlCtrlV2_x4_speed::UnlockUserInterface(void) {
     404  reglages_groupbox->setEnabled(true);
     405}
     406
     407void BlCtrlV2_x4_speed::SetEnabled(bool status) {
     408  enabled = status;
     409  if (enabled == true) {
     410    LockUserInterface();
     411
     412    flight_start_time = GetTime();
     413  } else {
     414    UnlockUserInterface();
     415
     416    Time now = GetTime();
     417    int t_sec;
     418    FILE *file;
     419    char ligne[32];
     420
     421    t_sec = (now - flight_start_time) / 1000000000;
     422    time_sec += t_sec;
     423
     424    Printf("temps de vol: %is = %imin\n", t_sec, t_sec / 60);
     425    Printf("temps de vol total: %is = %imin = %ih\n", time_sec, time_sec / 60,
     426           time_sec / 3600);
     427
     428    file = fopen("/etc/flight_time", "w");
     429    if (file == NULL) {
     430      Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
     431    } else {
     432      sprintf(ligne, "%i", time_sec);
     433      fputs(ligne, file);
     434      fclose(file);
    166435    }
    167 
    168     if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
    169 }
    170 
    171 BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void)
    172 {
    173     SafeStop();
    174     Join();
    175     delete main_tab;
    176 }
    177 
    178 void BlCtrlV2_x4_speed::UseDefaultPlot(void)
    179 {
    180     Tab *plot_tab=new Tab(tab,"Mesures");
    181         DataPlot1D *av_g_plot=new DataPlot1D(plot_tab->NewRow(),"avg",0,10000);
    182             av_g_plot->AddCurve(output->Element(0,0));
    183             av_g_plot->AddCurve(output->Element(0,1),DataPlot::Blue);
    184         DataPlot1D *av_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"avd",0,10000);
    185             av_d_plot->AddCurve(output->Element(2,0));
    186             av_d_plot->AddCurve(output->Element(2,1),DataPlot::Blue);
    187         DataPlot1D *ar_g_plot=new DataPlot1D(plot_tab->NewRow(),"arg",0,10000);
    188             ar_g_plot->AddCurve(output->Element(3,0));
    189             ar_g_plot->AddCurve(output->Element(3,1),DataPlot::Blue);
    190         DataPlot1D *ar_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"ard",0,10000);
    191             ar_d_plot->AddCurve(output->Element(1,0));
    192             ar_d_plot->AddCurve(output->Element(1,1),DataPlot::Blue);
    193 }
    194 
    195 float BlCtrlV2_x4_speed::TrimValue(void)
    196 {
    197     return (float)trim->Value();
    198 }
    199 
    200 int BlCtrlV2_x4_speed::StartValue(void)
    201 {
    202     return start_value->Value();
    203 }
    204 
    205 void BlCtrlV2_x4_speed::Run(void)
    206 {
    207     WarnUponSwitches(true);
    208 
    209     SetPeriodUS(TAU_US);
    210 
    211     while(!ToBeStopped())
    212     {
    213         WaitPeriod();
    214 
    215         Update();
    216     }
    217 
    218     WarnUponSwitches(false);
    219 }
    220 
    221 void BlCtrlV2_x4_speed::Update(void)
    222 {
    223     float u_roll,u_pitch,u_yaw,u_gaz;
    224     float trim_roll,trim_pitch,trim_yaw;
    225     float pwm[4];
    226     uint16_t pwm_moteur[4];
    227 
    228 
    229     //on prend une fois pour toute le mutex et on fait des accès directs
    230     input->GetMutex();
    231 
    232     u_roll=input->ValueNoMutex(0,0);
    233     u_pitch=input->ValueNoMutex(1,0);
    234     u_yaw=input->ValueNoMutex(2,0);
    235     u_gaz=input->ValueNoMutex(3,0)+input->ValueNoMutex(7,0)*input->ValueNoMutex(7,0);//ugaz+trim*trim
    236     trim_roll=input->ValueNoMutex(4,0);
    237     trim_pitch=input->ValueNoMutex(5,0);
    238     trim_yaw=input->ValueNoMutex(6,0);
    239 
    240     input->ReleaseMutex();
    241 
    242     if(pas->CurrentIndex()==1)
    243     {
    244         trim_yaw=-trim_yaw;
    245         u_yaw=-u_yaw;
    246     }
    247 
    248     //rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
    249     //if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
    250 
    251     //avant gauche
    252     if(u_gaz+u_pitch+u_roll+u_yaw>0)
    253     {
    254         pwm[0]=trim_pitch+trim_roll+trim_yaw+sqrtf(u_gaz+u_pitch+u_roll+u_yaw);
    255     }else
    256     {
    257         pwm[0]=trim_pitch+trim_roll+trim_yaw;
    258     }
    259 
    260     //arriere gauche
    261     if(u_gaz-u_pitch+u_roll-u_yaw>0)
    262     {
    263         pwm[3]=-trim_pitch+trim_roll-trim_yaw+sqrtf(u_gaz-u_pitch+u_roll-u_yaw);
    264     }else
    265     {
    266         pwm[3]=-trim_pitch+trim_roll-trim_yaw;
    267     }
    268 
    269     //arriere droit
    270     if(u_gaz-u_pitch-u_roll+u_yaw>0)
    271     {
    272         pwm[1]=-trim_pitch-trim_roll+trim_yaw+sqrtf(u_gaz-u_pitch-u_roll+u_yaw);
    273     }else
    274     {
    275         pwm[1]=-trim_pitch-trim_roll+trim_yaw;
    276     }
    277 
    278     //avant droit
    279     if(u_gaz+u_pitch-u_roll-u_yaw>0)
    280     {
    281         pwm[2]=trim_pitch-trim_roll-trim_yaw+sqrtf(u_gaz+u_pitch-u_roll-u_yaw);
    282     }else
    283     {
    284         pwm[2]=trim_pitch-trim_roll-trim_yaw;
    285     }
    286 
    287 
    288     int_av_g+=ki->Value()*(pwm[0]-speed_av_g);
    289     pwm[0]=kp->Value()*(pwm[0]-speed_av_g)+int_av_g;
    290 
    291     int_ar_g+=ki->Value()*(pwm[3]-speed_ar_g);
    292     pwm[3]=kp->Value()*(pwm[3]-speed_ar_g)+int_ar_g;
    293 
    294     int_ar_d+=ki->Value()*(pwm[1]-speed_ar_d);
    295     pwm[1]=kp->Value()*(pwm[1]-speed_ar_d)+int_ar_d;
    296 
    297     int_av_d+=ki->Value()*(pwm[2]-speed_av_d);
    298     pwm[2]=kp->Value()*(pwm[2]-speed_av_d)+int_av_d;
    299 
    300 //rt_printf("%f\n",pwm[0]);
    301     for(int i=0;i<4;i++) pwm_moteur[i]=SatPWM(pwm[i],min->Value(),max->Value());
    302 
    303     if(button_avg->Clicked()==true)
    304     {
    305         tested_motor=0;
    306         StartTest();
    307     }
    308     if(button_avd->Clicked()==true)
    309     {
    310         tested_motor=2;
    311         StartTest();
    312     }
    313     if(button_arg->Clicked()==true)
    314     {
    315         tested_motor=3;
    316         StartTest();
    317     }
    318     if(button_ard->Clicked()==true)
    319     {
    320         tested_motor=1;
    321         StartTest();
    322     }
    323 
    324     if(tested_motor!=-1)
    325     {
    326         for(int i=0;i<4;i++)
    327         {
    328             pwm_moteur[i]=0;
    329         }
    330         pwm_moteur[tested_motor]=(uint16_t)test->Value();
    331 
    332         if(GetTime()>(start_time+2*1000000000)) StopTest();
    333     }
    334 
    335 
    336     i2cport->GetMutex();
    337 
    338     if(enabled==true)
    339     {
    340         i2cport->SetSlave(slave_address+av_g->CurrentIndex());
    341         WriteValue(pwm_moteur[0]);
    342 
    343         i2cport->SetSlave(slave_address+av_d->CurrentIndex());
    344         WriteValue(pwm_moteur[2]);
    345 
    346         i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
    347         WriteValue(pwm_moteur[3]);
    348 
    349         i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
    350         WriteValue(pwm_moteur[1]);
    351 
    352     }
    353     else
    354     {
    355         for(int i=0;i<4;i++)
    356         {
    357             i2cport->SetSlave(slave_address+i);
    358             WriteValue(0);
    359         }
    360         int_av_g=0;
    361         int_av_d=0;
    362         int_ar_g=0;
    363         int_ar_d=0;
    364     }
    365 
    366     i2cport->SetSlave(slave_address+av_g->CurrentIndex());
    367     speed_av_g=GetSpeed();
    368 
    369     i2cport->SetSlave(slave_address+av_d->CurrentIndex());
    370     speed_av_d=GetSpeed();
    371 
    372     i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
    373     speed_ar_g=GetSpeed();
    374 
    375     i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
    376     speed_ar_d=GetSpeed();
    377 
    378     i2cport->ReleaseMutex();
    379 
    380 
    381     //on prend une fois pour toute le mutex et on fait des accès directs
    382     output->GetMutex();
    383     output->SetValueNoMutex(0,0,speed_av_g);
    384     output->SetValueNoMutex(1,0,speed_ar_d);
    385     output->SetValueNoMutex(2,0,speed_av_d);
    386     output->SetValueNoMutex(3,0,speed_ar_g);
    387    // rt_printf("%i %i %i %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
    388     output->ReleaseMutex();
    389 
    390     output->SetDataTime(GetTime());
    391     ProcessUpdate(output);
    392 
    393 }
    394 
    395 void BlCtrlV2_x4_speed::StartTest(void)
    396 {
    397     start_time=GetTime();
    398     SetEnabled(true);
    399 }
    400 
    401 void BlCtrlV2_x4_speed::StopTest(void)
    402 {
    403     SetEnabled(false);
    404     tested_motor=-1;
    405 }
    406 
    407 uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons,uint16_t min,uint16_t max)
    408 {
    409   uint16_t sat_value=(uint16_t)vel_cons;
    410 
    411   if(vel_cons>((float)sat_value+0.5)) sat_value++;
    412 
    413   if(vel_cons<(float)min) sat_value=min;
    414   if(vel_cons>(float)max) sat_value=max;
    415 
    416   return sat_value;
    417 }
    418 
    419 void BlCtrlV2_x4_speed::LockUserInterface(void)
    420 {
    421     reglages_groupbox->setEnabled(false);
    422 }
    423 
    424 void BlCtrlV2_x4_speed::UnlockUserInterface(void)
    425 {
    426     reglages_groupbox->setEnabled(true);
    427 }
    428 
    429 void BlCtrlV2_x4_speed::SetEnabled(bool status)
    430 {
    431     enabled=status;
    432     if(enabled==true)
    433     {
    434         LockUserInterface();
    435 
    436         flight_start_time = GetTime();
    437     }
    438     else
    439     {
    440         UnlockUserInterface();
    441 
    442         Time now= GetTime();
    443         int t_sec;
    444         FILE *file;
    445         char ligne[32];
    446 
    447         t_sec=(now - flight_start_time)/1000000000;
    448         time_sec+=t_sec;
    449 
    450         Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
    451         Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    452 
    453         file=fopen("/etc/flight_time","w");
    454         if (file == NULL)
    455         {
    456             Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
    457         }
    458         else
    459         {
    460             sprintf(ligne,"%i",time_sec);
    461             fputs(ligne,file);
    462             fclose(file);
    463         }
    464     }
    465 }
    466 
    467 void BlCtrlV2_x4_speed::SetUroll(float value)
    468 {
    469     input->SetValue(0,0,value);
    470 }
    471 
    472 void BlCtrlV2_x4_speed::SetUpitch(float value)
    473 {
    474     input->SetValue(1,0,value);
    475 }
    476 
    477 void BlCtrlV2_x4_speed::SetUyaw(float value)
    478 {
    479     input->SetValue(2,0,value);
    480 }
    481 
    482 void BlCtrlV2_x4_speed::SetUgaz(float value)
    483 {
    484     input->SetValue(3,0,value);
    485 }
    486 
    487 void BlCtrlV2_x4_speed::SetRollTrim(float value)
    488 {
    489     input->SetValue(4,0,value);
    490 }
    491 
    492 void BlCtrlV2_x4_speed::SetPitchTrim(float value)
    493 {
    494     input->SetValue(5,0,value);
    495 }
    496 
    497 void BlCtrlV2_x4_speed::SetYawTrim(float value)
    498 {
    499     input->SetValue(6,0,value);
    500 }
    501 
    502 void BlCtrlV2_x4_speed::SetGazTrim(float value)
    503 {
    504     input->SetValue(7,0,value);
    505 }
    506 
    507 void BlCtrlV2_x4_speed::WriteValue(uint16_t value)
    508 {
    509     unsigned char tx[2];
    510     ssize_t written;
    511 
    512     tx[0]=(unsigned char)(value>>3);//msb
    513     tx[1]=16+8+(value&0x07);//16+8 pour recuperer la vitesse
    514     written = i2cport->Write(tx, 2);
    515     if(written<0)
    516     {
    517         Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
    518     }
    519     else if (written != 2)
    520     {
    521         Thread::Err("erreur rt_dev_write %i/2\n",written);
    522     }
    523 }
    524 
    525 float BlCtrlV2_x4_speed::GetSpeed(void)
    526 {
    527     ssize_t read;
    528     uint8_t value;
    529     read = i2cport->Read(&value, 1);
    530 
    531     if(read<0)
    532     {
    533         Thread::Err("erreur rt_dev_read (%s)\n",strerror(-read));
    534     }
    535     else if (read != 1)
    536     {
    537         Thread::Err("erreur rt_dev_read %i/2\n",read);
    538 
    539     }
    540 
    541     return value*780./poles->Value();
     436  }
     437}
     438
     439void BlCtrlV2_x4_speed::SetUroll(float value) { input->SetValue(0, 0, value); }
     440
     441void BlCtrlV2_x4_speed::SetUpitch(float value) { input->SetValue(1, 0, value); }
     442
     443void BlCtrlV2_x4_speed::SetUyaw(float value) { input->SetValue(2, 0, value); }
     444
     445void BlCtrlV2_x4_speed::SetUgaz(float value) { input->SetValue(3, 0, value); }
     446
     447void BlCtrlV2_x4_speed::SetRollTrim(float value) {
     448  input->SetValue(4, 0, value);
     449}
     450
     451void BlCtrlV2_x4_speed::SetPitchTrim(float value) {
     452  input->SetValue(5, 0, value);
     453}
     454
     455void BlCtrlV2_x4_speed::SetYawTrim(float value) {
     456  input->SetValue(6, 0, value);
     457}
     458
     459void BlCtrlV2_x4_speed::SetGazTrim(float value) {
     460  input->SetValue(7, 0, value);
     461}
     462
     463void BlCtrlV2_x4_speed::WriteValue(uint16_t value) {
     464  unsigned char tx[2];
     465  ssize_t written;
     466
     467  tx[0] = (unsigned char)(value >> 3); // msb
     468  tx[1] = 16 + 8 + (value & 0x07);     // 16+8 pour recuperer la vitesse
     469  written = i2cport->Write(tx, 2);
     470  if (written < 0) {
     471    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     472  } else if (written != 2) {
     473    Thread::Err("erreur rt_dev_write %i/2\n", written);
     474  }
     475}
     476
     477float BlCtrlV2_x4_speed::GetSpeed(void) {
     478  ssize_t read;
     479  uint8_t value;
     480  read = i2cport->Read(&value, 1);
     481
     482  if (read < 0) {
     483    Thread::Err("erreur rt_dev_read (%s)\n", strerror(-read));
     484  } else if (read != 1) {
     485    Thread::Err("erreur rt_dev_read %i/2\n", read);
     486  }
     487
     488  return value * 780. / poles->Value();
    542489}
    543490

  • trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h

    r3 r15  
    1616/*********************************************************************/
    1717
    18 
    1918#ifndef BLCTRLV2_X4_SPEED_H
    2019#define BLCTRLV2_X4_SPEED_H
    … …  
    2322#include <Thread.h>
    2423
    25 namespace flair
    26 {
    27     namespace core
    28     {
    29         class cvmatrix;
    30         class FrameworkManager;
    31         class I2cPort;
    32 
    33     }
    34     namespace gui
    35     {
    36         class TabWidget;
    37         class Tab;
    38         class SpinBox;
    39         class DoubleSpinBox;
    40         class ComboBox;
    41         class PushButton;
    42         class GroupBox;
    43     }
     24namespace flair {
     25namespace core {
     26class cvmatrix;
     27class FrameworkManager;
     28class I2cPort;
     29}
     30namespace gui {
     31class TabWidget;
     32class Tab;
     33class SpinBox;
     34class DoubleSpinBox;
     35class ComboBox;
     36class PushButton;
     37class GroupBox;
     38}
    4439}
    4540
    46 namespace flair
    47 {
    48 namespace actuator
    49 {
    50     class BlCtrlV2_x4_speed : public core::Thread,public core::IODevice
    51     {
     41namespace flair {
     42namespace actuator {
     43class BlCtrlV2_x4_speed : public core::Thread, public core::IODevice {
    5244
    53         public:
    54             BlCtrlV2_x4_speed(core::FrameworkManager* parent,std::string name,core::I2cPort* i2cport,uint8_t base_address,uint8_t priority);
    55             ~BlCtrlV2_x4_speed();
    56             void UseDefaultPlot(void);
    57             void LockUserInterface(void);
    58             void UnlockUserInterface(void);
    59             void SetEnabled(bool status);
    60             void SetUroll(float value);
    61             void SetUpitch(float value);
    62             void SetUyaw(float value);
    63             void SetUgaz(float value);
    64             void SetRollTrim(float value);
    65             void SetPitchTrim(float value);
    66             void SetYawTrim(float value);
    67             void SetGazTrim(float value);
    68             float TrimValue(void);
    69             int StartValue(void);
     45public:
     46  BlCtrlV2_x4_speed(core::FrameworkManager *parent, std::string name,
     47                    core::I2cPort *i2cport, uint8_t base_address,
     48                    uint8_t priority);
     49  ~BlCtrlV2_x4_speed();
     50  void UseDefaultPlot(void);
     51  void LockUserInterface(void);
     52  void UnlockUserInterface(void);
     53  void SetEnabled(bool status);
     54  void SetUroll(float value);
     55  void SetUpitch(float value);
     56  void SetUyaw(float value);
     57  void SetUgaz(float value);
     58  void SetRollTrim(float value);
     59  void SetPitchTrim(float value);
     60  void SetYawTrim(float value);
     61  void SetGazTrim(float value);
     62  float TrimValue(void);
     63  int StartValue(void);
    7064
    71         private:
    72             /*!
    73             * \brief Update using provided datas
    74             *
    75             * Reimplemented from IODevice.
    76             *
    77             * \param data data from the parent to process
    78             */
    79             void UpdateFrom(core::io_data *data){};
    80             void WriteValue(uint16_t value);
    81             float GetSpeed(void);
    82             void StartTest(void);
    83             void StopTest(void);
    84             /*!
    85             * \brief Run function
    86             *
    87             * Reimplemented from Thread.
    88             *
    89             */
    90             void Run(void);
    91             void Update(void);
    92             gui::Tab* main_tab;
    93             gui::TabWidget* tab;
    94             gui::GroupBox* reglages_groupbox;
    95             gui::SpinBox *min,*max,*test;
    96             gui::PushButton *button_avg,*button_avd,*button_arg,*button_ard;
    97             gui::ComboBox *av_g,*av_d,*ar_g,*ar_d,*pas;
    98             gui::DoubleSpinBox *trim,*kp,*ki;
    99             gui::SpinBox *start_value,*poles;
    100             core::Time start_time,flight_start_time;
    101             int time_sec;
    102             float speed_av_g,speed_av_d,speed_ar_g,speed_ar_d;
    103             float int_av_g,int_av_d,int_ar_g,int_ar_d;
     65private:
     66  /*!
     67  * \brief Update using provided datas
     68  *
     69  * Reimplemented from IODevice.
     70  *
     71  * \param data data from the parent to process
     72  */
     73  void UpdateFrom(core::io_data *data){};
     74  void WriteValue(uint16_t value);
     75  float GetSpeed(void);
     76  void StartTest(void);
     77  void StopTest(void);
     78  /*!
     79  * \brief Run function
     80  *
     81  * Reimplemented from Thread.
     82  *
     83  */
     84  void Run(void);
     85  void Update(void);
     86  gui::Tab *main_tab;
     87  gui::TabWidget *tab;
     88  gui::GroupBox *reglages_groupbox;
     89  gui::SpinBox *min, *max, *test;
     90  gui::PushButton *button_avg, *button_avd, *button_arg, *button_ard;
     91  gui::ComboBox *av_g, *av_d, *ar_g, *ar_d, *pas;
     92  gui::DoubleSpinBox *trim, *kp, *ki;
     93  gui::SpinBox *start_value, *poles;
     94  core::Time start_time, flight_start_time;
     95  int time_sec;
     96  float speed_av_g, speed_av_d, speed_ar_g, speed_ar_d;
     97  float int_av_g, int_av_d, int_ar_g, int_ar_d;
    10498
    105             //matrix
    106             core::cvmatrix *input;
    107             core::cvmatrix *output;
     99  // matrix
     100  core::cvmatrix *input;
     101  core::cvmatrix *output;
    108102
    109             int tested_motor;
    110             core::I2cPort* i2cport;
    111             uint8_t slave_address;
     103  int tested_motor;
     104  core::I2cPort *i2cport;
     105  uint8_t slave_address;
    112106
    113             bool enabled;
     107  bool enabled;
    114108
    115             uint16_t SatPWM(float vel_cons,uint16_t min,uint16_t max);
    116     };
     109  uint16_t SatPWM(float vel_cons, uint16_t min, uint16_t max);
     110};
    117111} // end namespace actuator
    118112} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/Bldc.cpp

    r3 r15  
    2727using namespace flair::gui;
    2828
    29 namespace flair { namespace actuator {
     29namespace flair {
     30namespace actuator {
    3031
    31 Bldc::Bldc(const IODevice* parent,Layout* layout,string name,uint8_t motors_count) : IODevice(parent,name) {
    32     pimpl_=new Bldc_impl(this,layout,name,motors_count);
     32Bldc::Bldc(const IODevice *parent, Layout *layout, string name,
     33           uint8_t motors_count)
     34    : IODevice(parent, name) {
     35  pimpl_ = new Bldc_impl(this, layout, name, motors_count);
    3336
    34     cvmatrix_descriptor* desc=new cvmatrix_descriptor(motors_count,2);
    35     for(int i=0;i<motors_count;i++) {
    36         ostringstream speed,current;
    37         speed << "speed_" << i;
    38         desc->SetElementName(i,0,speed.str());
     37  cvmatrix_descriptor *desc = new cvmatrix_descriptor(motors_count, 2);
     38  for (int i = 0; i < motors_count; i++) {
     39    ostringstream speed, current;
     40    speed << "speed_" << i;
     41    desc->SetElementName(i, 0, speed.str());
    3942
    40         current << "current_" << i;
    41         desc->SetElementName(i,1,current.str());
    42     }
     43    current << "current_" << i;
     44    desc->SetElementName(i, 1, current.str());
     45  }
    4346
    44     output=new cvmatrix(this,desc,floatType);
    45     AddDataToLog(output);
     47  output = new cvmatrix(this, desc, floatType);
     48  AddDataToLog(output);
    4649}
    4750
    48 Bldc::Bldc(const Object* parent,string name,uint8_t motors_count) : IODevice(parent,name) {
    49     pimpl_=new Bldc_impl(this,motors_count);
     51Bldc::Bldc(const Object *parent, string name, uint8_t motors_count)
     52    : IODevice(parent, name) {
     53  pimpl_ = new Bldc_impl(this, motors_count);
    5054}
    5155
    52 Bldc::~Bldc() {
    53     delete pimpl_;
     56Bldc::~Bldc() { delete pimpl_; }
     57
     58void Bldc::UpdateFrom(const io_data *data) { pimpl_->UpdateFrom(data); }
     59
     60void Bldc::LockUserInterface(void) const { pimpl_->LockUserInterface(); }
     61
     62void Bldc::UnlockUserInterface(void) const { pimpl_->UnlockUserInterface(); }
     63
     64Layout *Bldc::GetLayout(void) const { return (Layout *)pimpl_->layout; }
     65
     66void Bldc::UseDefaultPlot(TabWidget *tabwidget) {
     67  pimpl_->UseDefaultPlot(tabwidget);
    5468}
    5569
    56 void Bldc::UpdateFrom(const io_data *data) {
    57     pimpl_->UpdateFrom(data);
     70uint8_t Bldc::MotorsCount(void) const { return pimpl_->motors_count; }
     71
     72cvmatrix *Bldc::Output(void) const { return output; }
     73
     74bool Bldc::AreEnabled(void) const { return pimpl_->are_enabled; }
     75
     76void Bldc::SetEnabled(bool status) {
     77  if (pimpl_->are_enabled != status) {
     78    pimpl_->are_enabled = status;
     79    if (pimpl_->are_enabled) {
     80      LockUserInterface();
     81    } else {
     82      UnlockUserInterface();
     83    }
     84  }
    5885}
    5986
    60 void Bldc::LockUserInterface(void) const {
    61     pimpl_->LockUserInterface();
    62 }
    63 
    64 void Bldc::UnlockUserInterface(void) const {
    65     pimpl_->UnlockUserInterface();
    66 }
    67 
    68 Layout* Bldc::GetLayout(void) const {
    69     return (Layout*)pimpl_->layout;
    70 }
    71 
    72 void Bldc::UseDefaultPlot(TabWidget* tabwidget) {
    73     pimpl_->UseDefaultPlot(tabwidget);
    74 }
    75 
    76 uint8_t Bldc::MotorsCount(void) const {
    77     return pimpl_->motors_count;
    78 }
    79 
    80 cvmatrix *Bldc::Output(void) const {
    81     return output;
    82 }
    83 
    84 bool Bldc::AreEnabled(void) const {
    85     return pimpl_->are_enabled;
    86 }
    87 
    88 void Bldc::SetEnabled(bool status) {
    89     if(pimpl_->are_enabled!=status) {
    90         pimpl_->are_enabled=status;
    91         if(pimpl_->are_enabled) {
    92             LockUserInterface();
    93         } else {
    94             UnlockUserInterface();
    95         }
    96     }
    97 }
    98 
    99 void Bldc::SetPower(int motor_id,float value) {
    100     //use output mutex to avoid making a new mutex
    101     output->GetMutex();
    102     pimpl_->power[motor_id]=value;
    103     output->ReleaseMutex();
     87void Bldc::SetPower(int motor_id, float value) {
     88  // use output mutex to avoid making a new mutex
     89  output->GetMutex();
     90  pimpl_->power[motor_id] = value;
     91  output->ReleaseMutex();
    10492}
    10593

  • trunk/lib/FlairSensorActuator/src/Bldc.h

    r3 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class cvmatrix;
    25     }
    26     namespace gui
    27     {
    28         class Layout;
    29         class TabWidget;
    30     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class cvmatrix;
     23}
     24namespace gui {
     25class Layout;
     26class TabWidget;
     27}
    3128}
    3229
    3330class Bldc_impl;
    3431
    35 namespace flair
    36 {
    37 namespace actuator
    38 {
    39     /*! \class Bldc
    40     *
    41     * \brief Base class for brushless motors drivers
    42     */
    43     class Bldc : public core::IODevice
    44     {
    45         friend class ::Bldc_impl;
     32namespace flair {
     33namespace actuator {
     34/*! \class Bldc
     35*
     36* \brief Base class for brushless motors drivers
     37*/
     38class Bldc : public core::IODevice {
     39  friend class ::Bldc_impl;
    4640
    47         public:
    48             /*!
    49             * \brief Constructor
    50             *
    51             * Construct a Bldc.
    52             *
    53             * \param parent parent
    54             * \param layout layout
    55             * \param name name
    56             * \param motors_count number of motors
    57             */
    58             Bldc(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count);
     41public:
     42  /*!
     43  * \brief Constructor
     44  *
     45  * Construct a Bldc.
     46  *
     47  * \param parent parent
     48  * \param layout layout
     49  * \param name name
     50  * \param motors_count number of motors
     51  */
     52  Bldc(const core::IODevice *parent, gui::Layout *layout, std::string name,
     53       uint8_t motors_count);
    5954
    60             /*!
    61             * \brief Constructor
    62             *
    63             * Construct a Bldc. \n
    64             * This contructor must only be called for a simulated device.
    65             *
    66             * \param parent parent
    67             * \param name name
    68             * \param motors_count number of motors
    69             */
    70             Bldc(const core::Object* parent,std::string name,uint8_t motors_count);
     55  /*!
     56  * \brief Constructor
     57  *
     58  * Construct a Bldc. \n
     59  * This contructor must only be called for a simulated device.
     60  *
     61  * \param parent parent
     62  * \param name name
     63  * \param motors_count number of motors
     64  */
     65  Bldc(const core::Object *parent, std::string name, uint8_t motors_count);
    7166
    72             /*!
    73             * \brief Destructor
    74             *
    75             */
    76             ~Bldc();
     67  /*!
     68  * \brief Destructor
     69  *
     70  */
     71  ~Bldc();
    7772
    78             /*!
    79             * \brief Lock user interface
    80             *
    81             */
    82             void LockUserInterface(void) const;
     73  /*!
     74  * \brief Lock user interface
     75  *
     76  */
     77  void LockUserInterface(void) const;
    8378
    84             /*!
    85             * \brief Unlock user interface
    86             *
    87             */
    88             void UnlockUserInterface(void) const;
     79  /*!
     80  * \brief Unlock user interface
     81  *
     82  */
     83  void UnlockUserInterface(void) const;
    8984
    90             /*!
    91             * \brief Use default plot
    92             *
    93             * \param tabwidget TabWidget to draw plots
    94             */
    95             void UseDefaultPlot(gui::TabWidget* tabwidget);
     85  /*!
     86  * \brief Use default plot
     87  *
     88  * \param tabwidget TabWidget to draw plots
     89  */
     90  void UseDefaultPlot(gui::TabWidget *tabwidget);
    9691
    97             /*!
    98             * \brief Output from motors
    99             *
    100             * First column is real speed if available, secund column is current if available
    101             *
    102             */
    103             core::cvmatrix *Output(void) const;
     92  /*!
     93  * \brief Output from motors
     94  *
     95  * First column is real speed if available, secund column is current if
     96  *available
     97  *
     98  */
     99  core::cvmatrix *Output(void) const;
    104100
    105             /*!
    106             * \brief Motors count
    107             *
    108             * \return number of motors
    109             */
    110             uint8_t MotorsCount(void) const;
     101  /*!
     102  * \brief Motors count
     103  *
     104  * \return number of motors
     105  */
     106  uint8_t MotorsCount(void) const;
    111107
    112             /*!
    113             * \brief Enable motors
    114             *
    115             * \param true to enable all motors
    116             */
    117             void SetEnabled(bool status);
     108  /*!
     109  * \brief Enable motors
     110  *
     111  * \param true to enable all motors
     112  */
     113  void SetEnabled(bool status);
    118114
    119             /*!
    120             * \brief Are motors enabled?
    121             *
    122             * \return true if motors are enabled
    123             */
    124             bool AreEnabled(void) const;
     115  /*!
     116  * \brief Are motors enabled?
     117  *
     118  * \return true if motors are enabled
     119  */
     120  bool AreEnabled(void) const;
    125121
    126             /*!
    127             * \brief Set motor power
    128             *
    129             * Changes the power (from 0 to 1) of a specific motor. \n
    130             * By default power is set to 1 for each motor which has no effect. \n
    131             * A value <1 will decrease the power of a motor sent to the reimplemented Bldc class through SetMotors. \n
    132             * The power value is applied after applying saturation between min value and max value.
    133             * So the resulting value cannot be higher than max value
    134             * but it can be lower than min value.
    135             *
    136             * \param motor_id id of the motor
    137             * \param value power value (from 0 to 1)
    138             *
    139             */
    140             void SetPower(int motor_id,float value);
     122  /*!
     123  * \brief Set motor power
     124  *
     125  * Changes the power (from 0 to 1) of a specific motor. \n
     126  * By default power is set to 1 for each motor which has no effect. \n
     127  * A value <1 will decrease the power of a motor sent to the reimplemented Bldc
     128  *class through SetMotors. \n
     129  * The power value is applied after applying saturation between min value and
     130  *max value.
     131  * So the resulting value cannot be higher than max value
     132  * but it can be lower than min value.
     133  *
     134  * \param motor_id id of the motor
     135  * \param value power value (from 0 to 1)
     136  *
     137  */
     138  void SetPower(int motor_id, float value);
    141139
    142             /*!
    143             * \brief Layout
    144             *
    145             * This the same Layout as passed to the constructor
    146             *
    147             * \return a Layout
    148             */
    149             gui::Layout* GetLayout(void) const;
     140  /*!
     141  * \brief Layout
     142  *
     143  * This the same Layout as passed to the constructor
     144  *
     145  * \return a Layout
     146  */
     147  gui::Layout *GetLayout(void) const;
    150148
    151             /*!
    152             * \brief Has speed measurement
    153             *
    154             * \return true if it has speed measurement
    155             */
    156             virtual bool HasSpeedMeasurement(void) const=0;
     149  /*!
     150  * \brief Has speed measurement
     151  *
     152  * \return true if it has speed measurement
     153  */
     154  virtual bool HasSpeedMeasurement(void) const = 0;
    157155
    158             /*!
    159             * \brief Has current measurement
    160             *
    161             * \return true if it has current measurement
    162             */
    163             virtual bool HasCurrentMeasurement(void) const=0;
     156  /*!
     157  * \brief Has current measurement
     158  *
     159  * \return true if it has current measurement
     160  */
     161  virtual bool HasCurrentMeasurement(void) const = 0;
    164162
    165         protected:
    166             core::cvmatrix *output;
     163protected:
     164  core::cvmatrix *output;
    167165
    168         private:
    169             /*!
    170             * \brief Update using provided datas
    171             *
    172             * Reimplemented from IODevice.
    173             *
    174             * \param data data from the parent to process
    175             */
    176             void UpdateFrom(const core::io_data *data);
     166private:
     167  /*!
     168  * \brief Update using provided datas
     169  *
     170  * Reimplemented from IODevice.
     171  *
     172  * \param data data from the parent to process
     173  */
     174  void UpdateFrom(const core::io_data *data);
    177175
    178             /*!
    179             * \brief Set motors values
    180             *
    181             * values size must be the same as MotorsCount()
    182             *
    183             * \param values set motors values
    184             */
    185             virtual void SetMotors(float* values)=0;
     176  /*!
     177  * \brief Set motors values
     178  *
     179  * values size must be the same as MotorsCount()
     180  *
     181  * \param values set motors values
     182  */
     183  virtual void SetMotors(float *values) = 0;
    186184
    187             class Bldc_impl* pimpl_;
    188     };
     185  class Bldc_impl *pimpl_;
     186};
    189187} // end namespace actuator
    190188} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp

    r3 r15  
    3535using namespace flair::actuator;
    3636
    37 Bldc_impl::Bldc_impl(Bldc* self,Layout* layout,string name,uint8_t motors_count) {
    38     this->self=self;
    39     this->motors_count=motors_count;
    40     this->layout=layout;
    41         are_enabled=false;
    42         is_running=false;
    43         tested_motor=-1;
    44 
    45     values=(float*)malloc(motors_count*sizeof(float));
    46     power=(float*)malloc(motors_count*sizeof(float));
    47     for(int i=0;i<motors_count;i++) power[i]=1;
    48 
    49     //station sol
    50         GroupBox* groupbox=new GroupBox(layout->NewRow(),"bldc");
    51         flight_time=new Label(groupbox->NewRow(),"flight time");
    52         min_value=new DoubleSpinBox(groupbox->NewRow(),"min value:",0,1,.1,2);
    53         max_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"max value:",0,1,.1,2);
    54         test_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"test value:",0,1,0.1);
    55 
    56     int index=0;
    57     button_test=(PushButton**)malloc(motors_count*sizeof(PushButton*));
    58     for(int i=0;i<motors_count/2;i++) {
    59         for(int j=0;j<2;j++) {
    60             ostringstream test_name;
    61             test_name << "test motor " << index;
    62             button_test[index]=new PushButton(groupbox->At(2+i,j),test_name.str());
    63             index++;
    64         }
    65     }
    66 
    67         //flight time
    68         FILE *file;
    69     file=fopen("/etc/flight_time","r");
     37Bldc_impl::Bldc_impl(Bldc *self, Layout *layout, string name,
     38                     uint8_t motors_count) {
     39  this->self = self;
     40  this->motors_count = motors_count;
     41  this->layout = layout;
     42  are_enabled = false;
     43  is_running = false;
     44  tested_motor = -1;
     45
     46  values = (float *)malloc(motors_count * sizeof(float));
     47  power = (float *)malloc(motors_count * sizeof(float));
     48  for (int i = 0; i < motors_count; i++)
     49    power[i] = 1;
     50
     51  // station sol
     52  GroupBox *groupbox = new GroupBox(layout->NewRow(), "bldc");
     53  flight_time = new Label(groupbox->NewRow(), "flight time");
     54  min_value = new DoubleSpinBox(groupbox->NewRow(), "min value:", 0, 1, .1, 2);
     55  max_value =
     56      new DoubleSpinBox(groupbox->LastRowLastCol(), "max value:", 0, 1, .1, 2);
     57  test_value =
     58      new DoubleSpinBox(groupbox->LastRowLastCol(), "test value:", 0, 1, 0.1);
     59
     60  int index = 0;
     61  button_test = (PushButton **)malloc(motors_count * sizeof(PushButton *));
     62  for (int i = 0; i < motors_count / 2; i++) {
     63    for (int j = 0; j < 2; j++) {
     64      ostringstream test_name;
     65      test_name << "test motor " << index;
     66      button_test[index] =
     67          new PushButton(groupbox->At(2 + i, j), test_name.str());
     68      index++;
     69    }
     70  }
     71
     72  // flight time
     73  FILE *file;
     74  file = fopen("/etc/flight_time", "r");
     75  if (file == NULL) {
     76    time_sec = 0;
     77  } else {
     78    char ligne[32];
     79    fgets(ligne, 32, file);
     80    time_sec = atoi(ligne);
     81    fclose(file);
     82  }
     83  flight_time->SetText("total flight time: %is = %imin = %ih\n", time_sec,
     84                       time_sec / 60, time_sec / 3600);
     85}
     86
     87Bldc_impl::Bldc_impl(Bldc *self, uint8_t motors_count) {
     88  this->self = self;
     89  this->motors_count = motors_count;
     90  values = NULL;
     91  button_test = NULL;
     92  power = NULL;
     93}
     94
     95Bldc_impl::~Bldc_impl() {
     96  if (values != NULL)
     97    free(values);
     98  if (button_test != NULL)
     99    free(button_test);
     100  if (power != NULL)
     101    free(power);
     102}
     103
     104void Bldc_impl::UseDefaultPlot(TabWidget *tab) {
     105  Tab *plot_tab = new Tab(tab, "speeds");
     106  plots = new DataPlot1D(plot_tab->NewRow(), "values", 0, 1);
     107  for (int i = 0; i < motors_count; i++) {
     108    plots->AddCurve(self->output->Element(i));
     109  }
     110}
     111
     112void Bldc_impl::UpdateFrom(const io_data *data) {
     113  cvmatrix *input = (cvmatrix *)data;
     114  bool is_motor_running = false;
     115
     116  if (values == NULL)
     117    return; // nothing to do in simulator
     118
     119  if (input->Rows() != motors_count) {
     120    self->Err("nb motors mismatch\n");
     121    return;
     122  }
     123
     124  input->GetMutex();
     125  for (int i = 0; i < motors_count; i++) {
     126    if (input->ValueNoMutex(i, 0) != 0)
     127      is_motor_running = true;
     128    if (are_enabled) {
     129      // Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
     130      values[i] = power[i] * Sat(input->ValueNoMutex(i, 0));
     131      // Printf("%i %f\n",i,values[i]);
     132    } else {
     133      values[i] = 0;
     134    }
     135  }
     136  input->ReleaseMutex();
     137
     138  if (are_enabled && is_motor_running && !is_running) {
     139    flight_start_time = GetTime();
     140    is_running = true;
     141  }
     142  if ((!are_enabled || !is_motor_running) && is_running) {
     143    Time now = GetTime();
     144    int t_sec;
     145    FILE *file;
     146    char ligne[32];
     147
     148    t_sec = (now - flight_start_time) / 1000000000;
     149    time_sec += t_sec;
     150
     151    Printf("temps de vol: %is = %imin\n", t_sec, t_sec / 60);
     152    // Printf("temps de vol total: %is = %imin =
     153    // %ih\n",time_sec,time_sec/60,time_sec/3600);
     154    flight_time->SetText("total flight time: %is = %imin = %ih\n", time_sec,
     155                         time_sec / 60, time_sec / 3600);
     156
     157    file = fopen("/etc/flight_time", "w");
    70158    if (file == NULL) {
    71         time_sec=0;
     159      Printf("Erreur a l'ouverture du fichier d'info vol\n");
    72160    } else {
    73         char ligne[32];
    74         fgets(ligne, 32, file);
    75         time_sec=atoi(ligne);
    76         fclose(file);
    77     }
    78     flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    79 }
    80 
    81 Bldc_impl::Bldc_impl(Bldc* self,uint8_t motors_count) {
    82     this->self=self;
    83     this->motors_count=motors_count;
    84     values=NULL;
    85     button_test=NULL;
    86     power=NULL;
    87 }
    88 
    89 Bldc_impl::~Bldc_impl() {
    90     if(values!=NULL) free(values);
    91     if(button_test!=NULL) free(button_test);
    92     if(power!=NULL) free(power);
    93 }
    94 
    95 void Bldc_impl::UseDefaultPlot(TabWidget* tab) {
    96     Tab* plot_tab=new Tab(tab,"speeds");
    97     plots=new DataPlot1D(plot_tab->NewRow(),"values",0,1);
    98     for(int i=0;i<motors_count;i++) {
    99         plots->AddCurve(self->output->Element(i));
    100     }
    101 }
    102 
    103 void Bldc_impl::UpdateFrom(const io_data *data) {
    104     cvmatrix* input=(cvmatrix*)data;
    105     bool is_motor_running=false;
    106 
    107     if(values==NULL) return;//nothing to do in simulator
    108 
    109     if(input->Rows()!=motors_count) {
    110         self->Err("nb motors mismatch\n");
    111         return;
    112     }
    113 
    114     input->GetMutex();
    115     for(int i=0;i<motors_count;i++) {
    116         if(input->ValueNoMutex(i,0)!=0) is_motor_running=true;
    117         if(are_enabled) {
    118             //Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
    119             values[i]=power[i]*Sat(input->ValueNoMutex(i,0));
    120             //Printf("%i %f\n",i,values[i]);
    121         } else {
    122             values[i]=0;
    123         }
    124     }
    125     input->ReleaseMutex();
    126 
    127     if(are_enabled && is_motor_running && !is_running) {
    128         flight_start_time = GetTime();
    129         is_running=true;
    130     }
    131     if((!are_enabled || !is_motor_running) && is_running) {
    132         Time now= GetTime();
    133         int t_sec;
    134         FILE *file;
    135         char ligne[32];
    136 
    137         t_sec=(now - flight_start_time)/1000000000;
    138         time_sec+=t_sec;
    139 
    140         Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
    141         //Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    142         flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    143 
    144         file=fopen("/etc/flight_time","w");
    145         if (file == NULL) {
    146             Printf("Erreur a l'ouverture du fichier d'info vol\n");
    147         } else {
    148             sprintf(ligne,"%i",time_sec);
    149             fputs(ligne,file);
    150             fclose(file);
    151         }
    152         is_running=false;
    153     }
    154 
    155     for(int i=0;i<motors_count;i++) {
    156         if(button_test[i]->Clicked()==true) {
    157             if(!are_enabled) {
    158                 tested_motor=i;
    159                 test_start_time=GetTime();
    160                 LockUserInterface();
    161             } else {
    162                 self->Warn("testing motor is not possible when enabled\n");
    163             }
    164         }
    165     }
    166     if(tested_motor!=-1) {
    167         for(int i=0;i<motors_count;i++) {
    168             values[i]=0;
    169         }
    170         values[tested_motor]=test_value->Value();
    171 
    172         if(GetTime()>(test_start_time+2*1000000000)) {
    173             tested_motor=-1;
    174             UnlockUserInterface();
    175         }
    176     }
    177 
    178     self->SetMotors(values);
    179     self->output->SetDataTime(data->DataTime());
    180     self->ProcessUpdate(self->output);
     161      sprintf(ligne, "%i", time_sec);
     162      fputs(ligne, file);
     163      fclose(file);
     164    }
     165    is_running = false;
     166  }
     167
     168  for (int i = 0; i < motors_count; i++) {
     169    if (button_test[i]->Clicked() == true) {
     170      if (!are_enabled) {
     171        tested_motor = i;
     172        test_start_time = GetTime();
     173        LockUserInterface();
     174      } else {
     175        self->Warn("testing motor is not possible when enabled\n");
     176      }
     177    }
     178  }
     179  if (tested_motor != -1) {
     180    for (int i = 0; i < motors_count; i++) {
     181      values[i] = 0;
     182    }
     183    values[tested_motor] = test_value->Value();
     184
     185    if (GetTime() > (test_start_time + 2 * 1000000000)) {
     186      tested_motor = -1;
     187      UnlockUserInterface();
     188    }
     189  }
     190
     191  self->SetMotors(values);
     192  self->output->SetDataTime(data->DataTime());
     193  self->ProcessUpdate(self->output);
    181194}
    182195
    183196float Bldc_impl::Sat(float value) {
    184     float result=value;
    185 
    186     if(result<min_value->Value()) {
    187         result=min_value->Value();
    188     }
    189     if(result>max_value->Value()) {
    190         result=max_value->Value();
    191     }
    192 
    193     return result;
    194 }
    195 
    196 void Bldc_impl::LockUserInterface(void) const {
    197     layout->setEnabled(false);
    198 }
    199 
    200 void Bldc_impl::UnlockUserInterface(void) const {
    201     layout->setEnabled(true);
    202 }
     197  float result = value;
     198
     199  if (result < min_value->Value()) {
     200    result = min_value->Value();
     201  }
     202  if (result > max_value->Value()) {
     203    result = max_value->Value();
     204  }
     205
     206  return result;
     207}
     208
     209void Bldc_impl::LockUserInterface(void) const { layout->setEnabled(false); }
     210
     211void Bldc_impl::UnlockUserInterface(void) const { layout->setEnabled(true); }

  • trunk/lib/FlairSensorActuator/src/Camera.cpp

    r3 r15  
    3131using namespace flair::gui;
    3232
    33 namespace flair { namespace sensor {
     33namespace flair {
     34namespace sensor {
    3435
    35 Camera::Camera(const FrameworkManager* parent,string name,uint16_t width,uint16_t height,cvimage::Type::Format format) : IODevice(parent,name) {
    36     plot_tab=NULL;
     36Camera::Camera(const FrameworkManager *parent, string name, uint16_t width,
     37               uint16_t height, cvimage::Type::Format format)
     38    : IODevice(parent, name) {
     39  plot_tab = NULL;
    3740
    38     //do not allocate imagedata, allocation is done by the camera
    39     output=new cvimage((IODevice*)this,width,height,format,"out",false);
     41  // do not allocate imagedata, allocation is done by the camera
     42  output = new cvimage((IODevice *)this, width, height, format, "out", false);
    4043
    41     //station sol
    42     main_tab=new Tab(parent->GetTabWidget(),name);
    43     tab=new TabWidget(main_tab->NewRow(),name);
    44         sensor_tab=new Tab(tab,"Setup");
    45         setup_groupbox=new GroupBox(sensor_tab->NewRow(),name);
    46         setup_layout=new GridLayout(sensor_tab->NewRow(),"setup");
     44  // station sol
     45  main_tab = new Tab(parent->GetTabWidget(), name);
     46  tab = new TabWidget(main_tab->NewRow(), name);
     47  sensor_tab = new Tab(tab, "Setup");
     48  setup_groupbox = new GroupBox(sensor_tab->NewRow(), name);
     49  setup_layout = new GridLayout(sensor_tab->NewRow(), "setup");
    4750}
    4851
    49 Camera::Camera(const IODevice* parent,std::string name) : IODevice(parent,name) {
    50     plot_tab=NULL;
    51     main_tab=NULL;
    52     tab=NULL;
    53         sensor_tab=NULL;
    54         setup_groupbox=NULL;
     52Camera::Camera(const IODevice *parent, std::string name)
     53    : IODevice(parent, name) {
     54  plot_tab = NULL;
     55  main_tab = NULL;
     56  tab = NULL;
     57  sensor_tab = NULL;
     58  setup_groupbox = NULL;
    5559
    56     output=NULL;
     60  output = NULL;
    5761}
    5862
    5963Camera::~Camera() {
    60     if(main_tab!=NULL) delete main_tab;
     64  if (main_tab != NULL)
     65    delete main_tab;
    6166}
    6267
    6368DataType const &Camera::GetOutputDataType() const {
    64     return output->GetDataType();
     69  return output->GetDataType();
    6570}
    6671
    67 GroupBox* Camera::GetGroupBox(void) const {
    68     return setup_groupbox;
     72GroupBox *Camera::GetGroupBox(void) const { return setup_groupbox; }
     73
     74GridLayout *Camera::GetLayout(void) const { return setup_layout; }
     75
     76void Camera::UseDefaultPlot(const core::cvimage *image) {
     77  if (tab == NULL) {
     78    Err("not applicable for simulation part.\n");
     79    return;
     80  }
     81
     82  plot_tab = new Tab(tab, "Picture");
     83  Picture *plot = new Picture(plot_tab->NewRow(), ObjectName(), image);
    6984}
    7085
    71 GridLayout* Camera::GetLayout(void) const {
    72     return setup_layout;
     86Tab *Camera::GetPlotTab(void) const { return plot_tab; }
     87
     88uint16_t Camera::Width(void) const { return output->GetDataType().GetWidth(); }
     89
     90uint16_t Camera::Height(void) const {
     91  return output->GetDataType().GetHeight();
    7392}
    7493
    75 void Camera::UseDefaultPlot(const core::cvimage *image) {
    76     if(tab==NULL) {
    77         Err("not applicable for simulation part.\n");
    78         return;
    79     }
    80 
    81     plot_tab=new Tab(tab,"Picture");
    82         Picture* plot=new Picture(plot_tab->NewRow(),ObjectName(),image);
    83 }
    84 
    85 Tab* Camera::GetPlotTab(void) const {
    86     return plot_tab;
    87 }
    88 
    89 uint16_t Camera::Width(void) const {
    90     return output->GetDataType().GetWidth();
    91 }
    92 
    93 uint16_t Camera::Height(void) const {
    94     return output->GetDataType().GetHeight();
    95 }
    96 
    97 core::cvimage* Camera::Output(void) {
    98     return output;
    99 }
     94core::cvimage *Camera::Output(void) { return output; }
    10095
    10196void Camera::SaveToFile(string filename) const {
    102     Printf("saving %s, size %i\n",filename.c_str(),output->img->imageSize);
    103     std::ofstream pFile;
    104         pFile.open (filename);
    105         output->GetMutex();
    106         pFile.write(output->img->imageData, output->img->imageSize);
    107     output->ReleaseMutex();
     97  Printf("saving %s, size %i\n", filename.c_str(), output->img->imageSize);
     98  std::ofstream pFile;
     99  pFile.open(filename);
     100  output->GetMutex();
     101  pFile.write(output->img->imageData, output->img->imageSize);
     102  output->ReleaseMutex();
    108103
    109         pFile.close();
     104  pFile.close();
    110105}
    111106

  • trunk/lib/FlairSensorActuator/src/Camera.h

    r3 r15  
    1818#include <cvimage.h>
    1919
    20 namespace flair
    21 {
    22     namespace gui
    23     {
    24         class GroupBox;
    25         class Tab;
    26         class TabWidget;
    27         class Picture;
    28         class GridLayout;
    29     }
     20namespace flair {
     21namespace gui {
     22class GroupBox;
     23class Tab;
     24class TabWidget;
     25class Picture;
     26class GridLayout;
     27}
    3028}
    3129
    32 namespace flair
    33 {
    34 namespace sensor
    35 {
    36     /*! \class Camera
    37     *
    38     * \brief Base class for Camera
    39     *
    40     * Use this class to define a custom Camera.
    41     *
    42     */
    43     class Camera : public core::IODevice
    44     {
    45         public:
    46             /*!
    47             * \brief Constructor
    48             *
    49             * Construct a Camera.
    50             *
    51             * \param parent parent
    52             * \param name name
    53             * \param width width
    54             * \param height height
    55             * \param format image format
    56             */
    57             Camera(const core::FrameworkManager* parent,std::string name,uint16_t width,uint16_t height,core::cvimage::Type::Format format);
     30namespace flair {
     31namespace sensor {
     32/*! \class Camera
     33*
     34* \brief Base class for Camera
     35*
     36* Use this class to define a custom Camera.
     37*
     38*/
     39class Camera : public core::IODevice {
     40public:
     41  /*!
     42  * \brief Constructor
     43  *
     44  * Construct a Camera.
     45  *
     46  * \param parent parent
     47  * \param name name
     48  * \param width width
     49  * \param height height
     50  * \param format image format
     51  */
     52  Camera(const core::FrameworkManager *parent, std::string name, uint16_t width,
     53         uint16_t height, core::cvimage::Type::Format format);
    5854
    59             /*!
    60             * \brief Constructor
    61             *
    62             * Construct a Camera. \n
    63             * This contructor must only be called for a simulated device.
    64             *
    65             * \param parent parent
    66             * \param name name
    67             */
    68             Camera(const core::IODevice* parent,std::string name);
     55  /*!
     56  * \brief Constructor
     57  *
     58  * Construct a Camera. \n
     59  * This contructor must only be called for a simulated device.
     60  *
     61  * \param parent parent
     62  * \param name name
     63  */
     64  Camera(const core::IODevice *parent, std::string name);
    6965
    70             /*!
    71             * \brief Destructor
    72             *
    73             */
    74             ~Camera();
     66  /*!
     67  * \brief Destructor
     68  *
     69  */
     70  ~Camera();
    7571
    76             /*!
    77             * \brief Use default plot
    78             *
    79             * \param image image to display
    80             */
    81             void UseDefaultPlot(const core::cvimage *image);
     72  /*!
     73  * \brief Use default plot
     74  *
     75  * \param image image to display
     76  */
     77  void UseDefaultPlot(const core::cvimage *image);
    8278
    83             /*!
    84             * \brief get Layout
    85             *
    86             * \return a Layout available
    87             */
    88             gui::GridLayout* GetLayout(void) const;
     79  /*!
     80  * \brief get Layout
     81  *
     82  * \return a Layout available
     83  */
     84  gui::GridLayout *GetLayout(void) const;
    8985
    90             /*!
    91             * \brief plot tab
    92             *
    93             * \return plot tab
    94             */
    95             gui::Tab* GetPlotTab(void) const;
     86  /*!
     87  * \brief plot tab
     88  *
     89  * \return plot tab
     90  */
     91  gui::Tab *GetPlotTab(void) const;
    9692
    97             /*!
    98             * \brief Save picture to file
    99             *
    100             * \param filename filename
    101             */
    102             void SaveToFile(std::string filename) const;
     93  /*!
     94  * \brief Save picture to file
     95  *
     96  * \param filename filename
     97  */
     98  void SaveToFile(std::string filename) const;
    10399
    104             /*!
    105             * \brief Width
    106             *
    107             * \return width
    108             */
    109             uint16_t Width(void) const;
     100  /*!
     101  * \brief Width
     102  *
     103  * \return width
     104  */
     105  uint16_t Width(void) const;
    110106
    111             /*!
    112             * \brief Height
    113             *
    114             * \return height
    115             */
    116             uint16_t Height(void) const;
     107  /*!
     108  * \brief Height
     109  *
     110  * \return height
     111  */
     112  uint16_t Height(void) const;
    117113
    118              /*!
    119             * \brief Output matrix
    120             *
    121             * Output matrix is of the same size as declared in constructor. \n
    122             *
    123             * \return the output matrix
    124             */
    125             core::cvimage* Output(void);
     114  /*!
     115 * \brief Output matrix
     116 *
     117 * Output matrix is of the same size as declared in constructor. \n
     118 *
     119 * \return the output matrix
     120 */
     121  core::cvimage *Output(void);
    126122
    127             core::DataType const &GetOutputDataType() const;
     123  core::DataType const &GetOutputDataType() const;
    128124
    129         protected:
    130             /*!
    131             * \brief get GroupBox
    132             *
    133             * \return a GroupBox available
    134             */
    135             gui::GroupBox* GetGroupBox(void) const;
     125protected:
     126  /*!
     127  * \brief get GroupBox
     128  *
     129  * \return a GroupBox available
     130  */
     131  gui::GroupBox *GetGroupBox(void) const;
    136132
    137             core::cvimage *output;
     133  core::cvimage *output;
    138134
    139         private:
    140             gui::Tab *main_tab,*sensor_tab,*plot_tab;
    141             gui::TabWidget *tab;
    142             gui::GroupBox* setup_groupbox;
    143             gui::GridLayout* setup_layout;
    144     };
     135private:
     136  gui::Tab *main_tab, *sensor_tab, *plot_tab;
     137  gui::TabWidget *tab;
     138  gui::GroupBox *setup_groupbox;
     139  gui::GridLayout *setup_layout;
     140};
    145141} // end namespace sensor
    146142} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/Controller.cpp

    r3 r15  
    1919using namespace flair::core;
    2020
    21 namespace flair { namespace sensor {
     21namespace flair {
     22namespace sensor {
    2223
    23 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)) {
    24     SetLeftForce(leftForce);
    25     SetLeftTimeout(leftTimeout);
    26     SetRightForce(rightForce);
    27     SetRightTimeout(rightTimeout);
    28     buffer[0]=(char)ControllerAction::Rumble;
     24RumbleMessage::RumbleMessage(unsigned int leftForce, unsigned int leftTimeout,
     25                             unsigned int rightForce, unsigned int rightTimeout)
     26    : Message(sizeof(ControllerAction) + sizeof(leftForce) +
     27              sizeof(leftTimeout) + sizeof(rightForce) + sizeof(rightTimeout)) {
     28  SetLeftForce(leftForce);
     29  SetLeftTimeout(leftTimeout);
     30  SetRightForce(rightForce);
     31  SetRightTimeout(rightTimeout);
     32  buffer[0] = (char)ControllerAction::Rumble;
    2933}
    3034
    3135unsigned int RumbleMessage::GetLeftForce() const {
    32     unsigned int leftForce;
    33     memcpy(&leftForce,buffer+leftForceOffset,sizeof(leftForce));
    34     return leftForce;
     36  unsigned int leftForce;
     37  memcpy(&leftForce, buffer + leftForceOffset, sizeof(leftForce));
     38  return leftForce;
    3539}
    3640
    3741unsigned int RumbleMessage::GetLeftTimeout() const {
    38     unsigned int leftTimeout;
    39     memcpy(&leftTimeout,buffer+leftTimeoutOffset,sizeof(leftTimeout));
    40     return leftTimeout;
     42  unsigned int leftTimeout;
     43  memcpy(&leftTimeout, buffer + leftTimeoutOffset, sizeof(leftTimeout));
     44  return leftTimeout;
    4145}
    4246
    4347unsigned int RumbleMessage::GetRightForce() const {
    44     unsigned int rightForce;
    45     memcpy(&rightForce,buffer+rightForceOffset,sizeof(rightForce));
    46     return rightForce;
     48  unsigned int rightForce;
     49  memcpy(&rightForce, buffer + rightForceOffset, sizeof(rightForce));
     50  return rightForce;
    4751}
    4852
    4953unsigned int RumbleMessage::GetRightTimeout() const {
    50     unsigned int rightTimeout;
    51     memcpy(&rightTimeout,buffer+rightTimeoutOffset,sizeof(rightTimeout));
    52     return rightTimeout;
     54  unsigned int rightTimeout;
     55  memcpy(&rightTimeout, buffer + rightTimeoutOffset, sizeof(rightTimeout));
     56  return rightTimeout;
    5357}
    5458
    5559void RumbleMessage::SetLeftForce(unsigned int leftForce) {
    56     memcpy(buffer+leftForceOffset,&leftForce,sizeof(leftForce));
     60  memcpy(buffer + leftForceOffset, &leftForce, sizeof(leftForce));
    5761}
    5862
    5963void RumbleMessage::SetLeftTimeout(unsigned int leftTimeout) {
    60     memcpy(buffer+leftTimeoutOffset,&leftTimeout,sizeof(leftTimeout));
     64  memcpy(buffer + leftTimeoutOffset, &leftTimeout, sizeof(leftTimeout));
    6165}
    6266
    6367void RumbleMessage::SetRightForce(unsigned int rightForce) {
    64     memcpy(buffer+rightForceOffset,&rightForce,sizeof(rightForce));
     68  memcpy(buffer + rightForceOffset, &rightForce, sizeof(rightForce));
    6569}
    6670
    67 void RumbleMessage::SetRightTimeout(unsigned int rightTimeout){
    68     memcpy(buffer+rightTimeoutOffset,&rightTimeout,sizeof(rightTimeout));
     71void RumbleMessage::SetRightTimeout(unsigned int rightTimeout) {
     72  memcpy(buffer + rightTimeoutOffset, &rightTimeout, sizeof(rightTimeout));
    6973}
    7074
    71 SwitchLedMessage::SwitchLedMessage(bool isOn, unsigned int ledId):Message(sizeof(ControllerAction)+sizeof(isOn)+sizeof(ledId)) {
    72     if (isOn) SetOn(); else SetOff();
    73     SetLedId(ledId);
     75SwitchLedMessage::SwitchLedMessage(bool isOn, unsigned int ledId)
     76    : Message(sizeof(ControllerAction) + sizeof(isOn) + sizeof(ledId)) {
     77  if (isOn)
     78    SetOn();
     79  else
     80    SetOff();
     81  SetLedId(ledId);
    7482}
    7583
    7684bool SwitchLedMessage::IsOn() const {
    77     bool isOn;
    78     memcpy(&isOn,buffer+isOnOffset,sizeof(isOn));
    79     return isOn;
     85  bool isOn;
     86  memcpy(&isOn, buffer + isOnOffset, sizeof(isOn));
     87  return isOn;
    8088}
    8189
    8290unsigned int SwitchLedMessage::GetLedId() const {
    83     unsigned int ledId;
    84     memcpy(&ledId,buffer+ledIdOffset,sizeof(ledId));
    85     return ledId;
     91  unsigned int ledId;
     92  memcpy(&ledId, buffer + ledIdOffset, sizeof(ledId));
     93  return ledId;
    8694}
    8795
    8896void SwitchLedMessage::SetOn() {
    89     bool isOn=true;
    90     memcpy(buffer+isOnOffset,&isOn,sizeof(isOn));
     97  bool isOn = true;
     98  memcpy(buffer + isOnOffset, &isOn, sizeof(isOn));
    9199}
    92100
    93101void SwitchLedMessage::SetOff() {
    94     bool isOn=false;
    95     memcpy(buffer+isOnOffset,&isOn,sizeof(isOn));
     102  bool isOn = false;
     103  memcpy(buffer + isOnOffset, &isOn, sizeof(isOn));
    96104}
    97105
    98106void SwitchLedMessage::SetLedId(unsigned int ledId) {
    99     memcpy(buffer+ledIdOffset,&ledId,sizeof(ledId));
     107  memcpy(buffer + ledIdOffset, &ledId, sizeof(ledId));
    100108}
    101109
    102 FlashLedMessage::FlashLedMessage(unsigned int ledId,unsigned int onTime,unsigned int offTime):Message(sizeof(ControllerAction)+sizeof(ledId)+sizeof(onTime)+sizeof(offTime)) {
    103     SetLedId(ledId);
    104     SetOnTime(onTime);
    105     SetOffTime(offTime);
     110FlashLedMessage::FlashLedMessage(unsigned int ledId, unsigned int onTime,
     111                                 unsigned int offTime)
     112    : Message(sizeof(ControllerAction) + sizeof(ledId) + sizeof(onTime) +
     113              sizeof(offTime)) {
     114  SetLedId(ledId);
     115  SetOnTime(onTime);
     116  SetOffTime(offTime);
    106117}
    107118
    108119unsigned int FlashLedMessage::GetLedId() const {
    109     unsigned int ledId;
    110     memcpy(&ledId,buffer+ledIdOffset,sizeof(ledId));
    111     return ledId;
     120  unsigned int ledId;
     121  memcpy(&ledId, buffer + ledIdOffset, sizeof(ledId));
     122  return ledId;
    112123}
    113124
    114125unsigned int FlashLedMessage::GetOnTime() const {
    115     unsigned int onTime;
    116     memcpy(&onTime,buffer+onTimeOffset,sizeof(onTime));
    117     return onTime;
     126  unsigned int onTime;
     127  memcpy(&onTime, buffer + onTimeOffset, sizeof(onTime));
     128  return onTime;
    118129}
    119130
    120131unsigned int FlashLedMessage::GetOffTime() const {
    121     unsigned int offTime;
    122     memcpy(&offTime,buffer+offTimeOffset,sizeof(offTime));
    123     return offTime;
     132  unsigned int offTime;
     133  memcpy(&offTime, buffer + offTimeOffset, sizeof(offTime));
     134  return offTime;
    124135}
    125136
    126137void FlashLedMessage::SetLedId(unsigned int ledId) {
    127     memcpy(buffer+ledIdOffset,&ledId,sizeof(ledId));
     138  memcpy(buffer + ledIdOffset, &ledId, sizeof(ledId));
    128139}
    129140
    130141void FlashLedMessage::SetOnTime(unsigned int onTime) {
    131     memcpy(buffer+onTimeOffset,&onTime,sizeof(onTime));
     142  memcpy(buffer + onTimeOffset, &onTime, sizeof(onTime));
    132143}
    133144
    134145void FlashLedMessage::SetOffTime(unsigned int offTime) {
    135     memcpy(buffer+offTimeOffset,&offTime,sizeof(offTime));
     146  memcpy(buffer + offTimeOffset, &offTime, sizeof(offTime));
    136147}
    137148

  • trunk/lib/FlairSensorActuator/src/Controller.h

    r3 r15  
    2323
    2424namespace flair {
    25     namespace core {
    26         class Message;
    27     }
     25namespace core {
     26class Message;
     27}
    2828}
    2929
    30 namespace flair { namespace sensor {
     30namespace flair {
     31namespace sensor {
    3132
    32     enum class ControllerAction {
    33         SetLedOn,
    34         SetLedOff,
    35         Rumble,
    36         FlashLed,
    37         Exit
    38     };
     33enum class ControllerAction { SetLedOn, SetLedOff, Rumble, FlashLed, Exit };
    3934
    40     class RumbleMessage: public core::Message {
    41     public:
    42         RumbleMessage(unsigned int leftForce,unsigned int leftTimeout,unsigned int rightForce,unsigned int rightTimeout);
    43         unsigned int GetLeftForce() const;
    44         unsigned int GetLeftTimeout() const;
    45         unsigned int GetRightForce() const;
    46         unsigned int GetRightTimeout() const;
    47         void SetLeftForce(unsigned int leftForce);
    48         void SetLeftTimeout(unsigned int leftTimeout);
    49         void SetRightForce(unsigned int rightForce);
    50         void SetRightTimeout(unsigned int rightTimeout);
    51     private:
    52         static const unsigned int leftForceOffset=sizeof(ControllerAction);
    53         static const unsigned int leftTimeoutOffset=sizeof(ControllerAction)+sizeof(unsigned int);
    54         static const unsigned int rightForceOffset=sizeof(ControllerAction)+2*sizeof(unsigned int);
    55         static const unsigned int rightTimeoutOffset=sizeof(ControllerAction)+3*sizeof(unsigned int);
    56     };
     35class RumbleMessage : public core::Message {
     36public:
     37  RumbleMessage(unsigned int leftForce, unsigned int leftTimeout,
     38                unsigned int rightForce, unsigned int rightTimeout);
     39  unsigned int GetLeftForce() const;
     40  unsigned int GetLeftTimeout() const;
     41  unsigned int GetRightForce() const;
     42  unsigned int GetRightTimeout() const;
     43  void SetLeftForce(unsigned int leftForce);
     44  void SetLeftTimeout(unsigned int leftTimeout);
     45  void SetRightForce(unsigned int rightForce);
     46  void SetRightTimeout(unsigned int rightTimeout);
    5747
    58     class SwitchLedMessage: public core::Message {
    59     public:
    60         SwitchLedMessage(bool isOn,unsigned int ledId);
    61         bool IsOn() const;
    62         unsigned int GetLedId() const;
    63         void SetOn();
    64         void SetOff();
    65         void SetLedId(unsigned int ledId);
    66     private:
    67         static const unsigned int isOnOffset=sizeof(ControllerAction);
    68         static const unsigned int ledIdOffset=sizeof(ControllerAction)+sizeof(bool);
    69     };
     48private:
     49  static const unsigned int leftForceOffset = sizeof(ControllerAction);
     50  static const unsigned int leftTimeoutOffset =
     51      sizeof(ControllerAction) + sizeof(unsigned int);
     52  static const unsigned int rightForceOffset =
     53      sizeof(ControllerAction) + 2 * sizeof(unsigned int);
     54  static const unsigned int rightTimeoutOffset =
     55      sizeof(ControllerAction) + 3 * sizeof(unsigned int);
     56};
    7057
    71     class FlashLedMessage: public core::Message {
    72     public:
    73         FlashLedMessage(unsigned int ledId,unsigned int onTime,unsigned int offTime);
    74         unsigned int GetLedId() const;
    75         unsigned int GetOnTime() const;
    76         unsigned int GetOffTime() const;
    77         void SetLedId(unsigned int ledId);
    78         void SetOnTime(unsigned int onTime);
    79         void SetOffTime(unsigned int offTime);
    80     private:
    81         static const unsigned int ledIdOffset=sizeof(ControllerAction);
    82         static const unsigned int onTimeOffset=sizeof(ControllerAction)+sizeof(unsigned int);
    83         static const unsigned int offTimeOffset=sizeof(ControllerAction)+2*sizeof(unsigned int);
    84     };
     58class SwitchLedMessage : public core::Message {
     59public:
     60  SwitchLedMessage(bool isOn, unsigned int ledId);
     61  bool IsOn() const;
     62  unsigned int GetLedId() const;
     63  void SetOn();
     64  void SetOff();
     65  void SetLedId(unsigned int ledId);
    8566
    86 }}
     67private:
     68  static const unsigned int isOnOffset = sizeof(ControllerAction);
     69  static const unsigned int ledIdOffset =
     70      sizeof(ControllerAction) + sizeof(bool);
     71};
     72
     73class FlashLedMessage : public core::Message {
     74public:
     75  FlashLedMessage(unsigned int ledId, unsigned int onTime,
     76                  unsigned int offTime);
     77  unsigned int GetLedId() const;
     78  unsigned int GetOnTime() const;
     79  unsigned int GetOffTime() const;
     80  void SetLedId(unsigned int ledId);
     81  void SetOnTime(unsigned int onTime);
     82  void SetOffTime(unsigned int offTime);
     83
     84private:
     85  static const unsigned int ledIdOffset = sizeof(ControllerAction);
     86  static const unsigned int onTimeOffset =
     87      sizeof(ControllerAction) + sizeof(unsigned int);
     88  static const unsigned int offTimeOffset =
     89      sizeof(ControllerAction) + 2 * sizeof(unsigned int);
     90};
     91}
     92}
    8793
    8894#endif // CONTROLLER_H

  • trunk/lib/FlairSensorActuator/src/Gps.cpp

    r3 r15  
    3838using namespace flair::gui;
    3939
    40 namespace flair
    41 {
    42 namespace sensor
    43 {
    44 
    45 Gps::Gps(const FrameworkManager* parent,string name,NMEAFlags_t NMEAFlags) : IODevice(parent,name)
    46 {
    47     this->NMEAFlags=NMEAFlags;
    48 
    49     nmea_zero_INFO(&info);
    50     nmea_parser_init(&parser);
    51     alt_ref=0;
    52 
    53 
    54     if((NMEAFlags&GGA)==0)
     40namespace flair {
     41namespace sensor {
     42
     43Gps::Gps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags)
     44    : IODevice(parent, name) {
     45  this->NMEAFlags = NMEAFlags;
     46
     47  nmea_zero_INFO(&info);
     48  nmea_parser_init(&parser);
     49  alt_ref = 0;
     50
     51  if ((NMEAFlags & GGA) == 0) {
     52    Err("Enable at least GGA sentence\n");
     53  }
     54
     55  int index = 0;
     56  if ((NMEAFlags & GGA) != 0) {
     57    index += 3;
     58  }
     59  if ((NMEAFlags & VTG) != 0) {
     60    index += 2;
     61  }
     62  if ((NMEAFlags & GST) != 0) {
     63    index += 3;
     64  }
     65
     66  cvmatrix_descriptor *desc = new cvmatrix_descriptor(index, 1);
     67  index = 0;
     68  if ((NMEAFlags & GGA) != 0) {
     69    desc->SetElementName(0, 0, "e");
     70    desc->SetElementName(1, 0, "n");
     71    desc->SetElementName(2, 0, "u");
     72    index += 3;
     73  }
     74  if ((NMEAFlags & VTG) != 0) {
     75    desc->SetElementName(index, 0, "ve");
     76    desc->SetElementName(index + 1, 0, "vn");
     77    index += 2;
     78  }
     79  if ((NMEAFlags & GST) != 0) {
     80    desc->SetElementName(index, 0, "dev_lat");
     81    desc->SetElementName(index + 1, 0, "dev_lon");
     82    desc->SetElementName(index + 2, 0, "dev_elv");
     83    index += 3;
     84  }
     85  output = new cvmatrix((IODevice *)this, desc, floatType);
     86  AddDataToLog(output);
     87
     88  // station sol
     89  main_tab = new Tab(parent->GetTabWidget(), name);
     90  tab = new TabWidget(main_tab->NewRow(), name);
     91  sensor_tab = new Tab(tab, "Reglages");
     92  GroupBox *reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
     93  button_ref = new PushButton(reglages_groupbox->NewRow(), "set ref");
     94  nb_sat_label = new Label(reglages_groupbox->NewRow(), "nb_sat");
     95  fix_label = new Label(reglages_groupbox->LastRowLastCol(), "fix");
     96
     97  position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
     98
     99  fix = FixQuality_t::Invalid;
     100  nb_sat = 0;
     101  take_ref = false;
     102  nb_sat_label->SetText("nb_sat: %i", nb_sat);
     103  fix_label->SetText("fix: %i", fix);
     104}
     105
     106Gps::~Gps() {
     107  nmea_parser_destroy(&parser);
     108  delete main_tab;
     109}
     110
     111void Gps::UseDefaultPlot(void) {
     112  int index = 0;
     113  plot_tab = new Tab(tab, "Mesures");
     114
     115  if ((NMEAFlags & GGA) != 0) {
     116    e_plot = new DataPlot1D(plot_tab->NewRow(), "e", -10, 10);
     117    e_plot->AddCurve(output->Element(index));
     118    n_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "n", -10, 10);
     119    n_plot->AddCurve(output->Element(index + 1));
     120    u_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "u", -10, 10);
     121    u_plot->AddCurve(output->Element(index + 2));
     122    index += 3;
     123  }
     124  if ((NMEAFlags & VTG) != 0) {
     125    ve_plot = new DataPlot1D(plot_tab->NewRow(), "ve", -10, 10);
     126    ve_plot->AddCurve(output->Element(index));
     127    vn_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vn", -10, 10);
     128    vn_plot->AddCurve(output->Element(index + 1));
     129    index += 2;
     130  }
     131
     132  Tab *map_tab = new Tab(tab, "carte");
     133  map = new Map(map_tab->NewRow(), "map");
     134  map->AddPoint(position, "drone");
     135}
     136
     137DataPlot1D *Gps::EPlot(void) const {
     138  if ((NMEAFlags & GGA) != 0) {
     139    return e_plot;
     140  } else {
     141    Err("GGA sentence not requested\n");
     142    return NULL;
     143  }
     144}
     145
     146DataPlot1D *Gps::NPlot(void) const {
     147  if ((NMEAFlags & GGA) != 0) {
     148    return n_plot;
     149  } else {
     150    Err("GGA sentence not requested\n");
     151    return NULL;
     152  }
     153}
     154
     155DataPlot1D *Gps::UPlot(void) const {
     156  if ((NMEAFlags & GGA) != 0) {
     157    return u_plot;
     158  } else {
     159    Err("GGA sentence not requested\n");
     160    return NULL;
     161  }
     162}
     163
     164DataPlot1D *Gps::VEPlot(void) const {
     165  if ((NMEAFlags & VTG) != 0) {
     166    return ve_plot;
     167  } else {
     168    Err("GGA sentence not requested\n");
     169    return NULL;
     170  }
     171}
     172
     173DataPlot1D *Gps::VNPlot(void) const {
     174  if ((NMEAFlags & VTG) != 0) {
     175    return vn_plot;
     176  } else {
     177    Err("GGA sentence not requested\n");
     178    return NULL;
     179  }
     180}
     181
     182Layout *Gps::GetLayout(void) const { return sensor_tab; }
     183
     184Tab *Gps::GetPlotTab(void) const { return plot_tab; }
     185
     186TabWidget *Gps::GetTab(void) const { return tab; }
     187
     188uint16_t Gps::NbSat(void) const {
     189  output->GetMutex();
     190  uint16_t result = nb_sat;
     191  output->ReleaseMutex();
     192  return result;
     193}
     194
     195Gps::FixQuality_t Gps::FixQuality(void) const {
     196  output->GetMutex();
     197  FixQuality_t result = fix;
     198  output->ReleaseMutex();
     199  return result;
     200}
     201
     202void Gps::SetRef(void) { take_ref = true; }
     203
     204void Gps::GetENUPosition(Vector3D *point) {
     205  output->GetMutex();
     206  point->x = output->ValueNoMutex(0, 0);
     207  point->y = output->ValueNoMutex(1, 0);
     208  point->z = output->ValueNoMutex(2, 0);
     209  output->ReleaseMutex();
     210}
     211
     212void Gps::parseFrame(const char *frame, int frame_size) {
     213
     214  int result;
     215
     216  result = nmea_parse(&parser, frame, frame_size, &info);
     217  if (result != 1) {
     218    Warn("unrecognized nmea sentence\n");
     219    Warn("%s\n", frame);
     220  }
     221
     222  result = nmea_parse_GPGGA(frame, frame_size, &pack);
     223
     224  if (result == 1) {
     225    // Printf("%s\n",frame);
     226    // Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f
     227    // angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
     228    // Printf("lat:%f long:%f\n",pos.lat,pos.lon);
     229    output->GetMutex(); // on utilise le mutex de l'output pour fix et nb_sat
     230    if ((int)fix != pack.sig) {
     231      fix = (FixQuality_t)pack.sig;
     232      fix_label->SetText("fix: %i", fix);
     233    }
     234    if (nb_sat != pack.satinuse) {
     235      nb_sat = pack.satinuse;
     236      nb_sat_label->SetText("nb_sat: %i", nb_sat);
     237    }
     238    output->ReleaseMutex();
     239
     240    nmea_info2pos(&info, &pos);
     241    position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),
     242                             info.elv);
     243
     244    if ((info.sig == 2 && alt_ref == 0) || button_ref->Clicked() == true ||
     245        take_ref == true) {
     246      Printf("prise pos ref\n");
     247      lat_ref = pos.lat;
     248      long_ref = pos.lon;
     249      alt_ref = info.elv;
     250      take_ref = false;
     251    }
     252    // if(alt_ref!=0)
    55253    {
    56         Err("Enable at least GGA sentence\n");
    57     }
    58 
    59     int index=0;
    60     if((NMEAFlags&GGA)!=0)
    61     {
    62         index+=3;
    63     }
    64     if((NMEAFlags&VTG)!=0)
    65     {
    66         index+=2;
    67     }
    68     if((NMEAFlags&GST)!=0)
    69     {
    70         index+=3;
    71     }
    72 
    73     cvmatrix_descriptor* desc=new cvmatrix_descriptor(index,1);
    74     index=0;
    75     if((NMEAFlags&GGA)!=0)
    76     {
    77         desc->SetElementName(0,0,"e");
    78         desc->SetElementName(1,0,"n");
    79         desc->SetElementName(2,0,"u");
    80         index+=3;
    81     }
    82     if((NMEAFlags&VTG)!=0)
    83     {
    84         desc->SetElementName(index,0,"ve");
    85         desc->SetElementName(index+1,0,"vn");
    86         index+=2;
    87     }
    88     if((NMEAFlags&GST)!=0)
    89     {
    90         desc->SetElementName(index,0,"dev_lat");
    91         desc->SetElementName(index+1,0,"dev_lon");
    92         desc->SetElementName(index+2,0,"dev_elv");
    93         index+=3;
    94     }
    95     output=new cvmatrix((IODevice*)this,desc,floatType);
    96     AddDataToLog(output);
    97 
    98     //station sol
    99     main_tab=new Tab(parent->GetTabWidget(),name);
    100     tab=new TabWidget(main_tab->NewRow(),name);
    101         sensor_tab=new Tab(tab,"Reglages");
    102         GroupBox* reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
    103         button_ref=new PushButton(reglages_groupbox->NewRow(),"set ref");
    104         nb_sat_label=new Label(reglages_groupbox->NewRow(),"nb_sat");
    105         fix_label=new Label(reglages_groupbox->LastRowLastCol(),"fix");
    106 
    107     position=new GeoCoordinate((IODevice*)this,"position",0,0,0);
    108 
    109     fix=FixQuality_t::Invalid;
    110     nb_sat=0;
    111     take_ref=false;
    112     nb_sat_label->SetText("nb_sat: %i",nb_sat);
    113     fix_label->SetText("fix: %i",fix);
    114 }
    115 
    116 Gps::~Gps()
    117 {
    118     nmea_parser_destroy(&parser);
    119     delete main_tab;
    120 }
    121 
    122 void Gps::UseDefaultPlot(void)
    123 {
    124     int index=0;
    125     plot_tab=new Tab(tab,"Mesures");
    126 
    127     if((NMEAFlags&GGA)!=0)
    128     {
    129         e_plot=new DataPlot1D(plot_tab->NewRow(),"e",-10,10);
    130             e_plot->AddCurve(output->Element(index));
    131         n_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"n",-10,10);
    132             n_plot->AddCurve(output->Element(index+1));
    133         u_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"u",-10,10);
    134             u_plot->AddCurve(output->Element(index+2));
    135         index+=3;
    136     }
    137     if((NMEAFlags&VTG)!=0)
    138     {
    139         ve_plot=new DataPlot1D(plot_tab->NewRow(),"ve",-10,10);
    140             ve_plot->AddCurve(output->Element(index));
    141         vn_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"vn",-10,10);
    142             vn_plot->AddCurve(output->Element(index+1));
    143         index+=2;
    144     }
    145 
    146     Tab* map_tab=new Tab(tab,"carte");
    147         map=new Map(map_tab->NewRow(),"map");
    148             map->AddPoint(position,"drone");
    149 }
    150 
    151 DataPlot1D* Gps::EPlot(void) const
    152 {
    153     if((NMEAFlags&GGA)!=0)
    154     {
    155         return e_plot;
    156     }
    157     else
    158     {
    159         Err("GGA sentence not requested\n");
    160         return NULL;
    161     }
    162 }
    163 
    164 DataPlot1D* Gps::NPlot(void) const
    165 {
    166     if((NMEAFlags&GGA)!=0)
    167     {
    168         return n_plot;
    169     }
    170     else
    171     {
    172         Err("GGA sentence not requested\n");
    173         return NULL;
    174     }
    175 }
    176 
    177 DataPlot1D* Gps::UPlot(void) const
    178 {
    179     if((NMEAFlags&GGA)!=0)
    180     {
    181         return u_plot;
    182     }
    183     else
    184     {
    185         Err("GGA sentence not requested\n");
    186         return NULL;
    187     }
    188 }
    189 
    190 DataPlot1D* Gps::VEPlot(void) const
    191 {
    192     if((NMEAFlags&VTG)!=0)
    193     {
    194         return ve_plot;
    195     }
    196     else
    197     {
    198         Err("GGA sentence not requested\n");
    199         return NULL;
    200     }
    201 }
    202 
    203 DataPlot1D* Gps::VNPlot(void) const
    204 {
    205     if((NMEAFlags&VTG)!=0)
    206     {
    207         return vn_plot;
    208     }
    209     else
    210     {
    211         Err("GGA sentence not requested\n");
    212         return NULL;
    213     }
    214 }
    215 
    216 Layout* Gps::GetLayout(void) const
    217 {
    218     return sensor_tab;
    219 }
    220 
    221 Tab* Gps::GetPlotTab(void) const
    222 {
    223     return plot_tab;
    224 }
    225 
    226 TabWidget* Gps::GetTab(void) const
    227 {
    228     return tab;
    229 }
    230 
    231 uint16_t Gps::NbSat(void) const
    232 {
    233     output->GetMutex();
    234     uint16_t result=nb_sat;
    235     output->ReleaseMutex();
    236     return result;
    237 }
    238 
    239 Gps::FixQuality_t Gps::FixQuality(void) const
    240 {
    241     output->GetMutex();
    242     FixQuality_t result=fix;
    243     output->ReleaseMutex();
    244     return result;
    245 }
    246 
    247 void Gps::SetRef(void)
    248 {
    249     take_ref=true;
    250 }
    251 
    252 void Gps::GetENUPosition(Vector3D *point)
    253 {
    254     output->GetMutex();
    255     point->x=output->ValueNoMutex(0,0);
    256     point->y=output->ValueNoMutex(1,0);
    257     point->z=output->ValueNoMutex(2,0);
    258     output->ReleaseMutex();
    259 }
    260 
    261 void Gps::parseFrame(const char *frame, int frame_size){
    262 
    263     int result;
    264 
    265     result=nmea_parse(&parser, frame, frame_size, &info);
    266     if(result!=1)
    267     {
    268         Warn("unrecognized nmea sentence\n");
    269         Warn("%s\n",frame);
    270     }
    271 
    272     result=nmea_parse_GPGGA(frame, frame_size, &pack);
    273 
    274     if(result==1)
    275     {
    276         //Printf("%s\n",frame);
    277         //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);
    278         //Printf("lat:%f long:%f\n",pos.lat,pos.lon);
    279         output->GetMutex();//on utilise le mutex de l'output pour fix et nb_sat
    280         if((int)fix!=pack.sig)
    281         {
    282             fix=(FixQuality_t)pack.sig;
    283             fix_label->SetText("fix: %i",fix);
    284         }
    285         if(nb_sat!=pack.satinuse)
    286         {
    287             nb_sat=pack.satinuse;
    288             nb_sat_label->SetText("nb_sat: %i",nb_sat);
    289         }
    290         output->ReleaseMutex();
    291 
    292 
    293         nmea_info2pos(&info,&pos);
    294         position->SetCoordinates(Euler::ToDegree(pos.lat),Euler::ToDegree(pos.lon),info.elv);
    295 
    296         if((info.sig==2 && alt_ref==0) || button_ref->Clicked()==true || take_ref==true)
    297         {
    298             Printf("prise pos ref\n");
    299             lat_ref=pos.lat;
    300             long_ref=pos.lon;
    301             alt_ref=info.elv;
    302             take_ref=false;
    303         }
    304         //if(alt_ref!=0)
    305         {
    306             double x,y,z;
    307             double e,n,u;
    308             Geographique_2_ECEF(pos.lon,pos.lat,info.elv,x,y,z);
    309             ECEF_2_ENU(x,y,z,e, n, u,long_ref,lat_ref,alt_ref);
    310             //Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
    311 
    312             //on prend une fois pour toute le mutex et on fait des accès directs
    313             output->GetMutex();
    314             output->SetValueNoMutex( 0, 0,e);
    315             output->SetValueNoMutex( 1, 0,n);
    316             output->SetValueNoMutex( 2, 0,u);
    317 
    318             int index=3;
    319             if((NMEAFlags&VTG)!=0)
    320             {
    321                 output->SetValueNoMutex( index, 0,info.speed*1000./3600.*sin(Euler::ToRadian(info.direction)));
    322                 output->SetValueNoMutex( index+1, 0,info.speed*1000./3600.*cos(Euler::ToRadian(info.direction)));
    323                 index+=2;
    324             }
    325             if((NMEAFlags&GST)!=0)
    326             {
    327                 //Thread::Printf("dev_lon:%f dev_lat:%f dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
    328                 output->SetValueNoMutex( index, 0,info.dev_lat);
    329                 output->SetValueNoMutex( index+1, 0,info.dev_lon);
    330                 output->SetValueNoMutex( index+2, 0,info.dev_elv);
    331                 index+=3;
    332             }
    333             output->ReleaseMutex();
    334 
    335             output->SetDataTime(GetTime());
    336             ProcessUpdate(output);
    337         }
    338     }
     254      double x, y, z;
     255      double e, n, u;
     256      Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
     257      ECEF_2_ENU(x, y, z, e, n, u, long_ref, lat_ref, alt_ref);
     258      // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
     259
     260      // on prend une fois pour toute le mutex et on fait des accès directs
     261      output->GetMutex();
     262      output->SetValueNoMutex(0, 0, e);
     263      output->SetValueNoMutex(1, 0, n);
     264      output->SetValueNoMutex(2, 0, u);
     265
     266      int index = 3;
     267      if ((NMEAFlags & VTG) != 0) {
     268        output->SetValueNoMutex(index, 0,
     269                                info.speed * 1000. / 3600. *
     270                                    sin(Euler::ToRadian(info.direction)));
     271        output->SetValueNoMutex(index + 1, 0,
     272                                info.speed * 1000. / 3600. *
     273                                    cos(Euler::ToRadian(info.direction)));
     274        index += 2;
     275      }
     276      if ((NMEAFlags & GST) != 0) {
     277        // Thread::Printf("dev_lon:%f dev_lat:%f
     278        // dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
     279        output->SetValueNoMutex(index, 0, info.dev_lat);
     280        output->SetValueNoMutex(index + 1, 0, info.dev_lon);
     281        output->SetValueNoMutex(index + 2, 0, info.dev_elv);
     282        index += 3;
     283      }
     284      output->ReleaseMutex();
     285
     286      output->SetDataTime(GetTime());
     287      ProcessUpdate(output);
     288    }
     289  }
    339290}
    340291

  • trunk/lib/FlairSensorActuator/src/Gps.h

    r3 r15  
    1717#include <nmea/nmea.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class cvmatrix;
    24         class FrameworkManager;
    25         class GeoCoordinate;
    26         class Vector3D;
    27     }
    28     namespace gui
    29     {
    30         class Layout;
    31         class DataPlot1D;
    32         class Tab;
    33         class TabWidget;
    34         class PushButton;
    35         class Map;
    36         class Label;
    37     }
     19namespace flair {
     20namespace core {
     21class cvmatrix;
     22class FrameworkManager;
     23class GeoCoordinate;
     24class Vector3D;
    3825}
    39 
    40 namespace flair
    41 {
    42 namespace sensor
    43 {
    44     /*! \class Gps
    45     *
    46     * \brief Base class for GPS
    47     */
    48     class Gps : public core::IODevice
    49     {
    50         public:
    51             /*!
    52             \enum FixQuality_t
    53             \brief Fix qualty indicators
    54             */
    55             enum class FixQuality_t {
    56                 Invalid=0,/*!< invalid */
    57                 Gps=1,/*!< Gps */
    58                 DGps=2,/*!< Differential Gps */
    59                 Pps=3,/*!< Pps */
    60                 Rtk=4,/*!< RTK */
    61                 RtkFloat=5,/*!< RTK float */
    62                 Estimated=6,/*!< Estimated */
    63                 Manual=7,/*!< Manual */
    64                 Simulation=8,/*!< Simulation */
    65                 };
    66 
    67             /*!
    68             \enum NMEAFlags_t
    69             \brief NMEA flags
    70             */
    71             enum NMEAFlags_t {
    72                 GGA=0x01,/*!< GGA */
    73                 VTG=0x02,/*!< VTG */
    74                 GST=0x04,/*!< GST */
    75                 };
    76 
    77             /*!
    78             * \brief Constructor
    79             *
    80             * Construct a Gps.
    81             *
    82             * \param parent parent
    83             * \param name name
    84             * \param NMEAFlags NMEA sentances to enable
    85             */
    86             Gps(const core::FrameworkManager* parent,std::string name,NMEAFlags_t NMEAFlags);
    87 
    88             /*!
    89             * \brief Destructor
    90             *
    91             */
    92             ~Gps();
    93 
    94             /*!
    95             * \brief Use default plot
    96             *
    97             */
    98             void UseDefaultPlot(void);
    99 
    100             /*!
    101             * \brief East plot
    102             *
    103             * \return east plot
    104             */
    105             gui::DataPlot1D* EPlot(void) const;
    106 
    107             /*!
    108             * \brief North plot
    109             *
    110             * \return north plot
    111             */
    112             gui::DataPlot1D* NPlot(void) const;
    113 
    114             /*!
    115             * \brief Up plot
    116             *
    117             * \return up plot
    118             */
    119             gui::DataPlot1D* UPlot(void) const;
    120 
    121             /*!
    122             * \brief East velocity plot
    123             *
    124             * \return east velocity plot
    125             */
    126             gui::DataPlot1D* VEPlot(void) const;
    127 
    128             /*!
    129             * \brief North velocity plot
    130             *
    131             * \return north velocity plot
    132             */
    133             gui::DataPlot1D* VNPlot(void) const;
    134 
    135             /*!
    136             * \brief Main tab
    137             *
    138             * \return main tab
    139             */
    140             gui::TabWidget* GetTab(void) const;
    141 
    142             /*!
    143             * \brief Setup Layout
    144             *
    145             * \return setup Layout
    146             */
    147             gui::Layout* GetLayout(void) const;
    148 
    149             /*!
    150             * \brief Plot tab
    151             *
    152             * \return plot Tab
    153             */
    154             gui::Tab* GetPlotTab(void) const;
    155 
    156             /*!
    157             * \brief Number of used satellites
    158             *
    159             * \return number of used satellites
    160             */
    161             uint16_t NbSat(void) const;
    162 
    163             /*!
    164             * \brief Fix Quality
    165             *
    166             * \return fix quality
    167             */
    168             FixQuality_t FixQuality(void) const;
    169 
    170             /*!
    171             * \brief Set reference for ENU coordinates
    172             *
    173             * The actual position is used as reference to calculate
    174             * ENU coordinates.
    175             *
    176             * \return fix quality
    177             */
    178             void SetRef(void);
    179 
    180             /*!
    181             * \brief Get ENU position
    182             *
    183             * \param point to store position
    184             */
    185             void GetENUPosition(core::Vector3D *point);
    186 
    187         protected:
    188             /*!
    189             * \brief Parse a NMEA frame
    190             *
    191             * This function must be called by the reimplemented class. \n
    192             * When a frame is parsed, GPS datas are filled.
    193             *
    194             * \param frame NMEA frame
    195             * \param frame_size frame size
    196             *
    197             */
    198             void parseFrame(const char *frame, int frame_size);
    199 
    200             NMEAFlags_t NMEAFlags;
    201 
    202         protected:
    203             core::GeoCoordinate *position;
    204 
    205         private:
    206             /*!
    207             * \brief Update using provided datas
    208             *
    209             * Reimplemented from IODevice.
    210             *
    211             * \param data data from the parent to process
    212             */
    213             void UpdateFrom(const core::io_data *data){};
    214 
    215             gui::Tab *main_tab,*sensor_tab;
    216             gui::TabWidget* tab;
    217             gui::PushButton *button_ref;
    218             gui::DataPlot1D* e_plot;
    219             gui::DataPlot1D* n_plot;
    220             gui::DataPlot1D* u_plot;
    221             gui::DataPlot1D* ve_plot;
    222             gui::DataPlot1D* vn_plot;
    223             gui::Tab* plot_tab;
    224             gui::Map *map;
    225             gui::Label *nb_sat_label,*fix_label;
    226             uint16_t nb_sat;
    227             FixQuality_t fix;
    228             bool take_ref;
    229             nmeaINFO info;
    230             nmeaPARSER parser;
    231             nmeaGPGGA pack;
    232             nmeaPOS pos;
    233             double lat_ref,long_ref,alt_ref;
    234 
    235             //matrix
    236             core::cvmatrix *output;
    237     };
     26namespace gui {
     27class Layout;
     28class DataPlot1D;
     29class Tab;
     30class TabWidget;
     31class PushButton;
     32class Map;
     33class Label;
     34}
     35}
     36
     37namespace flair {
     38namespace sensor {
     39/*! \class Gps
     40*
     41* \brief Base class for GPS
     42*/
     43class Gps : public core::IODevice {
     44public:
     45  /*!
     46  \enum FixQuality_t
     47  \brief Fix qualty indicators
     48  */
     49  enum class FixQuality_t {
     50    Invalid = 0,    /*!< invalid */
     51    Gps = 1,        /*!< Gps */
     52    DGps = 2,       /*!< Differential Gps */
     53    Pps = 3,        /*!< Pps */
     54    Rtk = 4,        /*!< RTK */
     55    RtkFloat = 5,   /*!< RTK float */
     56    Estimated = 6,  /*!< Estimated */
     57    Manual = 7,     /*!< Manual */
     58    Simulation = 8, /*!< Simulation */
     59  };
     60
     61  /*!
     62  \enum NMEAFlags_t
     63  \brief NMEA flags
     64  */
     65  enum NMEAFlags_t {
     66    GGA = 0x01, /*!< GGA */
     67    VTG = 0x02, /*!< VTG */
     68    GST = 0x04, /*!< GST */
     69  };
     70
     71  /*!
     72  * \brief Constructor
     73  *
     74  * Construct a Gps.
     75  *
     76  * \param parent parent
     77  * \param name name
     78  * \param NMEAFlags NMEA sentances to enable
     79  */
     80  Gps(const core::FrameworkManager *parent, std::string name,
     81      NMEAFlags_t NMEAFlags);
     82
     83  /*!
     84  * \brief Destructor
     85  *
     86  */
     87  ~Gps();
     88
     89  /*!
     90  * \brief Use default plot
     91  *
     92  */
     93  void UseDefaultPlot(void);
     94
     95  /*!
     96  * \brief East plot
     97  *
     98  * \return east plot
     99  */
     100  gui::DataPlot1D *EPlot(void) const;
     101
     102  /*!
     103  * \brief North plot
     104  *
     105  * \return north plot
     106  */
     107  gui::DataPlot1D *NPlot(void) const;
     108
     109  /*!
     110  * \brief Up plot
     111  *
     112  * \return up plot
     113  */
     114  gui::DataPlot1D *UPlot(void) const;
     115
     116  /*!
     117  * \brief East velocity plot
     118  *
     119  * \return east velocity plot
     120  */
     121  gui::DataPlot1D *VEPlot(void) const;
     122
     123  /*!
     124  * \brief North velocity plot
     125  *
     126  * \return north velocity plot
     127  */
     128  gui::DataPlot1D *VNPlot(void) const;
     129
     130  /*!
     131  * \brief Main tab
     132  *
     133  * \return main tab
     134  */
     135  gui::TabWidget *GetTab(void) const;
     136
     137  /*!
     138  * \brief Setup Layout
     139  *
     140  * \return setup Layout
     141  */
     142  gui::Layout *GetLayout(void) const;
     143
     144  /*!
     145  * \brief Plot tab
     146  *
     147  * \return plot Tab
     148  */
     149  gui::Tab *GetPlotTab(void) const;
     150
     151  /*!
     152  * \brief Number of used satellites
     153  *
     154  * \return number of used satellites
     155  */
     156  uint16_t NbSat(void) const;
     157
     158  /*!
     159  * \brief Fix Quality
     160  *
     161  * \return fix quality
     162  */
     163  FixQuality_t FixQuality(void) const;
     164
     165  /*!
     166  * \brief Set reference for ENU coordinates
     167  *
     168  * The actual position is used as reference to calculate
     169  * ENU coordinates.
     170  *
     171  * \return fix quality
     172  */
     173  void SetRef(void);
     174
     175  /*!
     176  * \brief Get ENU position
     177  *
     178  * \param point to store position
     179  */
     180  void GetENUPosition(core::Vector3D *point);
     181
     182protected:
     183  /*!
     184  * \brief Parse a NMEA frame
     185  *
     186  * This function must be called by the reimplemented class. \n
     187  * When a frame is parsed, GPS datas are filled.
     188  *
     189  * \param frame NMEA frame
     190  * \param frame_size frame size
     191  *
     192  */
     193  void parseFrame(const char *frame, int frame_size);
     194
     195  NMEAFlags_t NMEAFlags;
     196
     197protected:
     198  core::GeoCoordinate *position;
     199
     200private:
     201  /*!
     202  * \brief Update using provided datas
     203  *
     204  * Reimplemented from IODevice.
     205  *
     206  * \param data data from the parent to process
     207  */
     208  void UpdateFrom(const core::io_data *data){};
     209
     210  gui::Tab *main_tab, *sensor_tab;
     211  gui::TabWidget *tab;
     212  gui::PushButton *button_ref;
     213  gui::DataPlot1D *e_plot;
     214  gui::DataPlot1D *n_plot;
     215  gui::DataPlot1D *u_plot;
     216  gui::DataPlot1D *ve_plot;
     217  gui::DataPlot1D *vn_plot;
     218  gui::Tab *plot_tab;
     219  gui::Map *map;
     220  gui::Label *nb_sat_label, *fix_label;
     221  uint16_t nb_sat;
     222  FixQuality_t fix;
     223  bool take_ref;
     224  nmeaINFO info;
     225  nmeaPARSER parser;
     226  nmeaGPGGA pack;
     227  nmeaPOS pos;
     228  double lat_ref, long_ref, alt_ref;
     229
     230  // matrix
     231  core::cvmatrix *output;
     232};
    238233} // end namespace sensor
    239234} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp

    r3 r15  
    2525using namespace flair::core;
    2626
    27 namespace flair { namespace sensor {
     27namespace flair {
     28namespace sensor {
    2829
    29 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) {
    30     pimpl_=new Gx3_25_imu_impl(this,name,serialport,command,GetGroupBox());
     30Gx3_25_imu::Gx3_25_imu(const FrameworkManager *parent, string name,
     31                       SerialPort *serialport, Command_t command,
     32                       uint8_t priority)
     33    : Imu(parent, name), Thread(parent, name, priority) {
     34  pimpl_ = new Gx3_25_imu_impl(this, name, serialport, command, GetGroupBox());
    3135
    32     if(command==EulerAnglesAndAngularRates) {
     36  if (command == EulerAnglesAndAngularRates) {
    3337
    34     } else if(command==AccelerationAngularRateAndOrientationMatrix) {
    35         Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
    36     } else {
    37         Thread::Err("command not supported (%i)\n",command);
    38     }
    39     //AddDataToLog(imudata);
     38  } else if (command == AccelerationAngularRateAndOrientationMatrix) {
     39    Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
     40  } else {
     41    Thread::Err("command not supported (%i)\n", command);
     42  }
     43  // AddDataToLog(imudata);
    4044}
    4145
    4246Gx3_25_imu::~Gx3_25_imu() {
    43     SafeStop();
    44     Join();
    45     delete pimpl_;
     47  SafeStop();
     48  Join();
     49  delete pimpl_;
    4650}
    4751
    48 void Gx3_25_imu::Run(void) {
    49     pimpl_->Run();
    50 }
     52void Gx3_25_imu::Run(void) { pimpl_->Run(); }
    5153
    5254} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu.h

    r3 r15  
    1818
    1919namespace flair {
    20     namespace core {
    21         class FrameworkManager;
    22         class SerialPort;
    23     }
     20namespace core {
     21class FrameworkManager;
     22class SerialPort;
     23}
    2424}
    2525
    2626class Gx3_25_imu_impl;
    2727
    28 namespace flair { namespace sensor {
    29     /*! \class Gx3_25_imu
    30     *
    31     * \brief Class for 3dmgx3-25 Imu
    32     */
    33     class Gx3_25_imu : public Imu, public core::Thread {
    34         friend class ::Gx3_25_imu_impl;
     28namespace flair {
     29namespace sensor {
     30/*! \class Gx3_25_imu
     31*
     32* \brief Class for 3dmgx3-25 Imu
     33*/
     34class Gx3_25_imu : public Imu, public core::Thread {
     35  friend class ::Gx3_25_imu_impl;
    3536
    36         public:
    37             /*!
    38             \enum Command_t
    39             \brief Command for the continuous mode
    40             */
    41             enum Command_t{
    42                 EulerAnglesAndAngularRates=0xcf,/*!< Euler angles and angular rates */
    43                 AccelerationAngularRateAndOrientationMatrix=0xc8,/*!< Acceleration, angular rate and orientation matrix */
    44                 };
     37public:
     38  /*!
     39  \enum Command_t
     40  \brief Command for the continuous mode
     41  */
     42  enum Command_t {
     43    EulerAnglesAndAngularRates = 0xcf, /*!< Euler angles and angular rates */
     44    AccelerationAngularRateAndOrientationMatrix =
     45        0xc8, /*!< Acceleration, angular rate and orientation matrix */
     46  };
    4547
    46             /*!
    47             * \brief Constructor
    48             *
    49             * Construct a Gx3_25_imu.
    50             *
    51             * \param parent parent
    52             * \param name name
    53             * \param serialport SerialPort
    54             * \param command command for continuous mode
    55             * \param priority priority of the Thread
    56             */
    57             Gx3_25_imu(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Command_t command,uint8_t priority);
     48  /*!
     49  * \brief Constructor
     50  *
     51  * Construct a Gx3_25_imu.
     52  *
     53  * \param parent parent
     54  * \param name name
     55  * \param serialport SerialPort
     56  * \param command command for continuous mode
     57  * \param priority priority of the Thread
     58  */
     59  Gx3_25_imu(const core::FrameworkManager *parent, std::string name,
     60             core::SerialPort *serialport, Command_t command, uint8_t priority);
    5861
    59             /*!
    60             * \brief Destructor
    61             *
    62             */
    63             ~Gx3_25_imu();
     62  /*!
     63  * \brief Destructor
     64  *
     65  */
     66  ~Gx3_25_imu();
    6467
    65         private:
    66             /*!
    67             * \brief Run function
    68             *
    69             * Reimplemented from Thread.
    70             *
    71             */
    72             void Run(void);
     68private:
     69  /*!
     70  * \brief Run function
     71  *
     72  * Reimplemented from Thread.
     73  *
     74  */
     75  void Run(void);
    7376
    74             /*!
    75             * \brief Update using provided datas
    76             *
    77             * Reimplemented from IODevice.
    78             *
    79             * \param data data from the parent to process
    80             */
    81             void UpdateFrom(const core::io_data *data){};
     77  /*!
     78  * \brief Update using provided datas
     79  *
     80  * Reimplemented from IODevice.
     81  *
     82  * \param data data from the parent to process
     83  */
     84  void UpdateFrom(const core::io_data *data){};
    8285
    83             class Gx3_25_imu_impl* pimpl_;
    84     };
     86  class Gx3_25_imu_impl *pimpl_;
     87};
    8588} // end namespace sensor
    8689} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

    r3 r15  
    3838using namespace flair::sensor;
    3939
    40 Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu* self,string name,SerialPort *serialport,Gx3_25_imu::Command_t command,GroupBox* setupgroupbox) {
    41     int err = 0;
    42 
    43     this->self=self;
    44     this->command=(uint8_t)command;
    45     this->setupgroupbox=setupgroupbox;
    46     this->serialport=serialport;
    47 
    48     ahrsData=new AhrsData((Imu*)self);
    49 
    50     //station sol
    51     button_bias=new PushButton(setupgroupbox->NewRow(),"gyros bias");
    52     data_rate=new SpinBox(setupgroupbox->NewRow(),"data rate (Hz):",1,500,1,200);
    53     data_rate_label=new Label(setupgroupbox->LastRowLastCol(),"data_rate");
    54     gyro_acc_size=new SpinBox(setupgroupbox->NewRow(),"gyro and acc filter win size:",1,32,1,15);
    55     mag_size=new SpinBox(setupgroupbox->LastRowLastCol(),"mag filter win size:",1,32,1,17);
    56     up_comp=new SpinBox(setupgroupbox->NewRow(),"up compensation (s):",1,1000,1,10);
    57     north_comp=new SpinBox(setupgroupbox->LastRowLastCol(),"north compensation (s):",1,1000,1,10);
    58     coning=new CheckBox(setupgroupbox->NewRow(),"enable Coning&Sculling:",true);
    59     disable_magn=new CheckBox(setupgroupbox->LastRowLastCol(),"disable magnetometer:",true);
    60     disable_north_comp=new CheckBox(setupgroupbox->NewRow(),"disable magnetic north compensation:",false);
    61     disable_grav_comp=new CheckBox(setupgroupbox->NewRow(),"disable gravity compensation:",false);
    62 }
    63 
    64 Gx3_25_imu_impl::~Gx3_25_imu_impl() {
    65 }
     40Gx3_25_imu_impl::Gx3_25_imu_impl(Gx3_25_imu *self, string name,
     41                                 SerialPort *serialport,
     42                                 Gx3_25_imu::Command_t command,
     43                                 GroupBox *setupgroupbox) {
     44  int err = 0;
     45
     46  this->self = self;
     47  this->command = (uint8_t)command;
     48  this->setupgroupbox = setupgroupbox;
     49  this->serialport = serialport;
     50
     51  ahrsData = new AhrsData((Imu *)self);
     52
     53  // station sol
     54  button_bias = new PushButton(setupgroupbox->NewRow(), "gyros bias");
     55  data_rate =
     56      new SpinBox(setupgroupbox->NewRow(), "data rate (Hz):", 1, 500, 1, 200);
     57  data_rate_label = new Label(setupgroupbox->LastRowLastCol(), "data_rate");
     58  gyro_acc_size = new SpinBox(setupgroupbox->NewRow(),
     59                              "gyro and acc filter win size:", 1, 32, 1, 15);
     60  mag_size = new SpinBox(setupgroupbox->LastRowLastCol(),
     61                         "mag filter win size:", 1, 32, 1, 17);
     62  up_comp = new SpinBox(setupgroupbox->NewRow(), "up compensation (s):", 1,
     63                        1000, 1, 10);
     64  north_comp = new SpinBox(setupgroupbox->LastRowLastCol(),
     65                           "north compensation (s):", 1, 1000, 1, 10);
     66  coning =
     67      new CheckBox(setupgroupbox->NewRow(), "enable Coning&Sculling:", true);
     68  disable_magn = new CheckBox(setupgroupbox->LastRowLastCol(),
     69                              "disable magnetometer:", true);
     70  disable_north_comp = new CheckBox(
     71      setupgroupbox->NewRow(), "disable magnetic north compensation:", false);
     72  disable_grav_comp = new CheckBox(setupgroupbox->NewRow(),
     73                                   "disable gravity compensation:", false);
     74}
     75
     76Gx3_25_imu_impl::~Gx3_25_imu_impl() {}
    6677
    6778void Gx3_25_imu_impl::Run(void) {
    68     Time imuTime;
    69     ImuData* imuData;
    70     self->GetDatas(&imuData);
    71 
    72     self->WarnUponSwitches(true);
    73 
    74     //reset IMU to be sure it is at 115200
    75     Printf("Gx3_25_imu::Reset IMU at 921600\n");
    76     serialport->SetBaudrate(921600);
    77     DeviceReset();
    78     self->Thread::SleepMS(100);
    79     serialport->FlushInput();
    80     serialport->SetBaudrate(115200);
    81 
    82     SetBaudrate(921600);
    83     Printf("Gx3_25_imu firmware number: %i\n",FirmwareNumber());
    84     PrintModelInfo();
    85     SamplingSettings();
    86     GyrosBias();
    87     //RealignUpNorth(true,true);
    88 
    89     //on provoque les 9 ValueChanged
    90     for(int i=0; i<9; i++) {
    91         if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
    92                 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
    93             if(setupgroupbox->isEnabled()==true) SamplingSettings();
    94         }
    95     }
    96 
    97     //periode a passer an argument (reglable)
    98     //ou plutot laisser la periode geree par le centrale (polling)
    99     //self->SetPeriodMS(2);
    100     SetContinuousMode(command);
    101 
    102     //_printf("firmware version: %i\n",get_firmware_number());
    103 
    104     while(!self->ToBeStopped()) {
    105         if(command==Gx3_25_imu::EulerAnglesAndAngularRates) {
    106             uint8_t response[31] = {0};
    107             uint8_t *buf=&response[1];
    108             GetData(response,sizeof(response),&imuTime);
    109 
    110             Euler eulerAngles;
    111             eulerAngles.roll=Dequeue(&buf);
    112             eulerAngles.pitch=Dequeue(&buf);
    113             eulerAngles.yaw=Dequeue(&buf);
    114 
    115             Vector3D filteredAngRates;
    116             filteredAngRates.x=Dequeue(&buf);
    117             filteredAngRates.y=Dequeue(&buf);
    118             filteredAngRates.z=Dequeue(&buf);
    119 
    120             ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),filteredAngRates);
    121         } else if(command==Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) {
    122             uint8_t response[67] = {0};
    123             uint8_t *buf=&response[1];
    124             GetData(response,sizeof(response),&imuTime);
    125 
    126             Vector3D rawAcc;
    127             rawAcc.x=9.80665*Dequeue(&buf);
    128             rawAcc.y=9.80665*Dequeue(&buf);
    129             rawAcc.z=9.80665*Dequeue(&buf);
    130 
    131             Vector3D filteredAngRates;
    132             filteredAngRates.x=Dequeue(&buf);
    133             filteredAngRates.y=Dequeue(&buf);
    134             filteredAngRates.z=Dequeue(&buf);
    135 
    136             RotationMatrix matrix;
    137             matrix(0,0)=Dequeue(&buf);
    138             matrix(0,1)=Dequeue(&buf);
    139             matrix(0,2)=Dequeue(&buf);
    140             matrix(1,0)=Dequeue(&buf);
    141             matrix(1,1)=Dequeue(&buf);
    142             matrix(1,2)=Dequeue(&buf);
    143             matrix(2,0)=Dequeue(&buf);
    144             matrix(2,1)=Dequeue(&buf);
    145             matrix(2,2)=Dequeue(&buf);
    146 
    147             ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),filteredAngRates);
    148             imuData->SetRawAcc(rawAcc);
    149         }
    150 
    151         //change settings as soon as possible
    152         //we assume that new imu datas will not come so fast
    153         //so newt message from imu is an ack from the change settings
    154         if(button_bias->Clicked()==true) GyrosBias();
    155 
    156         if(data_rate->ValueChanged()==true || disable_magn->ValueChanged()==true ||disable_north_comp->ValueChanged()==true ||disable_grav_comp->ValueChanged()==true ||coning->ValueChanged()==true
    157                 || gyro_acc_size->ValueChanged()==true || mag_size->ValueChanged()==true ||up_comp->ValueChanged()==true || north_comp->ValueChanged()==true) {
    158             if(setupgroupbox->isEnabled()==true) SamplingSettings();
    159         }
    160 
    161         imuData->SetDataTime(imuTime);
    162         ahrsData->SetDataTime(imuTime);
    163         self->UpdateImu();
    164         self->ProcessUpdate(ahrsData);
    165     }
    166     SetContinuousMode(0);
    167     SetBaudrate(115200);
    168 
    169     self->WarnUponSwitches(false);
    170 }
    171 
    172 void Gx3_25_imu_impl::GetData(uint8_t* buf,ssize_t buf_size,Time *time) {
     79  Time imuTime;
     80  ImuData *imuData;
     81  self->GetDatas(&imuData);
     82
     83  self->WarnUponSwitches(true);
     84
     85  // reset IMU to be sure it is at 115200
     86  Printf("Gx3_25_imu::Reset IMU at 921600\n");
     87  serialport->SetBaudrate(921600);
     88  DeviceReset();
     89  self->Thread::SleepMS(100);
     90  serialport->FlushInput();
     91  serialport->SetBaudrate(115200);
     92
     93  SetBaudrate(921600);
     94  Printf("Gx3_25_imu firmware number: %i\n", FirmwareNumber());
     95  PrintModelInfo();
     96  SamplingSettings();
     97  GyrosBias();
     98  // RealignUpNorth(true,true);
     99
     100  // on provoque les 9 ValueChanged
     101  for (int i = 0; i < 9; i++) {
     102    if (data_rate->ValueChanged() == true ||
     103        disable_magn->ValueChanged() == true ||
     104        disable_north_comp->ValueChanged() == true ||
     105        disable_grav_comp->ValueChanged() == true ||
     106        coning->ValueChanged() == true ||
     107        gyro_acc_size->ValueChanged() == true ||
     108        mag_size->ValueChanged() == true || up_comp->ValueChanged() == true ||
     109        north_comp->ValueChanged() == true) {
     110      if (setupgroupbox->isEnabled() == true)
     111        SamplingSettings();
     112    }
     113  }
     114
     115  // periode a passer an argument (reglable)
     116  // ou plutot laisser la periode geree par le centrale (polling)
     117  // self->SetPeriodMS(2);
     118  SetContinuousMode(command);
     119
     120  //_printf("firmware version: %i\n",get_firmware_number());
     121
     122  while (!self->ToBeStopped()) {
     123    if (command == Gx3_25_imu::EulerAnglesAndAngularRates) {
     124      uint8_t response[31] = {0};
     125      uint8_t *buf = &response[1];
     126      GetData(response, sizeof(response), &imuTime);
     127
     128      Euler eulerAngles;
     129      eulerAngles.roll = Dequeue(&buf);
     130      eulerAngles.pitch = Dequeue(&buf);
     131      eulerAngles.yaw = Dequeue(&buf);
     132
     133      Vector3D filteredAngRates;
     134      filteredAngRates.x = Dequeue(&buf);
     135      filteredAngRates.y = Dequeue(&buf);
     136      filteredAngRates.z = Dequeue(&buf);
     137
     138      ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),
     139                                             filteredAngRates);
     140    } else if (command ==
     141               Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix) {
     142      uint8_t response[67] = {0};
     143      uint8_t *buf = &response[1];
     144      GetData(response, sizeof(response), &imuTime);
     145
     146      Vector3D rawAcc;
     147      rawAcc.x = 9.80665 * Dequeue(&buf);
     148      rawAcc.y = 9.80665 * Dequeue(&buf);
     149      rawAcc.z = 9.80665 * Dequeue(&buf);
     150
     151      Vector3D filteredAngRates;
     152      filteredAngRates.x = Dequeue(&buf);
     153      filteredAngRates.y = Dequeue(&buf);
     154      filteredAngRates.z = Dequeue(&buf);
     155
     156      RotationMatrix matrix;
     157      matrix(0, 0) = Dequeue(&buf);
     158      matrix(0, 1) = Dequeue(&buf);
     159      matrix(0, 2) = Dequeue(&buf);
     160      matrix(1, 0) = Dequeue(&buf);
     161      matrix(1, 1) = Dequeue(&buf);
     162      matrix(1, 2) = Dequeue(&buf);
     163      matrix(2, 0) = Dequeue(&buf);
     164      matrix(2, 1) = Dequeue(&buf);
     165      matrix(2, 2) = Dequeue(&buf);
     166
     167      ahrsData->SetQuaternionAndAngularRates(matrix.ToEuler().ToQuaternion(),
     168                                             filteredAngRates);
     169      imuData->SetRawAcc(rawAcc);
     170    }
     171
     172    // change settings as soon as possible
     173    // we assume that new imu datas will not come so fast
     174    // so newt message from imu is an ack from the change settings
     175    if (button_bias->Clicked() == true)
     176      GyrosBias();
     177
     178    if (data_rate->ValueChanged() == true ||
     179        disable_magn->ValueChanged() == true ||
     180        disable_north_comp->ValueChanged() == true ||
     181        disable_grav_comp->ValueChanged() == true ||
     182        coning->ValueChanged() == true ||
     183        gyro_acc_size->ValueChanged() == true ||
     184        mag_size->ValueChanged() == true || up_comp->ValueChanged() == true ||
     185        north_comp->ValueChanged() == true) {
     186      if (setupgroupbox->isEnabled() == true)
     187        SamplingSettings();
     188    }
     189
     190    imuData->SetDataTime(imuTime);
     191    ahrsData->SetDataTime(imuTime);
     192    self->UpdateImu();
     193    self->ProcessUpdate(ahrsData);
     194  }
     195  SetContinuousMode(0);
     196  SetBaudrate(115200);
     197
     198  self->WarnUponSwitches(false);
     199}
     200
     201void Gx3_25_imu_impl::GetData(uint8_t *buf, ssize_t buf_size, Time *time) {
     202  ssize_t read = 0;
     203  ssize_t written = 0;
     204  /*
     205      written = serialport->Write(&command, sizeof(command));
     206      if(written<0)
     207      {
     208          self->Thread::Err("Write error (%s)\n",strerror(-written));
     209      }
     210      else if (written != sizeof(command))
     211      {
     212          self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
     213      }
     214  */
     215  read = serialport->Read(buf, buf_size);
     216  *time = GetTime();
     217  if (read < 0) {
     218    self->Thread::Err("Read error (%s)\n", strerror(-read));
     219  } else if (read != buf_size) {
     220    self->Thread::Err("Read error %i/%i\n", read, buf_size);
     221  }
     222
     223  if (CalcChecksum(buf, buf_size) == false) {
     224    self->Thread::Err("wrong checksum\n");
     225    return;
     226  }
     227}
     228
     229float Gx3_25_imu_impl::Dequeue(uint8_t **buf) {
     230  union float_4uint8 {
     231    float f;
     232    uint8_t b[4];
     233  } float_value;
     234
     235  float_value.b[0] = (*buf)[3];
     236  float_value.b[1] = (*buf)[2];
     237  float_value.b[2] = (*buf)[1];
     238  float_value.b[3] = (*buf)[0];
     239  (*buf) += sizeof(float);
     240
     241  return float_value.f;
     242}
     243
     244void Gx3_25_imu_impl::GyrosBias(void) {
     245  if (setupgroupbox->isEnabled() == true) { // devrait toujours etre bon
     246    uint8_t response[19] = {0};
     247    uint8_t command[5];
    173248    ssize_t read = 0;
    174249    ssize_t written = 0;
    175     /*
    176         written = serialport->Write(&command, sizeof(command));
    177         if(written<0)
    178         {
    179             self->Thread::Err("Write error (%s)\n",strerror(-written));
    180         }
    181         else if (written != sizeof(command))
    182         {
    183             self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    184         }
    185     */
    186     read = serialport->Read(buf,buf_size);
    187     *time=GetTime();
    188     if(read<0) {
    189         self->Thread::Err("Read error (%s)\n",strerror(-read));
    190     } else if (read != buf_size) {
    191         self->Thread::Err("Read error %i/%i\n",read,buf_size);
    192     }
    193 
    194     if(CalcChecksum(buf,buf_size)==false) {
    195         self->Thread::Err("wrong checksum\n");
    196         return;
    197     }
    198 }
    199 
    200 float Gx3_25_imu_impl::Dequeue(uint8_t** buf) {
    201     union float_4uint8 {
    202         float f;
    203         uint8_t b[4];
    204     }
    205     float_value;
    206 
    207     float_value.b[0]=(*buf)[3];
    208     float_value.b[1]=(*buf)[2];
    209     float_value.b[2]=(*buf)[1];
    210     float_value.b[3]=(*buf)[0];
    211     (*buf)+=sizeof(float);
    212 
    213     return float_value.f;
    214 }
    215 
    216 void Gx3_25_imu_impl::GyrosBias(void) {
    217     if(setupgroupbox->isEnabled()==true) { //devrait toujours etre bon
    218         uint8_t response[19] = {0};
    219         uint8_t command[5];
    220         ssize_t read = 0;
    221         ssize_t written = 0;
    222 
    223         Printf("Gx3_25_imu::gyros_bias: %s\n",self->IODevice::ObjectName().c_str());
    224 
    225         command[0]=0xcd;//entete
    226         command[1]=0xc1;//confirm
    227         command[2]=0x29;//confirm
    228         command[3]=0x27;//time MSB en us
    229         command[4]=0x10;//time LSB en us
    230 
    231         written = serialport->Write( &command, sizeof(command));
    232         if(written<0) {
    233             self->Thread::Err("Write error (%s)\n",strerror(-written));
    234         } else if (written != sizeof(command)) {
    235             self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    236         }
    237 
    238         self->SleepUS(1.1*(command[3]*256+command[4]));//on fait un sleep un peu plus grand
    239 
    240         read = serialport->Read(&response[0],sizeof(response));
    241         if(read<0) {
    242             self->Thread::Err("Read error (%s)\n",strerror(-read));
    243         } else if (read != sizeof(response)) {
    244             self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    245         }
    246 
    247         if(CalcChecksum(response,sizeof(response))==false) {
    248             self->Thread::Err("wrong checksum\n");
    249             //return -1;
    250         }
    251 
    252         Printf("Gx3_25_imu::gyros_bias: %s ok\n",self->IODevice::ObjectName().c_str());
    253 
    254     } else {
    255         self->Thread::Err("error locked\n");
    256     }
     250
     251    Printf("Gx3_25_imu::gyros_bias: %s\n",
     252           self->IODevice::ObjectName().c_str());
     253
     254    command[0] = 0xcd; // entete
     255    command[1] = 0xc1; // confirm
     256    command[2] = 0x29; // confirm
     257    command[3] = 0x27; // time MSB en us
     258    command[4] = 0x10; // time LSB en us
     259
     260    written = serialport->Write(&command, sizeof(command));
     261    if (written < 0) {
     262      self->Thread::Err("Write error (%s)\n", strerror(-written));
     263    } else if (written != sizeof(command)) {
     264      self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     265    }
     266
     267    self->SleepUS(1.1 * (command[3] * 256 +
     268                         command[4])); // on fait un sleep un peu plus grand
     269
     270    read = serialport->Read(&response[0], sizeof(response));
     271    if (read < 0) {
     272      self->Thread::Err("Read error (%s)\n", strerror(-read));
     273    } else if (read != sizeof(response)) {
     274      self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     275    }
     276
     277    if (CalcChecksum(response, sizeof(response)) == false) {
     278      self->Thread::Err("wrong checksum\n");
     279      // return -1;
     280    }
     281
     282    Printf("Gx3_25_imu::gyros_bias: %s ok\n",
     283           self->IODevice::ObjectName().c_str());
     284
     285  } else {
     286    self->Thread::Err("error locked\n");
     287  }
    257288}
    258289
    259290void Gx3_25_imu_impl::SetContinuousMode(uint8_t continuous_command) {
    260     uint8_t response[8] = {0};
    261     uint8_t command[4];
    262     ssize_t read = 0;
    263     ssize_t written = 0;
    264 
    265     command[0]=0xc4;//entete
    266     command[1]=0xc1;//confirm
    267     command[2]=0x29;//confirm
    268     command[3]=continuous_command;
    269 
    270     written = serialport->Write(command,sizeof(command));
    271     if(written<0) {
    272         self->Thread::Err("Write error (%s)\n",strerror(-written));
     291  uint8_t response[8] = {0};
     292  uint8_t command[4];
     293  ssize_t read = 0;
     294  ssize_t written = 0;
     295
     296  command[0] = 0xc4; // entete
     297  command[1] = 0xc1; // confirm
     298  command[2] = 0x29; // confirm
     299  command[3] = continuous_command;
     300
     301  written = serialport->Write(command, sizeof(command));
     302  if (written < 0) {
     303    self->Thread::Err("Write error (%s)\n", strerror(-written));
     304  } else if (written != sizeof(command)) {
     305    self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     306  }
     307
     308  read = serialport->Read(response, sizeof(response));
     309  if (read < 0) {
     310    self->Thread::Err("Read error (%s)\n", strerror(-read));
     311  } else if (read != sizeof(response)) {
     312    self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     313  }
     314
     315  if (CalcChecksum(response, sizeof(response)) == false) {
     316    self->Thread::Err("wrong checksum\n", self->IODevice::ObjectName().c_str());
     317  }
     318}
     319
     320void Gx3_25_imu_impl::SamplingSettings(void) {
     321  uint8_t response[19] = {0};
     322  uint8_t command[20];
     323  uint8_t result;
     324  ssize_t read = 0;
     325  ssize_t written = 0;
     326
     327  uint16_t rate = 1000 / data_rate->Value();
     328
     329  command[0] = 0xdb;               // entete
     330  command[1] = 0xa8;               // confirm
     331  command[2] = 0xb9;               // confirm
     332  command[3] = 1;                  // change values
     333  command[4] = (rate >> 8) & 0xff; // data rate MSB
     334  command[5] = rate & 0xff;        // data rate LSB
     335  result = 0;
     336  if (disable_magn->IsChecked() == true)
     337    result |= 0x01;
     338  if (disable_north_comp->IsChecked() == true)
     339    result |= 0x04;
     340  if (disable_grav_comp->IsChecked() == true)
     341    result |= 0x08;
     342
     343  command[6] = result;
     344  result = 0x01; // Calculate orientation
     345  if (coning->IsChecked() == true)
     346    result |= 0x02;
     347
     348  command[7] = result;
     349  command[8] = gyro_acc_size->Value();             // gyro acc filter window
     350  command[9] = mag_size->Value();                  // mag filter window
     351  command[10] = (up_comp->Value() >> 8) & 0xff;    // up comp MSB
     352  command[11] = up_comp->Value() & 0xff;           // up comp LSB
     353  command[12] = (north_comp->Value() >> 8) & 0xff; // north comp MSB
     354  command[13] = north_comp->Value() & 0xff;        // north comp LSB
     355  command[14] = 0;                                 // reserved
     356  command[15] = 0;                                 // reserved
     357  command[16] = 0;                                 // reserved
     358  command[17] = 0;                                 // reserved
     359  command[18] = 0;                                 // reserved
     360  command[19] = 0;                                 // reserved
     361
     362  written = serialport->Write(&command, sizeof(command));
     363  if (written < 0) {
     364    self->Thread::Err("Write error (%s)\n", strerror(-written));
     365  } else if (written != sizeof(command)) {
     366    self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     367  }
     368
     369  read = serialport->Read(&response[0], sizeof(response));
     370  if (read < 0) {
     371    self->Thread::Err("Read error (%s)\n", strerror(-read));
     372  } else if (read != sizeof(response)) {
     373    self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     374  }
     375
     376  if (CalcChecksum(response, sizeof(response)) == false) {
     377    self->Thread::Err("wrong checksum\n", self->IODevice::ObjectName().c_str());
     378  } else {
     379    data_rate_label->SetText("real: %.2fHz", 1000. / rate);
     380  }
     381}
     382
     383void Gx3_25_imu_impl::SetBaudrate(int value) {
     384  uint8_t response[10] = {0};
     385  uint8_t command[11];
     386  ssize_t read = 0;
     387  ssize_t written = 0;
     388
     389  union int32_4uint8 {
     390    int32_t i;
     391    uint8_t b[4];
     392  } baudrate_value;
     393
     394  baudrate_value.i = value;
     395  Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n",
     396         self->IODevice::ObjectName().c_str(), baudrate_value.i);
     397
     398  command[0] = 0xd9; // entete
     399  command[1] = 0xc3; // confirm
     400  command[2] = 0x55; // confirm
     401  command[3] = 1;    // primary uart
     402  command[4] = 1;    // chgt temporaire
     403  command[5] = baudrate_value.b[3];
     404  command[6] = baudrate_value.b[2];
     405  command[7] = baudrate_value.b[1];
     406  command[8] = baudrate_value.b[0];
     407  command[9] = 2;  // uart enabled
     408  command[10] = 0; // reserved
     409
     410  written = serialport->Write(&command, sizeof(command));
     411  if (written < 0) {
     412    self->Thread::Err("Write error (%s)\n", strerror(-written));
     413  } else if (written != sizeof(command)) {
     414    self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     415  }
     416
     417  read = serialport->Read(&response[0], sizeof(response));
     418  if (read < 0) {
     419    self->Thread::Err("Read error (%s)\n", strerror(-read));
     420  } else if (read != sizeof(response)) {
     421    self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     422  }
     423
     424  if (CalcChecksum(response, sizeof(response)) == false) {
     425    self->Thread::Err("wrong checksum\n");
     426    return;
     427  }
     428
     429  serialport->SetBaudrate(value);
     430}
     431
     432int Gx3_25_imu_impl::FirmwareNumber(void) {
     433  uint8_t response[7] = {0};
     434  uint8_t command;
     435  ssize_t read = 0;
     436  ssize_t written = 0;
     437  union int32_4uint8 {
     438    int32_t i;
     439    uint8_t b[4];
     440  } value;
     441
     442  command = 0xe9; // entete
     443
     444  written = serialport->Write(&command, sizeof(command));
     445  if (written < 0) {
     446    self->Thread::Err("Write error (%s)\n", strerror(-written));
     447  } else if (written != sizeof(command)) {
     448    self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     449  }
     450
     451  read = serialport->Read(&response[0], sizeof(response));
     452  if (read < 0) {
     453    self->Thread::Err("Read error (%s)\n", strerror(-read));
     454  } else if (read != sizeof(response)) {
     455    self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     456  }
     457
     458  if (CalcChecksum(response, sizeof(response)) == false) {
     459    self->Thread::Err("wrong checksum\n");
     460    return -1;
     461  }
     462
     463  value.b[3] = response[1];
     464  value.b[2] = response[2];
     465  value.b[1] = response[3];
     466  value.b[0] = response[4];
     467
     468  return value.i;
     469}
     470
     471void Gx3_25_imu_impl::PrintModelInfo(void) {
     472  uint8_t response[20] = {0};
     473  uint8_t command[2];
     474  ssize_t read = 0;
     475  ssize_t written = 0;
     476
     477  for (int i = 0; i < 3; i++) {
     478    command[0] = 0xea; // entete
     479    command[1] = i;    // entete
     480
     481    written = serialport->Write(&command, sizeof(command));
     482    if (written < 0) {
     483      self->Thread::Err("Write error (%s)\n", strerror(-written));
    273484    } else if (written != sizeof(command)) {
    274         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    275     }
    276 
    277     read = serialport->Read(response,sizeof(response));
    278     if(read<0) {
    279         self->Thread::Err("Read error (%s)\n",strerror(-read));
     485      self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     486    }
     487
     488    read = serialport->Read(&response[0], sizeof(response));
     489    if (read < 0) {
     490      self->Thread::Err("Read error (%s)\n", strerror(-read));
    280491    } else if (read != sizeof(response)) {
    281         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    282     }
    283 
    284     if(CalcChecksum(response,sizeof(response))==false) {
    285         self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
    286     }
    287 }
    288 
    289 void Gx3_25_imu_impl::SamplingSettings(void) {
    290     uint8_t response[19] = {0};
    291     uint8_t command[20];
    292     uint8_t result;
    293     ssize_t read = 0;
    294     ssize_t written = 0;
    295 
    296     uint16_t rate=1000/data_rate->Value();
    297 
    298     command[0]=0xdb;//entete
    299     command[1]=0xa8;//confirm
    300     command[2]=0xb9;//confirm
    301     command[3]=1;//change values
    302     command[4]=(rate>>8)&0xff;//data rate MSB
    303     command[5]=rate&0xff;//data rate LSB
    304     result=0;
    305     if(disable_magn->IsChecked()==true) result|=0x01;
    306     if(disable_north_comp->IsChecked()==true) result|=0x04;
    307     if(disable_grav_comp->IsChecked()==true) result|=0x08;
    308 
    309     command[6]=result;
    310     result=0x01;//Calculate orientation
    311     if(coning->IsChecked()==true) result|=0x02;
    312 
    313     command[7]=result;
    314     command[8]=gyro_acc_size->Value();//gyro acc filter window
    315     command[9]=mag_size->Value();//mag filter window
    316     command[10]=(up_comp->Value()>>8)&0xff;//up comp MSB
    317     command[11]=up_comp->Value()&0xff;//up comp LSB
    318     command[12]=(north_comp->Value()>>8)&0xff;//north comp MSB
    319     command[13]=north_comp->Value()&0xff;//north comp LSB
    320     command[14]=0;//reserved
    321     command[15]=0;//reserved
    322     command[16]=0;//reserved
    323     command[17]=0;//reserved
    324     command[18]=0;//reserved
    325     command[19]=0;//reserved
    326 
    327     written = serialport->Write(&command, sizeof(command));
    328     if(written<0) {
    329         self->Thread::Err("Write error (%s)\n",strerror(-written));
    330     } else if (written != sizeof(command)) {
    331         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    332     }
    333 
    334     read = serialport->Read(&response[0],sizeof(response));
    335     if(read<0) {
    336         self->Thread::Err("Read error (%s)\n",strerror(-read));
    337     } else if (read != sizeof(response)) {
    338         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    339     }
    340 
    341     if(CalcChecksum(response,sizeof(response))==false) {
    342         self->Thread::Err("wrong checksum\n",self->IODevice::ObjectName().c_str());
    343     } else {
    344         data_rate_label->SetText("real: %.2fHz",1000./rate);
    345     }
    346 }
    347 
    348 void Gx3_25_imu_impl::SetBaudrate(int value) {
    349     uint8_t response[10] = {0};
    350     uint8_t command[11];
    351     ssize_t read = 0;
    352     ssize_t written = 0;
    353 
    354     union int32_4uint8 {
    355         int32_t i;
    356         uint8_t b[4];
    357     }
    358     baudrate_value;
    359 
    360     baudrate_value.i=value;
    361     Printf("Gx3_25_imu::SetBaudrate: %s ->%i\n",self->IODevice::ObjectName().c_str(),baudrate_value.i);
    362 
    363     command[0]=0xd9;//entete
    364     command[1]=0xc3;//confirm
    365     command[2]=0x55;//confirm
    366     command[3]=1;//primary uart
    367     command[4]=1;//chgt temporaire
    368     command[5]=baudrate_value.b[3];
    369     command[6]=baudrate_value.b[2];
    370     command[7]=baudrate_value.b[1];
    371     command[8]=baudrate_value.b[0];
    372     command[9]=2;//uart enabled
    373     command[10]=0;//reserved
    374 
    375     written = serialport->Write(&command, sizeof(command));
    376     if(written<0) {
    377         self->Thread::Err("Write error (%s)\n",strerror(-written));
    378     } else if (written != sizeof(command)) {
    379         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    380     }
    381 
    382     read = serialport->Read(&response[0],sizeof(response));
    383     if(read<0) {
    384         self->Thread::Err("Read error (%s)\n",strerror(-read));
    385     } else if (read != sizeof(response)) {
    386         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    387     }
    388 
    389     if(CalcChecksum(response,sizeof(response))==false) {
    390         self->Thread::Err("wrong checksum\n");
    391         return ;
    392     }
    393 
    394     serialport->SetBaudrate(value);
    395 }
    396 
    397 int Gx3_25_imu_impl::FirmwareNumber(void) {
    398     uint8_t response[7] = {0};
    399     uint8_t command;
    400     ssize_t read = 0;
    401     ssize_t written = 0;
    402     union int32_4uint8 {
    403         int32_t i;
    404         uint8_t b[4];
    405     }
    406     value;
    407 
    408     command=0xe9;//entete
    409 
    410     written = serialport->Write(&command, sizeof(command));
    411     if(written<0) {
    412         self->Thread::Err("Write error (%s)\n",strerror(-written));
    413     } else if (written != sizeof(command)) {
    414         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    415     }
    416 
    417     read = serialport->Read(&response[0],sizeof(response));
    418     if(read<0) {
    419         self->Thread::Err("Read error (%s)\n",strerror(-read));
    420     } else if (read != sizeof(response)) {
    421         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    422     }
    423 
    424     if(CalcChecksum(response,sizeof(response))==false) {
    425         self->Thread::Err("wrong checksum\n");
    426         return -1;
    427     }
    428 
    429     value.b[3]=response[1];
    430     value.b[2]=response[2];
    431     value.b[1]=response[3];
    432     value.b[0]=response[4];
    433 
    434     return value.i;
    435 
    436 }
    437 
    438 void Gx3_25_imu_impl::PrintModelInfo(void) {
    439     uint8_t response[20] = {0};
    440     uint8_t command[2];
    441     ssize_t read = 0;
    442     ssize_t written = 0;
    443 
    444     for(int i=0; i<3; i++) {
    445         command[0]=0xea;//entete
    446         command[1]=i;//entete
    447 
    448         written = serialport->Write(&command, sizeof(command));
    449         if(written<0) {
    450             self->Thread::Err("Write error (%s)\n",strerror(-written));
    451         } else if (written != sizeof(command)) {
    452             self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    453         }
    454 
    455         read = serialport->Read(&response[0],sizeof(response));
    456         if(read<0) {
    457             self->Thread::Err("Read error (%s)\n",strerror(-read));
    458         } else if (read != sizeof(response)) {
    459             self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    460         }
    461 
    462         if(CalcChecksum(response,sizeof(response))==false) {
    463             self->Thread::Err("wrong checksum\n");
    464             //return -1;
    465         }
    466 
    467         char* msg=(char*)(&response[2]);
    468         msg[16]=0;
    469         switch(i) {
    470         case 0:
    471             Printf("Gx3_25_imu model number: %s\n",msg);
    472             break;
    473         case 1:
    474             Printf("Gx3_25_imu serial number: %s\n",msg);
    475             break;
    476         case 2:
    477             Printf("Gx3_25_imu model name: %s\n",msg);
    478             break;
    479         }
    480     }
    481 
    482 }
    483 
    484 void Gx3_25_imu_impl::RealignUpNorth(bool realign_up,bool realign_north) {
    485     uint8_t response[7] = {0};
    486     uint8_t command[10];
    487     ssize_t read = 0;
    488     ssize_t written = 0;
    489 
    490     command[0]=0xdd;//entete
    491     command[1]=0x54;//confirm
    492     command[2]=0x4c;//confirm
    493     command[3]=0;//send reply
    494     command[4]=255;//up realign
    495     command[5]=1;//north realign
    496     command[6]=0;//reserved
    497     command[7]=0;//reserved
    498     command[8]=0;//reserved
    499     command[9]=0;//reserved
    500 
    501     written = serialport->Write(&command, sizeof(command));
    502     if(written<0) {
    503         self->Thread::Err("Write error (%s)\n",strerror(-written));
    504     } else if (written != sizeof(command)) {
    505         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    506     }
    507 
    508     read = serialport->Read(&response[0],sizeof(response));
    509     if(read<0) {
    510         self->Thread::Err("Read error(%s)\n",strerror(-read));
    511     } else if (read != sizeof(response)) {
    512         self->Thread::Err("Read error %i/%i\n",read,sizeof(response));
    513     }
    514 
    515     if(CalcChecksum(response,sizeof(response))==false) {
    516         self->Thread::Err("wrong checksum\n");
    517     }
     492      self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     493    }
     494
     495    if (CalcChecksum(response, sizeof(response)) == false) {
     496      self->Thread::Err("wrong checksum\n");
     497      // return -1;
     498    }
     499
     500    char *msg = (char *)(&response[2]);
     501    msg[16] = 0;
     502    switch (i) {
     503    case 0:
     504      Printf("Gx3_25_imu model number: %s\n", msg);
     505      break;
     506    case 1:
     507      Printf("Gx3_25_imu serial number: %s\n", msg);
     508      break;
     509    case 2:
     510      Printf("Gx3_25_imu model name: %s\n", msg);
     511      break;
     512    }
     513  }
     514}
     515
     516void Gx3_25_imu_impl::RealignUpNorth(bool realign_up, bool realign_north) {
     517  uint8_t response[7] = {0};
     518  uint8_t command[10];
     519  ssize_t read = 0;
     520  ssize_t written = 0;
     521
     522  command[0] = 0xdd; // entete
     523  command[1] = 0x54; // confirm
     524  command[2] = 0x4c; // confirm
     525  command[3] = 0;    // send reply
     526  command[4] = 255;  // up realign
     527  command[5] = 1;    // north realign
     528  command[6] = 0;    // reserved
     529  command[7] = 0;    // reserved
     530  command[8] = 0;    // reserved
     531  command[9] = 0;    // reserved
     532
     533  written = serialport->Write(&command, sizeof(command));
     534  if (written < 0) {
     535    self->Thread::Err("Write error (%s)\n", strerror(-written));
     536  } else if (written != sizeof(command)) {
     537    self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     538  }
     539
     540  read = serialport->Read(&response[0], sizeof(response));
     541  if (read < 0) {
     542    self->Thread::Err("Read error(%s)\n", strerror(-read));
     543  } else if (read != sizeof(response)) {
     544    self->Thread::Err("Read error %i/%i\n", read, sizeof(response));
     545  }
     546
     547  if (CalcChecksum(response, sizeof(response)) == false) {
     548    self->Thread::Err("wrong checksum\n");
     549  }
    518550}
    519551
    520552void Gx3_25_imu_impl::DeviceReset(void) {
    521     uint8_t command[3];
    522     ssize_t written = 0;
    523 
    524     command[0]=0xfe;//entete
    525     command[1]=0x9e;//confirm
    526     command[2]=0x3a;//confirm
    527 
    528     written = serialport->Write(&command, sizeof(command));
    529     if(written<0) {
    530         self->Thread::Err("Write error (%s)\n",strerror(-written));
    531     } else if (written != sizeof(command)) {
    532         self->Thread::Err("Write error %i/%i\n",written,sizeof(command));
    533     }
    534 }
    535 
    536 bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf,int size) {
    537     uint16_t tChksum,tResponseChksum;
    538 
    539     tChksum = 0;
    540     for (int i = 0; i < size - 2; i++) tChksum += buf[i];
    541     // Extract the big-endian checksum from reply
    542     tResponseChksum = 0;
    543     tResponseChksum = buf[size - 2] << 8;
    544     tResponseChksum += buf[size - 1];
    545 
    546     if(tChksum!=tResponseChksum)
    547         return false;
    548     else
    549         return true;
    550 }
     553  uint8_t command[3];
     554  ssize_t written = 0;
     555
     556  command[0] = 0xfe; // entete
     557  command[1] = 0x9e; // confirm
     558  command[2] = 0x3a; // confirm
     559
     560  written = serialport->Write(&command, sizeof(command));
     561  if (written < 0) {
     562    self->Thread::Err("Write error (%s)\n", strerror(-written));
     563  } else if (written != sizeof(command)) {
     564    self->Thread::Err("Write error %i/%i\n", written, sizeof(command));
     565  }
     566}
     567
     568bool Gx3_25_imu_impl::CalcChecksum(uint8_t *buf, int size) {
     569  uint16_t tChksum, tResponseChksum;
     570
     571  tChksum = 0;
     572  for (int i = 0; i < size - 2; i++)
     573    tChksum += buf[i];
     574  // Extract the big-endian checksum from reply
     575  tResponseChksum = 0;
     576  tResponseChksum = buf[size - 2] << 8;
     577  tResponseChksum += buf[size - 1];
     578
     579  if (tChksum != tResponseChksum)
     580    return false;
     581  else
     582    return true;
     583}

  • trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp

    r3 r15  
    3232using namespace flair::gui;
    3333
    34 namespace flair
    35 {
    36 namespace sensor
    37 {
    38 
    39 HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager* parent,string name,SerialPort *serialport,uint8_t priority) : LaserRangeFinder(parent,name), Thread(parent,name,priority)
    40 {
    41     main_tab=new Tab(parent->GetTabWidget(),name);
    42         cvmatrix_descriptor* desc=new cvmatrix_descriptor(1081,1);
    43     output=new cvmatrix((IODevice*)this,desc,SignedIntegerType(16));
    44 
    45     bufRetMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"BufRetMut");
    46     sendingCmdMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"SendingCmdMut");
    47 
    48     this->serialport=serialport;
    49     serialport->SetRxTimeout(100000000);
    50 }
    51 
    52 HokuyoUTM30Lx::~HokuyoUTM30Lx()
    53 {
    54     delete main_tab;
    55     SafeStop();
    56     Join();
    57 }
    58 
    59 void HokuyoUTM30Lx::Run(void)
    60 {
    61         /** Debut init **/
    62     char c,lastC;
    63     int startStep;
    64     bool mustDecode=false;
    65     stringstream ss;
    66     vector<string> ret;
    67     lastC=c=0;
    68 
    69         /** Fin init **/
    70         resetConfig();
    71         startLaser();
    72         Printf("Laser started\r\n");
    73 
    74     /** Debut running loop **/
    75     WarnUponSwitches(true);
    76 
    77     while(!ToBeStopped())
    78     {
    79         ss.clear();
    80         ss.str("");
    81         do{
    82             lastC=c;
    83             serialport->Read(&c,1);
    84             ss << c;
    85         }while(!(c=='\n'&&lastC=='\n'));
    86         ret=explode(ss.str(),'\n');
    87         if(!checkSum(ret.at(1)))
    88             perror("Bad Checksum !");
    89         if(ret.at(0).substr(0,2)=="MD")
    90             mustDecode=true;
    91 
    92         startStep=atoi(ret.at(0).substr(2,4).c_str());
    93         ret.erase(ret.begin());
    94         bufRetMut->GetMutex();
    95         bufRet.push(ret.at(0).substr(0,2));
    96         bufRetMut->ReleaseMutex();
    97         if(mustDecode){
    98             mustDecode=false;
    99             ss.clear();
    100             ss.str("");
    101             do{
    102                 lastC=c;
    103                 serialport->Read(&c,1);
    104                 ss << c;
    105             }while(!(c=='\n'&&lastC=='\n'));
    106             ret=explode(ss.str(),'\n');
    107             ret.erase(ret.begin());
    108             ret.erase(ret.begin());
    109             if(ret.at(0).size()==5)
    110                 if(!checkSum(ret.at(0)))
    111                     perror("TimeStamp checksum error!");
    112             ret.erase(ret.begin());
    113             Printf("!post\r\n");
    114             ret.pop_back();
    115             decodeDatas(ret, startStep);
    116         }
     34namespace flair {
     35namespace sensor {
     36
     37HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager *parent, string name,
     38                             SerialPort *serialport, uint8_t priority)
     39    : LaserRangeFinder(parent, name), Thread(parent, name, priority) {
     40  main_tab = new Tab(parent->GetTabWidget(), name);
     41  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1081, 1);
     42  output = new cvmatrix((IODevice *)this, desc, SignedIntegerType(16));
     43
     44  bufRetMut =
     45      new Mutex(reinterpret_cast<Object *>(this), (string)name + "BufRetMut");
     46  sendingCmdMut = new Mutex(reinterpret_cast<Object *>(this),
     47                            (string)name + "SendingCmdMut");
     48
     49  this->serialport = serialport;
     50  serialport->SetRxTimeout(100000000);
     51}
     52
     53HokuyoUTM30Lx::~HokuyoUTM30Lx() {
     54  delete main_tab;
     55  SafeStop();
     56  Join();
     57}
     58
     59void HokuyoUTM30Lx::Run(void) {
     60  /** Debut init **/
     61  char c, lastC;
     62  int startStep;
     63  bool mustDecode = false;
     64  stringstream ss;
     65  vector<string> ret;
     66  lastC = c = 0;
     67
     68  /** Fin init **/
     69  resetConfig();
     70  startLaser();
     71  Printf("Laser started\r\n");
     72
     73  /** Debut running loop **/
     74  WarnUponSwitches(true);
     75
     76  while (!ToBeStopped()) {
     77    ss.clear();
     78    ss.str("");
     79    do {
     80      lastC = c;
     81      serialport->Read(&c, 1);
     82      ss << c;
     83    } while (!(c == '\n' && lastC == '\n'));
     84    ret = explode(ss.str(), '\n');
     85    if (!checkSum(ret.at(1)))
     86      perror("Bad Checksum !");
     87    if (ret.at(0).substr(0, 2) == "MD")
     88      mustDecode = true;
     89
     90    startStep = atoi(ret.at(0).substr(2, 4).c_str());
     91    ret.erase(ret.begin());
     92    bufRetMut->GetMutex();
     93    bufRet.push(ret.at(0).substr(0, 2));
     94    bufRetMut->ReleaseMutex();
     95    if (mustDecode) {
     96      mustDecode = false;
     97      ss.clear();
     98      ss.str("");
     99      do {
     100        lastC = c;
     101        serialport->Read(&c, 1);
     102        ss << c;
     103      } while (!(c == '\n' && lastC == '\n'));
     104      ret = explode(ss.str(), '\n');
     105      ret.erase(ret.begin());
     106      ret.erase(ret.begin());
     107      if (ret.at(0).size() == 5)
     108        if (!checkSum(ret.at(0)))
     109          perror("TimeStamp checksum error!");
     110      ret.erase(ret.begin());
     111      Printf("!post\r\n");
     112      ret.pop_back();
     113      decodeDatas(ret, startStep);
    117114    }
    118     /** fin running loop **/
    119     stopLaser();
    120     WarnUponSwitches(false);
    121 }
    122 
    123 
    124 void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep){
    125             stringstream ss;
    126             for (unsigned i=0; i<datas.size(); i++){
    127                 if(datas.at(i).size()!=0){
    128                     if(!checkSum(datas.at(i)))
    129                         perror("Datas checksum error !");
    130                     datas.at(i).erase(datas.at(i).end()-1);
    131                     ss << datas.at(i);
    132                 }
    133             }
    134             output->GetMutex();
    135             for(unsigned i=0;i<1080;i++){
    136             //TODO: remettre -1 pour les points non lus
    137                 output->SetValueNoMutex(i,0,0);
    138             }
    139             for(unsigned i=0;i<ss.str().size();i+=3){
    140                 output->SetValueNoMutex(startStep+(i/3),0,decode3car(ss.str().substr(i,3).c_str()));
    141             }
    142             UpdateFrom(output);
    143             ProcessUpdate(output);
    144             output->ReleaseMutex();
    145 
    146 }
    147 
    148 bool HokuyoUTM30Lx::checkSum(string data){
    149     return (char)encodeSum(data.substr(0,data.size()-1).c_str(),data.size()-1)==data.at(data.size()-1);
    150 }
    151 
    152 void HokuyoUTM30Lx::startLaser(){
    153     string ret = sendCommand("BM");
    154     #ifdef VERBOSE
    155         if(ret == "00"){
    156             cout << "BM: Alright !" << endl;
    157         }else if(ret == "01"){
    158             cout << "BM: Laser malfunction !" << endl;
    159         }else if(ret == "02"){
    160             cout << "BM: Laser already started !" << endl;
    161         }
    162     #endif
    163 }
    164 
    165 void HokuyoUTM30Lx::stopLaser(){
    166     sendCommand("QT");
    167 }
    168 
    169 void HokuyoUTM30Lx::resetConfig(){
    170     sendCommand("RS");
    171 }
    172 
    173 vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter){
    174     istringstream split(str);
    175     vector<string> tokens;
    176     for (string each; getline(split, each, delimiter); tokens.push_back(each));
    177     return tokens;
    178 }
    179 
    180 int HokuyoUTM30Lx::encodeSum(const char* code, int byte) {
    181     unsigned char value = 0;
    182     int i;
    183     for (i = 0; i < byte; ++i) {
    184         value+=code[i];
     115  }
     116  /** fin running loop **/
     117  stopLaser();
     118  WarnUponSwitches(false);
     119}
     120
     121void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep) {
     122  stringstream ss;
     123  for (unsigned i = 0; i < datas.size(); i++) {
     124    if (datas.at(i).size() != 0) {
     125      if (!checkSum(datas.at(i)))
     126        perror("Datas checksum error !");
     127      datas.at(i).erase(datas.at(i).end() - 1);
     128      ss << datas.at(i);
    185129    }
    186     value &= 0x3f;
    187     value += 0x30;
    188     return value;
    189 }
    190 
    191 float HokuyoUTM30Lx::decode2car(const char* data){
    192     return ((data[0]-0x30)<<6)|((data[1]-0x30));
    193 }
    194 float HokuyoUTM30Lx::decode3car(const char* data){
    195     return ((data[0]-0x30)<<12)|((data[1]-0x30)<<6)|((data[2]-0x30));
    196 }
    197 float HokuyoUTM30Lx::decode4car(const char* data){
    198     return ((data[0]-0x30)<<18)|((data[1]-0x30)<<12)|((data[2]-0x30)<<6)|((data[3]-0x30));
    199 }
    200 
    201 string HokuyoUTM30Lx::sendCommand(string command){
    202     char c;
    203     string ret="00";
    204     c='\n';
    205     sendingCmdMut->GetMutex();
    206     serialport->Write(command.c_str(),command.size());
    207     serialport->Write(&c,1);
    208     sendingCmdMut->ReleaseMutex();
    209     return ret;
    210 }
    211 
    212 void HokuyoUTM30Lx::getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber){
    213     stringstream ss;
    214     string ret;
    215     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;
    216     ret = sendCommand(ss.str());
    217     #ifdef VERBOSE
    218         if(ret == "00"){
    219             cout << "MD: Alright !" << endl;
    220         }else if(ret == "01"){
    221             cout << "MD: Laser malfunction !" << endl;
    222         }else if(ret == "02"){
    223             cout << "MD: End Step has non-numeric value !" << endl;
    224         }else if(ret == "03"){
    225             cout << "MD: Cluster Count has non-numeric value !" << endl;
    226         }else if(ret == "04"){
    227             cout << "MD: End Step is out of range !" << endl;
    228         }else if(ret == "05"){
    229             cout << "MD: End Step is smaller than Starting Step !" << endl;
    230         }else if(ret == "06"){
    231             cout << "MD: Scan Interval has non-numeric value !" << endl;
    232         }else if(ret == "07"){
    233             cout << "MD: Number of Scan has non-numeric value !" << endl;
    234         }else if(ret == "98"){
    235             cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx operation." << endl;
    236         }else{
    237             /* Concerne :
    238             21~49 --- Processing stopped to verify the error.
    239             50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
    240             cout << "MD: Malfunction !" << endl;
    241         }
    242     #endif
    243     }
    244 cvmatrix* HokuyoUTM30Lx::getDatas(){
    245     return output;
    246 }
    247 
    248 void HokuyoUTM30Lx::UseDefaultPlot(void)
    249 {
    250     plot=new RangeFinderPlot(main_tab->NewRow(),"plot","x",-100,100,"y",-0,100,output,-45,225,output->Rows());
     130  }
     131  output->GetMutex();
     132  for (unsigned i = 0; i < 1080; i++) {
     133    // TODO: remettre -1 pour les points non lus
     134    output->SetValueNoMutex(i, 0, 0);
     135  }
     136  for (unsigned i = 0; i < ss.str().size(); i += 3) {
     137    output->SetValueNoMutex(startStep + (i / 3), 0,
     138                            decode3car(ss.str().substr(i, 3).c_str()));
     139  }
     140  UpdateFrom(output);
     141  ProcessUpdate(output);
     142  output->ReleaseMutex();
     143}
     144
     145bool HokuyoUTM30Lx::checkSum(string data) {
     146  return (char)encodeSum(data.substr(0, data.size() - 1).c_str(),
     147                         data.size() - 1) == data.at(data.size() - 1);
     148}
     149
     150void HokuyoUTM30Lx::startLaser() {
     151  string ret = sendCommand("BM");
     152#ifdef VERBOSE
     153  if (ret == "00") {
     154    cout << "BM: Alright !" << endl;
     155  } else if (ret == "01") {
     156    cout << "BM: Laser malfunction !" << endl;
     157  } else if (ret == "02") {
     158    cout << "BM: Laser already started !" << endl;
     159  }
     160#endif
     161}
     162
     163void HokuyoUTM30Lx::stopLaser() { sendCommand("QT"); }
     164
     165void HokuyoUTM30Lx::resetConfig() { sendCommand("RS"); }
     166
     167vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter) {
     168  istringstream split(str);
     169  vector<string> tokens;
     170  for (string each; getline(split, each, delimiter); tokens.push_back(each))
     171    ;
     172  return tokens;
     173}
     174
     175int HokuyoUTM30Lx::encodeSum(const char *code, int byte) {
     176  unsigned char value = 0;
     177  int i;
     178  for (i = 0; i < byte; ++i) {
     179    value += code[i];
     180  }
     181  value &= 0x3f;
     182  value += 0x30;
     183  return value;
     184}
     185
     186float HokuyoUTM30Lx::decode2car(const char *data) {
     187  return ((data[0] - 0x30) << 6) | ((data[1] - 0x30));
     188}
     189float HokuyoUTM30Lx::decode3car(const char *data) {
     190  return ((data[0] - 0x30) << 12) | ((data[1] - 0x30) << 6) |
     191         ((data[2] - 0x30));
     192}
     193float HokuyoUTM30Lx::decode4car(const char *data) {
     194  return ((data[0] - 0x30) << 18) | ((data[1] - 0x30) << 12) |
     195         ((data[2] - 0x30) << 6) | ((data[3] - 0x30));
     196}
     197
     198string HokuyoUTM30Lx::sendCommand(string command) {
     199  char c;
     200  string ret = "00";
     201  c = '\n';
     202  sendingCmdMut->GetMutex();
     203  serialport->Write(command.c_str(), command.size());
     204  serialport->Write(&c, 1);
     205  sendingCmdMut->ReleaseMutex();
     206  return ret;
     207}
     208
     209void HokuyoUTM30Lx::getMesure(int startStep, int endStep, int clusterCount,
     210                              int interval, int scanNumber) {
     211  stringstream ss;
     212  string ret;
     213  ss << "MD" << std::setfill('0') << std::setw(4) << startStep << std::setw(4)
     214     << endStep << std::setw(2) << clusterCount << std::setw(1) << interval
     215     << std::setw(2) << scanNumber;
     216  ret = sendCommand(ss.str());
     217#ifdef VERBOSE
     218  if (ret == "00") {
     219    cout << "MD: Alright !" << endl;
     220  } else if (ret == "01") {
     221    cout << "MD: Laser malfunction !" << endl;
     222  } else if (ret == "02") {
     223    cout << "MD: End Step has non-numeric value !" << endl;
     224  } else if (ret == "03") {
     225    cout << "MD: Cluster Count has non-numeric value !" << endl;
     226  } else if (ret == "04") {
     227    cout << "MD: End Step is out of range !" << endl;
     228  } else if (ret == "05") {
     229    cout << "MD: End Step is smaller than Starting Step !" << endl;
     230  } else if (ret == "06") {
     231    cout << "MD: Scan Interval has non-numeric value !" << endl;
     232  } else if (ret == "07") {
     233    cout << "MD: Number of Scan has non-numeric value !" << endl;
     234  } else if (ret == "98") {
     235    cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx "
     236            "operation." << endl;
     237  } else {
     238    /* Concerne :
     239    21~49 --- Processing stopped to verify the error.
     240    50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
     241    cout << "MD: Malfunction !" << endl;
     242  }
     243#endif
     244}
     245cvmatrix *HokuyoUTM30Lx::getDatas() { return output; }
     246
     247void HokuyoUTM30Lx::UseDefaultPlot(void) {
     248  plot = new RangeFinderPlot(main_tab->NewRow(), "plot", "x", -100, 100, "y",
     249                             -0, 100, output, -45, 225, output->Rows());
    251250}
    252251

  • trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h

    r3 r15  
    2121#include <vector>
    2222
    23 namespace flair
    24 {
    25     namespace core
    26     {
    27         class cvmatrix;
    28         class FrameworkManager;
    29         class SerialPort;
    30         class Mutex;
    31     }
    32     namespace gui
    33     {
    34         class Tab;
    35         class TabWidget;
    36         class RangeFinderPlot;
    37     }
     23namespace flair {
     24namespace core {
     25class cvmatrix;
     26class FrameworkManager;
     27class SerialPort;
     28class Mutex;
     29}
     30namespace gui {
     31class Tab;
     32class TabWidget;
     33class RangeFinderPlot;
     34}
    3835}
    3936
    40 namespace flair
    41 {
    42 namespace sensor
    43 {
    44     /*! \class HokuyoUTM30Lx
    45     *
    46     * \brief Classe intégrant le telemetre laser Hokuyo UTM 30lx
    47     */
    48     class HokuyoUTM30Lx : public core::Thread, public LaserRangeFinder
    49     {
    50         public:
     37namespace flair {
     38namespace sensor {
     39/*! \class HokuyoUTM30Lx
     40*
     41* \brief Classe intégrant le telemetre laser Hokuyo UTM 30lx
     42*/
     43class HokuyoUTM30Lx : public core::Thread, public LaserRangeFinder {
     44public:
     45  /*!
     46 * \brief Constructor
     47 *
     48 * Construct a Hokuyo UTM30-Lx.
     49 *
     50 * \param parent parent
     51 * \param name name
     52 * \param serialport serialport
     53 * \param priority priority of the Thread
     54 */
     55  HokuyoUTM30Lx(const core::FrameworkManager *parent, std::string name,
     56                core::SerialPort *serialport, uint8_t priority);
     57  void getMesure(int startStep, int endStep, int clusterCount, int interval,
     58                 int scanNumber = 0);
     59  core::cvmatrix *getDatas(void);
    5160
    52              /*!
    53             * \brief Constructor
    54             *
    55             * Construct a Hokuyo UTM30-Lx.
    56             *
    57             * \param parent parent
    58             * \param name name
    59             * \param serialport serialport
    60             * \param priority priority of the Thread
    61             */
    62             HokuyoUTM30Lx(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,uint8_t priority);
    63             void getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber=0);
    64             core::cvmatrix* getDatas(void);
     61  /*!
     62  * \brief Use default plot
     63  *
     64  */
     65  void UseDefaultPlot(void);
     66  /*!
     67  * \brief Destructor
     68  *
     69  */
     70  ~HokuyoUTM30Lx();
    6571
    66             /*!
    67             * \brief Use default plot
    68             *
    69             */
    70             void UseDefaultPlot(void);
    71             /*!
    72             * \brief Destructor
    73             *
    74             */
    75             ~HokuyoUTM30Lx();
     72private:
     73  core::SerialPort *serialport;
     74  core::Mutex *bufRetMut;
     75  core::Mutex *sendingCmdMut;
     76  gui::Tab *main_tab;
     77  gui::TabWidget *tab;
     78  gui::RangeFinderPlot *plot;
    7679
    77         private:
    78             core::SerialPort *serialport;
    79             core::Mutex* bufRetMut;
    80             core::Mutex* sendingCmdMut;
    81             gui::Tab* main_tab;
    82             gui::TabWidget* tab;
    83             gui::RangeFinderPlot* plot;
     80  // matrix
     81  core::cvmatrix *output;
    8482
    85             //matrix
    86             core::cvmatrix *output;
     83  std::queue<std::string> bufRet;
    8784
    88                         std::queue<std::string> bufRet;
     85  /*!
     86  * \brief Run function
     87  *
     88  * Reimplemented from Thread.
     89  *
     90  */
     91  void Run(void);
     92  /*!
     93  * \brief Send a command
     94  * \param command Command to send (see Hokuyo UTM 30-LX doc for more
     95  * informations)
     96  * \return Return code
     97  */
     98  std::string sendCommand(std::string command);
     99  /*!
     100  * \brief Start the laser
     101  *
     102  */
     103  void startLaser(void);
     104  /*!
     105  * \brief Stop the laser
     106  *
     107  */
     108  void stopLaser(void);
     109  /*!
     110* \brief Stop and reset the laser's settings
     111*
     112*/
     113  void resetConfig(void);
     114  /*!
     115* \brief Decode incomming datas
     116* \param datas Datas to decode
     117* \param startStep Set the first mesured point
     118* Decode mesured points from incoming datas and fill the output matrix
     119*/
     120  void decodeDatas(std::vector<std::string> datas, int startStep);
     121  /*!
     122  * \brief Explode a string into a vector
     123  * \param str The string to explode
     124  * \param delimiter The character separating elements
     125  * \return A vector containing the elements
     126  */
     127  static std::vector<std::string> explode(const std::string str,
     128                                          char delimiter);
     129  /*!
     130* \brief Calculate the checksum
     131* \param code Data from which calculate
     132* \param byte Data's size
     133* \return A character corresponding to the code's checksum
     134*/
     135  static int encodeSum(const char *code, int byte);
     136  /*!
     137* \brief Check if a data correspond to its checksum
     138* \param data Datas to check
     139*/
     140  static bool checkSum(std::string data);
     141  /*!
     142* \brief Decode datas using the 2 character encoding
     143* \param data Datas to decode
     144* \return Decoded datas
     145*/
     146  static float decode2car(const char *data);
     147  /*!
     148* \brief Decode datas using the 3 character encoding
     149* \param data Datas to decode
     150* \return Decoded datas
     151*/
     152  static float decode3car(const char *data);
     153  /*!
     154* \brief Decode datas using the 4 character encoding
     155* \param data Datas to decode
     156* \return Decoded datas
     157*/
     158  static float decode4car(const char *data);
    89159
    90             /*!
    91             * \brief Run function
    92             *
    93             * Reimplemented from Thread.
    94             *
    95             */
    96             void Run(void);
    97             /*!
    98             * \brief Send a command
    99             * \param command Command to send (see Hokuyo UTM 30-LX doc for more informations)
    100             * \return Return code
    101             */
    102             std::string sendCommand(std::string command);
    103             /*!
    104             * \brief Start the laser
    105             *
    106             */
    107             void startLaser(void);
    108             /*!
    109             * \brief Stop the laser
    110             *
    111             */
    112                         void stopLaser(void);
    113                         /*!
    114             * \brief Stop and reset the laser's settings
    115             *
    116             */
    117                     void resetConfig(void);
    118                     /*!
    119             * \brief Decode incomming datas
    120             * \param datas Datas to decode
    121             * \param startStep Set the first mesured point
    122             * Decode mesured points from incoming datas and fill the output matrix
    123             */
    124                     void decodeDatas(std::vector<std::string> datas, int startStep);
    125             /*!
    126             * \brief Explode a string into a vector
    127             * \param str The string to explode
    128             * \param delimiter The character separating elements
    129             * \return A vector containing the elements
    130             */
    131                         static std::vector<std::string> explode(const std::string str, char delimiter);
    132                         /*!
    133             * \brief Calculate the checksum
    134             * \param code Data from which calculate
    135             * \param byte Data's size
    136             * \return A character corresponding to the code's checksum
    137             */
    138                         static int encodeSum(const char* code, int byte);
    139                         /*!
    140             * \brief Check if a data correspond to its checksum
    141             * \param data Datas to check
    142             */
    143                         static bool checkSum(std::string data);
    144                         /*!
    145             * \brief Decode datas using the 2 character encoding
    146             * \param data Datas to decode
    147             * \return Decoded datas
    148             */
    149                         static float decode2car(const char* data);
    150                         /*!
    151             * \brief Decode datas using the 3 character encoding
    152             * \param data Datas to decode
    153             * \return Decoded datas
    154             */
    155                         static float decode3car(const char* data);
    156                         /*!
    157             * \brief Decode datas using the 4 character encoding
    158             * \param data Datas to decode
    159             * \return Decoded datas
    160             */
    161                         static float decode4car(const char* data);
    162 
    163                         /*!
    164             * \brief Update using provided datas
    165             *
    166             * Reimplemented from IODevice.
    167             *
    168             * \param data data from the parent to process
    169             */
    170             void UpdateFrom(const core::io_data *data){};
    171 
    172     };
     160  /*!
     161* \brief Update using provided datas
     162*
     163* Reimplemented from IODevice.
     164*
     165* \param data data from the parent to process
     166*/
     167  void UpdateFrom(const core::io_data *data){};
     168};
    173169} // end namespace sensor
    174170} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/HostEthController.cpp

    r3 r15  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
     13//  purpose:    Base class for host side remote controls that talks to target
     14//  side through ethernet connection
    1415//
    1516//
    … …  
    3738using std::string;
    3839
    39 namespace flair
    40 {
    41 namespace sensor
    42 {
    43 
    44 HostEthController::HostEthController(const FrameworkManager* parent,string name,string _address,int _port,uint32_t period,uint32_t _bitsPerAxis,uint8_t priority) :
    45     Thread(parent,name,priority),IODevice(parent,name),tab(new Tab(parent->GetTabWidget(),name)),axisNumber(0),buttonNumber(0),targetAddress(_address),targetPort(_port),
    46     bitsPerAxis(_bitsPerAxis),dataFrameBuffer(NULL),meaningfulDataAvailable(false) {
    47     tabWidget=new TabWidget(tab->NewRow(),name);
    48 
    49     const bool blocking=true;
    50     controlSocket=new TcpSocket((Thread*)this,"eth controller control socket", blocking, !blocking);
    51     dataSocket=new Socket((Thread*)this,"eth controller data socket",_address+":"+std::to_string(_port+1));
    52     dataSender=new DataSender((Thread*)this, this, "eth controller data sender thread");
    53     dataSender->SetPeriodMS(period);
    54     dataSender->Start();
    55 
    56     //test binary
    57     /*
    58     uint16_t testValue=0b0111011010; //0x1DA
    59     char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; // 0xAB65B6
    60     writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
    61     Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
    62     */
    63     connectionEstablishedMutex=new Mutex((Thread*)this);
     40namespace flair {
     41namespace sensor {
     42
     43HostEthController::HostEthController(const FrameworkManager *parent,
     44                                     string name, string _address, int _port,
     45                                     uint32_t period, uint32_t _bitsPerAxis,
     46                                     uint8_t priority)
     47    : Thread(parent, name, priority), IODevice(parent, name),
     48      tab(new Tab(parent->GetTabWidget(), name)), axisNumber(0),
     49      buttonNumber(0), targetAddress(_address), targetPort(_port),
     50      bitsPerAxis(_bitsPerAxis), dataFrameBuffer(NULL),
     51      meaningfulDataAvailable(false) {
     52  tabWidget = new TabWidget(tab->NewRow(), name);
     53
     54  const bool blocking = true;
     55  controlSocket = new TcpSocket((Thread *)this, "eth controller control socket",
     56                                blocking, !blocking);
     57  dataSocket = new Socket((Thread *)this, "eth controller data socket",
     58                          _address + ":" + std::to_string(_port + 1));
     59  dataSender =
     60      new DataSender((Thread *)this, this, "eth controller data sender thread");
     61  dataSender->SetPeriodMS(period);
     62  dataSender->Start();
     63
     64  // test binary
     65  /*
     66  uint16_t testValue=0b0111011010; //0x1DA
     67  char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; //
     68  0xAB65B6
     69  writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
     70  Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
     71  */
     72  connectionEstablishedMutex = new Mutex((Thread *)this);
    6473}
    6574
    6675HostEthController::~HostEthController() {
     76  SafeStop();
     77  Join();
     78
     79  if (!getFrameworkManager()->ConnectionLost())
     80    delete tab;
     81}
     82
     83void HostEthController::DrawUserInterface() {
     84  Tab *plotTab = new Tab(tabWidget, "Measures");
     85  axisPlot = new DataPlot1D *[axisNumber];
     86  for (unsigned int i = 0; i < axisNumber; i++) {
     87    // Start a new row or add up to the current row? We try to keep a 4/3 ratio
     88    unsigned int columns = sqrt(4.0 * axisNumber / 3.0);
     89    LayoutPosition *position;
     90    if (i % columns == 0) {
     91      position = plotTab->NewRow();
     92    } else {
     93      position = plotTab->LastRowLastCol();
     94    }
     95    axisPlot[i] = new DataPlot1D(position, axis->Name(i, 0),
     96                                 -(1 << (nativeBitsPerAxis - 1)) * 1.2,
     97                                 (1 << (nativeBitsPerAxis - 1)) * 1.5);
     98    axisPlot[i]->AddCurve(axis->Element(i));
     99  }
     100  // we don't plot the button state for now
     101}
     102
     103string HostEthController::GetAxisDescription(unsigned int axis) {
     104  return string("axis") + std::to_string(axis);
     105}
     106
     107string HostEthController::GetButtonDescription(unsigned int button) {
     108  return string("button") + std::to_string(button);
     109}
     110
     111bool HostEthController::ControllerInitialization() {
     112
     113  buttonOffset = (axisNumber * bitsPerAxis) / 8;
     114  if ((axisNumber * bitsPerAxis) % 8 != 0)
     115    buttonOffset++;
     116  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
     117  if ((buttonNumber % 8) != 0)
     118    dataFrameSize++;
     119  dataFrameBuffer = new char[dataFrameSize];
     120  return true;
     121}
     122
     123void HostEthController::SendControllerInfo() {
     124  // send axis info
     125  controlSocket->WriteUInt32((uint32_t)axisNumber, 0);
     126  controlSocket->WriteUInt32(bitsPerAxis, 0);
     127  for (unsigned int i = 0; i < axisNumber; i++) {
     128    // Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d
     129    // bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
     130    int stringLength = GetAxisDescription(i).length();
     131    controlSocket->WriteUInt32((uint32_t)stringLength, 0);
     132    controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength, 0);
     133  }
     134
     135  // send button info
     136  controlSocket->WriteUInt32((uint32_t)buttonNumber, 0);
     137  for (unsigned int i = 0; i < buttonNumber; i++) {
     138    int stringLength = GetButtonDescription(i).length();
     139    controlSocket->WriteUInt32((uint32_t)stringLength, 0);
     140    controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,
     141                               0);
     142  }
     143}
     144
     145bool HostEthController::ConnectedWithTarget() {
     146  char message[1024];
     147  ssize_t sent, received;
     148  static bool connectionEstablished = false;
     149
     150  connectionEstablishedMutex->GetMutex();
     151  if (!connectionEstablished &&
     152      controlSocket->Connect(targetPort, targetAddress, 10)) {
     153    Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
     154    SendControllerInfo();
     155    connectionEstablished = true;
     156  }
     157  connectionEstablishedMutex->ReleaseMutex();
     158  return connectionEstablished;
     159}
     160
     161void HostEthController::Run() {
     162  static int divider = 0;
     163  Message *msgControllerAction = new Message(1024);
     164  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
    67165    SafeStop();
    68     Join();
    69 
    70     if(!getFrameworkManager()->ConnectionLost()) delete tab;
    71 }
    72 
    73 void HostEthController::DrawUserInterface() {
    74     Tab* plotTab=new Tab(tabWidget,"Measures");
    75     axisPlot=new DataPlot1D *[axisNumber];
    76     for (unsigned int i=0; i<axisNumber;i++) {
    77         //Start a new row or add up to the current row? We try to keep a 4/3 ratio
    78         unsigned int columns=sqrt(4.0*axisNumber/3.0);
    79         LayoutPosition *position;
    80         if (i%columns==0) {
    81             position=plotTab->NewRow();
    82         } else {
    83             position=plotTab->LastRowLastCol();
     166    Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
     167  }
     168
     169  if (buttonNumber % 8 != 0) {
     170    SafeStop();
     171    Thread::Err("Button number is not multiple of 8\n");
     172  }
     173
     174  while (!ToBeStopped()) {
     175    // Thread::Info("Debug: entering acquisition loop\n");
     176    if (getFrameworkManager()->ConnectionLost() == true)
     177      SafeStop();
     178
     179    if (IsDataFrameReady()) { // wait for next data frame
     180      meaningfulDataAvailable = true;
     181      GetAxisData();
     182      GetButtonData();
     183
     184      if (ConnectedWithTarget()) {
     185        // read for commands from the target (remote control state change
     186        // requests such as rumble or led on/off)
     187        ssize_t bytesReceived =
     188            controlSocket->RecvMessage((char *)msgControllerAction->buffer,
     189                                       msgControllerAction->bufferSize);
     190        if (bytesReceived > 0) {
     191          // if the message is a cnx exit request we manage it here, if not it
     192          // will be managed by the derived class
     193          ControllerAction action;
     194          memcpy(&action, msgControllerAction->buffer,
     195                 sizeof(ControllerAction));
     196          if (action == ControllerAction::Exit) {
     197            // Thread::Info("Debug: exit request received from server\n");
     198            SafeStop();
     199          }
     200          ProcessMessage(msgControllerAction);
    84201        }
    85         axisPlot[i]=new DataPlot1D(position,axis->Name(i,0),-(1<<(nativeBitsPerAxis-1))*1.2,(1<<(nativeBitsPerAxis-1))*1.5);
    86         axisPlot[i]->AddCurve(axis->Element(i));
    87     }
    88     //we don't plot the button state for now
    89 }
    90 
    91 string HostEthController::GetAxisDescription(unsigned int axis) {
    92     return string("axis")+std::to_string(axis);
    93 }
    94 
    95 string HostEthController::GetButtonDescription(unsigned int button) {
    96     return string("button")+std::to_string(button);
    97 }
    98 
    99 bool HostEthController::ControllerInitialization() {
    100 
    101     buttonOffset=(axisNumber*bitsPerAxis)/8;
    102     if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
    103     dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
    104     if ((buttonNumber%8)!=0) dataFrameSize++;
    105     dataFrameBuffer=new char[dataFrameSize];
    106     return true;
    107 }
    108 
    109 void HostEthController::SendControllerInfo() {
    110     //send axis info
    111     controlSocket->WriteUInt32((uint32_t)axisNumber,0);
    112     controlSocket->WriteUInt32(bitsPerAxis,0);
    113     for (unsigned int i=0; i<axisNumber; i++) {
    114         //Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
    115         int stringLength=GetAxisDescription(i).length();
    116         controlSocket->WriteUInt32((uint32_t)stringLength,0);
    117         controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength,0);
    118     }
    119 
    120     //send button info
    121     controlSocket->WriteUInt32((uint32_t)buttonNumber,0);
    122     for (unsigned int i=0; i<buttonNumber; i++) {
    123         int stringLength=GetButtonDescription(i).length();
    124         controlSocket->WriteUInt32((uint32_t)stringLength,0);
    125         controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,0);
    126     }
    127 }
    128 
    129 bool HostEthController::ConnectedWithTarget() {
    130     char message[1024];
    131     ssize_t sent,received;
    132     static bool connectionEstablished=false;
    133 
    134     connectionEstablishedMutex->GetMutex();
    135     if (!connectionEstablished && controlSocket->Connect(targetPort,targetAddress,10)) {
    136         Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
    137         SendControllerInfo();
    138         connectionEstablished=true;
    139     }
    140     connectionEstablishedMutex->ReleaseMutex();
    141     return connectionEstablished;
    142 }
    143 
    144 void HostEthController::Run() {
    145     static int divider=0;
    146     Message *msgControllerAction=new Message(1024);
    147     if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
    148         SafeStop();
    149         Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
    150     }
    151 
    152     if(buttonNumber%8!=0) {
    153         SafeStop();
    154         Thread::Err("Button number is not multiple of 8\n");
    155     }
    156 
    157     while(!ToBeStopped()) {
    158         //Thread::Info("Debug: entering acquisition loop\n");
    159         if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
    160 
    161         if (IsDataFrameReady()) {  //wait for next data frame
    162             meaningfulDataAvailable=true;
    163             GetAxisData();
    164             GetButtonData();
    165 
    166             if (ConnectedWithTarget()) {
    167                 //read for commands from the target (remote control state change requests such as rumble or led on/off)
    168                 ssize_t bytesReceived=controlSocket->RecvMessage((char*)msgControllerAction->buffer,msgControllerAction->bufferSize);
    169                 if(bytesReceived>0) {
    170                     //if the message is a cnx exit request we manage it here, if not it will be managed by the derived class
    171                     ControllerAction action;
    172                     memcpy(&action,msgControllerAction->buffer,sizeof(ControllerAction));
    173                     if (action==ControllerAction::Exit) {
    174                         //Thread::Info("Debug: exit request received from server\n");
    175                         SafeStop();
    176                     }
    177                     ProcessMessage(msgControllerAction);
    178                 }
    179             }
    180         } else {//try to connect even if host is not sending anything
    181             ConnectedWithTarget();
    182         }
    183         //Thread::Info("Debug: exiting acquisition loop\n");
    184     }
    185 }
    186 
    187 bool HostEthController::writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits) {
    188     if (valueSizeInBits>16) return false;
    189     uint8_t remainingBitsToWrite=valueSizeInBits;
    190     //skip first bytes
    191     buffer+=offsetInBits/8;
    192     offsetInBits-=(offsetInBits/8)*8;
    193     while (remainingBitsToWrite>0) {
    194         uint8_t remainingBitsInByteBeforeWrite=8-offsetInBits;
    195         uint8_t bitsToWrite=remainingBitsToWrite<remainingBitsInByteBeforeWrite?remainingBitsToWrite:remainingBitsInByteBeforeWrite;
    196         uint8_t remainingBitsInByteAfterWrite=remainingBitsInByteBeforeWrite-bitsToWrite;
    197         //write in the current byte
    198         uint8_t byteMask=((1<<bitsToWrite)-1)<<remainingBitsInByteAfterWrite;
    199         (*buffer)&=~byteMask;
    200         uint16_t valueMask=(1<<remainingBitsToWrite)-1;
    201         (*buffer)|=((value&valueMask)>>(remainingBitsToWrite-bitsToWrite))<<remainingBitsInByteAfterWrite;
    202         //update state
    203         remainingBitsToWrite-=bitsToWrite;
    204         offsetInBits=(offsetInBits+bitsToWrite)%8;
    205         buffer++;
    206     }
    207     return true;
     202      }
     203    } else { // try to connect even if host is not sending anything
     204      ConnectedWithTarget();
     205    }
     206    // Thread::Info("Debug: exiting acquisition loop\n");
     207  }
     208}
     209
     210bool HostEthController::writeBits(uint16_t value, uint8_t valueSizeInBits,
     211                                  char *buffer, uint8_t offsetInBits) {
     212  if (valueSizeInBits > 16)
     213    return false;
     214  uint8_t remainingBitsToWrite = valueSizeInBits;
     215  // skip first bytes
     216  buffer += offsetInBits / 8;
     217  offsetInBits -= (offsetInBits / 8) * 8;
     218  while (remainingBitsToWrite > 0) {
     219    uint8_t remainingBitsInByteBeforeWrite = 8 - offsetInBits;
     220    uint8_t bitsToWrite = remainingBitsToWrite < remainingBitsInByteBeforeWrite
     221                              ? remainingBitsToWrite
     222                              : remainingBitsInByteBeforeWrite;
     223    uint8_t remainingBitsInByteAfterWrite =
     224        remainingBitsInByteBeforeWrite - bitsToWrite;
     225    // write in the current byte
     226    uint8_t byteMask = ((1 << bitsToWrite) - 1)
     227                       << remainingBitsInByteAfterWrite;
     228    (*buffer) &= ~byteMask;
     229    uint16_t valueMask = (1 << remainingBitsToWrite) - 1;
     230    (*buffer) |= ((value & valueMask) >> (remainingBitsToWrite - bitsToWrite))
     231                 << remainingBitsInByteAfterWrite;
     232    // update state
     233    remainingBitsToWrite -= bitsToWrite;
     234    offsetInBits = (offsetInBits + bitsToWrite) % 8;
     235    buffer++;
     236  }
     237  return true;
    208238}
    209239
    210240void HostEthController::BuildDataFrame() {
    211     //int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
    212     for (unsigned int i=0;i<axisNumber;i++) {
    213         //We shift value to always be positive (so that division/multiplication by power of 2 can easily be done with bit shifts)
    214         uint16_t shiftedNativeAxisValue=axis->Value(i,0)+(1<<(nativeBitsPerAxis-1));
    215         //int16_t nativeAxisValue=testValue[i];
    216         uint16_t scaledAxisValue;
    217         if (bitsPerAxis>nativeBitsPerAxis) {
    218             scaledAxisValue=shiftedNativeAxisValue<<(bitsPerAxis-nativeBitsPerAxis);
    219         } else {
    220             scaledAxisValue=shiftedNativeAxisValue>>(nativeBitsPerAxis-bitsPerAxis);
    221         }
    222         //Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
    223         unsigned int offsetInBits=i*bitsPerAxis;
    224         writeBits(scaledAxisValue,bitsPerAxis,dataFrameBuffer,offsetInBits);
    225     }
    226     //Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0], dataFrameBuffer[1], dataFrameBuffer[2]);
    227 
    228     int currentButton=0;
    229     uint8_t buttonArray[buttonNumber/8];
    230     for (unsigned int i=0;i<buttonNumber/8;i++) {
    231         buttonArray[i]=0;
    232         for(unsigned int j=0;j<8;j++) {
    233             bool buttonValue=button->Value(currentButton,0);
    234             if(buttonValue) buttonArray[i]+=1<<j;
    235             currentButton++;
    236         }
    237 
    238         dataSocket->HostToNetwork((char *)&buttonArray[i],sizeof(uint8_t));
    239         memcpy(dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),&buttonArray[i],sizeof(uint8_t));
    240     }
    241 }
    242 
    243 HostEthController::DataSender::DataSender(Object *parent,HostEthController* _hostEthController,string name,uint8_t priority):Thread(parent,name,priority),hostEthController(_hostEthController) {
    244 
    245 }
     241  // int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
     242  for (unsigned int i = 0; i < axisNumber; i++) {
     243    // We shift value to always be positive (so that division/multiplication by
     244    // power of 2 can easily be done with bit shifts)
     245    uint16_t shiftedNativeAxisValue =
     246        axis->Value(i, 0) + (1 << (nativeBitsPerAxis - 1));
     247    // int16_t nativeAxisValue=testValue[i];
     248    uint16_t scaledAxisValue;
     249    if (bitsPerAxis > nativeBitsPerAxis) {
     250      scaledAxisValue = shiftedNativeAxisValue
     251                        << (bitsPerAxis - nativeBitsPerAxis);
     252    } else {
     253      scaledAxisValue =
     254          shiftedNativeAxisValue >> (nativeBitsPerAxis - bitsPerAxis);
     255    }
     256    // Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis
     257    // value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
     258    unsigned int offsetInBits = i * bitsPerAxis;
     259    writeBits(scaledAxisValue, bitsPerAxis, dataFrameBuffer, offsetInBits);
     260  }
     261  // Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0],
     262  // dataFrameBuffer[1], dataFrameBuffer[2]);
     263
     264  int currentButton = 0;
     265  uint8_t buttonArray[buttonNumber / 8];
     266  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
     267    buttonArray[i] = 0;
     268    for (unsigned int j = 0; j < 8; j++) {
     269      bool buttonValue = button->Value(currentButton, 0);
     270      if (buttonValue)
     271        buttonArray[i] += 1 << j;
     272      currentButton++;
     273    }
     274
     275    dataSocket->HostToNetwork((char *)&buttonArray[i], sizeof(uint8_t));
     276    memcpy(dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
     277           &buttonArray[i], sizeof(uint8_t));
     278  }
     279}
     280
     281HostEthController::DataSender::DataSender(Object *parent,
     282                                          HostEthController *_hostEthController,
     283                                          string name, uint8_t priority)
     284    : Thread(parent, name, priority), hostEthController(_hostEthController) {}
    246285
    247286void HostEthController::DataSender::Run() {
    248     if(getFrameworkManager()->ErrorOccured()==true) {
    249         SafeStop();
    250     }
    251 
    252     while(!ToBeStopped()) {
    253         WaitPeriod();
    254         if (hostEthController->meaningfulDataAvailable && hostEthController->ConnectedWithTarget()) {
    255             //send the data
    256             hostEthController->BuildDataFrame();
    257             hostEthController->dataSocket->SendMessage(hostEthController->dataFrameBuffer,hostEthController->dataFrameSize);
    258         }
    259     }
     287  if (getFrameworkManager()->ErrorOccured() == true) {
     288    SafeStop();
     289  }
     290
     291  while (!ToBeStopped()) {
     292    WaitPeriod();
     293    if (hostEthController->meaningfulDataAvailable &&
     294        hostEthController->ConnectedWithTarget()) {
     295      // send the data
     296      hostEthController->BuildDataFrame();
     297      hostEthController->dataSocket->SendMessage(
     298          hostEthController->dataFrameBuffer, hostEthController->dataFrameSize);
     299    }
     300  }
    260301}
    261302

  • trunk/lib/FlairSensorActuator/src/HostEthController.h

    r3 r15  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
     13//  purpose:    Base class for host side remote controls that talks to target
     14//  side through ethernet connection
    1415//
    1516//
    … …  
    2425
    2526namespace flair {
    26     namespace core {
    27         class FrameworkManager;
    28         class cvmatrix;
    29         class TcpSocket;
    30         class Socket;
    31         class Mutex;
    32     }
    33     namespace gui {
    34         class Tab;
    35         class TabWidget;
    36         class DataPlot1D;
    37     }
     27namespace core {
     28class FrameworkManager;
     29class cvmatrix;
     30class TcpSocket;
     31class Socket;
     32class Mutex;
     33}
     34namespace gui {
     35class Tab;
     36class TabWidget;
     37class DataPlot1D;
     38}
    3839}
    3940
    40 namespace flair { namespace sensor {
    41     enum class ControllerAction;
     41namespace flair {
     42namespace sensor {
     43enum class ControllerAction;
    4244
    43     /*! \class HostEthController
    44     *
    45     * \brief Base Class for host side remote controls that talks to target side through ethernet connection
    46     *
    47     * There are 2 communication channels:
    48     *   - 1 connection with the ground station to display the values. Output for analog sticks is normalized in the range [-1, 1] (float values)
    49     *   - 1 connection with the target to send the controller values (and receive controller state modification requests)
    50     */
    51     class HostEthController : public core::Thread, public core::IODevice {
    52     public:
    53         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);
    54         ~HostEthController();
    55         void DrawUserInterface();
    56     protected:
    57         std::string controllerName;
    58         core::TcpSocket *controlSocket; //connection to the target
    59         core::Socket *dataSocket;
    60         std::string targetAddress;
    61         int targetPort;
    62         gui::Tab *tab;
    63         gui::TabWidget *tabWidget;
    64         virtual bool IsDataFrameReady() { return true;};
    65         virtual void CompleteDataFrameGrab() {};
    66 //        int8_t *datas;
    67 //        uint8_t dataSize;
    68         char *dataFrameBuffer;
    69         size_t dataFrameSize;
    70         virtual void ProcessMessage(core::Message *controllerAction) {};
     45/*! \class HostEthController
     46*
     47* \brief Base Class for host side remote controls that talks to target side
     48*through ethernet connection
     49*
     50* There are 2 communication channels:
     51*   - 1 connection with the ground station to display the values. Output for
     52*analog sticks is normalized in the range [-1, 1] (float values)
     53*   - 1 connection with the target to send the controller values (and receive
     54*controller state modification requests)
     55*/
     56class HostEthController : public core::Thread, public core::IODevice {
     57public:
     58  HostEthController(const core::FrameworkManager *parent, std::string name,
     59                    std::string address, int port, uint32_t period = 10,
     60                    uint32_t _bitsPerAxis = 7, uint8_t priority = 0);
     61  ~HostEthController();
     62  void DrawUserInterface();
    7163
    72         virtual std::string GetAxisDescription(unsigned int axis);
    73         virtual void GetAxisData()=0; //responsible for getting the axis data from the hardware
    74         unsigned int axisNumber;
    75         core::cvmatrix* axis;
    76         gui::DataPlot1D **axisPlot;
    77         uint32_t bitsPerAxis;
    78         uint32_t nativeBitsPerAxis;
     64protected:
     65  std::string controllerName;
     66  core::TcpSocket *controlSocket; // connection to the target
     67  core::Socket *dataSocket;
     68  std::string targetAddress;
     69  int targetPort;
     70  gui::Tab *tab;
     71  gui::TabWidget *tabWidget;
     72  virtual bool IsDataFrameReady() { return true; };
     73  virtual void CompleteDataFrameGrab(){};
     74  //        int8_t *datas;
     75  //        uint8_t dataSize;
     76  char *dataFrameBuffer;
     77  size_t dataFrameSize;
     78  virtual void ProcessMessage(core::Message *controllerAction){};
    7979
    80         virtual std::string GetButtonDescription(unsigned int button);
    81         virtual void GetButtonData()=0; //responsible for getting the button data from the hardware
    82         unsigned int buttonNumber;
    83         core::cvmatrix* button;
    84         uint8_t buttonOffset;
    85         bool meaningfulDataAvailable;
     80  virtual std::string GetAxisDescription(unsigned int axis);
     81  virtual void
     82  GetAxisData() = 0; // responsible for getting the axis data from the hardware
     83  unsigned int axisNumber;
     84  core::cvmatrix *axis;
     85  gui::DataPlot1D **axisPlot;
     86  uint32_t bitsPerAxis;
     87  uint32_t nativeBitsPerAxis;
    8688
    87     private:
    88         class DataSender : public core::Thread {
    89         public:
    90             DataSender(Object* parent,HostEthController* hostEthController,std::string name,uint8_t priority=0);
    91             void Run();
    92         private:
    93             HostEthController* hostEthController;
    94         };
    95         DataSender *dataSender;
     89  virtual std::string GetButtonDescription(unsigned int button);
     90  virtual void GetButtonData() = 0; // responsible for getting the button data
     91                                    // from the hardware
     92  unsigned int buttonNumber;
     93  core::cvmatrix *button;
     94  uint8_t buttonOffset;
     95  bool meaningfulDataAvailable;
    9696
    97         bool ControllerInitialization();
    98         bool ConnectedWithTarget();
    99         void SendControllerInfo();
    100         void Run();
    101         void BuildDataFrame();
    102         bool writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits);
    103         core::Mutex *connectionEstablishedMutex;
    104     };
     97private:
     98  class DataSender : public core::Thread {
     99  public:
     100    DataSender(Object *parent, HostEthController *hostEthController,
     101               std::string name, uint8_t priority = 0);
     102    void Run();
    105103
    106 }}
     104  private:
     105    HostEthController *hostEthController;
     106  };
     107  DataSender *dataSender;
     108
     109  bool ControllerInitialization();
     110  bool ConnectedWithTarget();
     111  void SendControllerInfo();
     112  void Run();
     113  void BuildDataFrame();
     114  bool writeBits(uint16_t value, uint8_t valueSizeInBits, char *buffer,
     115                 uint8_t offsetInBits);
     116  core::Mutex *connectionEstablishedMutex;
     117};
     118}
     119}
    107120
    108121#endif // HOSTETHCONTROLLER_H

  • trunk/lib/FlairSensorActuator/src/Imu.cpp

    r3 r15  
    3030using namespace flair::gui;
    3131
    32 namespace flair { namespace sensor {
     32namespace flair {
     33namespace sensor {
    3334
    34 Imu::Imu(const FrameworkManager* parent,string name) : IODevice(parent,name) {
    35     imuData=new ImuData(this);
     35Imu::Imu(const FrameworkManager *parent, string name) : IODevice(parent, name) {
     36  imuData = new ImuData(this);
    3637
    37     //station sol
    38     mainTab=new Tab(parent->GetTabWidget(),name);
    39     tab=new TabWidget(mainTab->NewRow(),name);
    40     sensorTab=new Tab(tab,"Reglages");
    41         setupGroupbox=new GroupBox(sensorTab->NewRow(),name);
    42         rotation=new OneAxisRotation(sensorTab->NewRow(),"post rotation");
     38  // station sol
     39  mainTab = new Tab(parent->GetTabWidget(), name);
     40  tab = new TabWidget(mainTab->NewRow(), name);
     41  sensorTab = new Tab(tab, "Reglages");
     42  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
     43  rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation");
    4344}
    4445
    45 Imu::Imu(const IODevice* parent,std::string name) : IODevice(parent,name) {
    46     imuData=new ImuData(this);
    47     mainTab=NULL;
    48     tab=NULL;
    49     sensorTab=NULL;
    50         setupGroupbox=NULL;
    51         rotation=NULL;
     46Imu::Imu(const IODevice *parent, std::string name) : IODevice(parent, name) {
     47  imuData = new ImuData(this);
     48  mainTab = NULL;
     49  tab = NULL;
     50  sensorTab = NULL;
     51  setupGroupbox = NULL;
     52  rotation = NULL;
    5253}
    5354
    5455Imu::~Imu() {
    55     if(mainTab!=NULL) delete mainTab;
     56  if (mainTab != NULL)
     57    delete mainTab;
    5658}
    5759
    58 void Imu::GetDatas(ImuData **outImuData) const {
    59     *outImuData=imuData;
    60 }
     60void Imu::GetDatas(ImuData **outImuData) const { *outImuData = imuData; }
    6161void Imu::UpdateImu() {
    62     if(rotation==NULL) {
    63         Err("not applicable for simulation part.\n");
    64         return;
    65     }
    66     Vector3D rawAcc,rawMag,rawGyr;
    67     imuData->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
    68     rotation->ComputeRotation(rawAcc);
    69     rotation->ComputeRotation(rawGyr);
    70     rotation->ComputeRotation(rawMag);
    71     imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
     62  if (rotation == NULL) {
     63    Err("not applicable for simulation part.\n");
     64    return;
     65  }
     66  Vector3D rawAcc, rawMag, rawGyr;
     67  imuData->GetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
     68  rotation->ComputeRotation(rawAcc);
     69  rotation->ComputeRotation(rawGyr);
     70  rotation->ComputeRotation(rawMag);
     71  imuData->SetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
    7272}
    7373
    74 GroupBox* Imu::GetGroupBox(void) const {
    75     return setupGroupbox;
    76 }
     74GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; }
    7775
    78 Layout* Imu::GetLayout(void) const {
    79     return sensorTab;
    80 }
     76Layout *Imu::GetLayout(void) const { return sensorTab; }
    8177
    8278void Imu::LockUserInterface(void) const {
    83     if(sensorTab==NULL) {
    84         Err("not applicable for simulation part.\n");
    85         return;
    86     }
    87     sensorTab->setEnabled(false);
     79  if (sensorTab == NULL) {
     80    Err("not applicable for simulation part.\n");
     81    return;
     82  }
     83  sensorTab->setEnabled(false);
    8884}
    8985
    9086void Imu::UnlockUserInterface(void) const {
    91     if(sensorTab==NULL) {
    92         Err("not applicable for simulation part.\n");
    93         return;
    94     }
    95     sensorTab->setEnabled(true);
     87  if (sensorTab == NULL) {
     88    Err("not applicable for simulation part.\n");
     89    return;
     90  }
     91  sensorTab->setEnabled(true);
    9692}
    9793
    9894void Imu::UseDefaultPlot(void) {
    99     if(tab==NULL) {
    100         Err("not applicable for simulation part.\n");
    101         return;
    102     }
     95  if (tab == NULL) {
     96    Err("not applicable for simulation part.\n");
     97    return;
     98  }
    10399
    104     plotTab=new Tab(tab,"IMU");
    105     axPlot=new DataPlot1D(plotTab->NewRow(),"acc_x",-10,10);
    106     axPlot->AddCurve(imuData->Element(ImuData::RawAx));
    107     ayPlot=new DataPlot1D(plotTab->LastRowLastCol(),"acc_y",-10,10);
    108     ayPlot->AddCurve(imuData->Element(ImuData::RawAy));
    109     azPlot=new DataPlot1D(plotTab->LastRowLastCol(),"acc_z",-10,10);
    110     azPlot->AddCurve(imuData->Element(ImuData::RawAz));
     100  plotTab = new Tab(tab, "IMU");
     101  axPlot = new DataPlot1D(plotTab->NewRow(), "acc_x", -10, 10);
     102  axPlot->AddCurve(imuData->Element(ImuData::RawAx));
     103  ayPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_y", -10, 10);
     104  ayPlot->AddCurve(imuData->Element(ImuData::RawAy));
     105  azPlot = new DataPlot1D(plotTab->LastRowLastCol(), "acc_z", -10, 10);
     106  azPlot->AddCurve(imuData->Element(ImuData::RawAz));
    111107
    112     if(plotTab==NULL) plotTab=new Tab(tab,"IMU");
    113     gxPlot=new DataPlot1D(plotTab->NewRow(),"gyr_x",-500,500);
    114     gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg));
    115     gyPlot=new DataPlot1D(plotTab->LastRowLastCol(),"gyr_y",-500,500);
    116     gyPlot->AddCurve(imuData->Element(ImuData::RawGyDeg));
    117     gzPlot=new DataPlot1D(plotTab->LastRowLastCol(),"gyr_z",-500,500);
    118     gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg));
     108  if (plotTab == NULL)
     109    plotTab = new Tab(tab, "IMU");
     110  gxPlot = new DataPlot1D(plotTab->NewRow(), "gyr_x", -500, 500);
     111  gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg));
     112  gyPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_y", -500, 500);
     113  gyPlot->AddCurve(imuData->Element(ImuData::RawGyDeg));
     114  gzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "gyr_z", -500, 500);
     115  gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg));
    119116
    120     if(plotTab==NULL)  plotTab=new Tab(tab,"IMU");
    121     mxPlot=new DataPlot1D(plotTab->NewRow(),"mag_x",-500,500);
    122     mxPlot->AddCurve(imuData->Element(ImuData::RawMx));
    123     myPlot=new DataPlot1D(plotTab->LastRowLastCol(),"mag_y",-500,500);
    124     myPlot->AddCurve(imuData->Element(ImuData::RawMy));
    125     mzPlot=new DataPlot1D(plotTab->LastRowLastCol(),"mag_z",-500,500);
    126     mzPlot->AddCurve(imuData->Element(ImuData::RawMz));
     117  if (plotTab == NULL)
     118    plotTab = new Tab(tab, "IMU");
     119  mxPlot = new DataPlot1D(plotTab->NewRow(), "mag_x", -500, 500);
     120  mxPlot->AddCurve(imuData->Element(ImuData::RawMx));
     121  myPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_y", -500, 500);
     122  myPlot->AddCurve(imuData->Element(ImuData::RawMy));
     123  mzPlot = new DataPlot1D(plotTab->LastRowLastCol(), "mag_z", -500, 500);
     124  mzPlot->AddCurve(imuData->Element(ImuData::RawMz));
    127125}
    128126
    129 Tab* Imu::GetPlotTab(void) const {
    130     return plotTab;
    131 }
     127Tab *Imu::GetPlotTab(void) const { return plotTab; }
    132128
    133129} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/Imu.h

    r3 r15  
    1717
    1818namespace flair {
    19     namespace core {
    20         class ImuData;
    21         class OneAxisRotation;
    22     }
    23     namespace gui {
    24         class Tab;
    25         class TabWidget;
    26         class GroupBox;
    27         class Layout;
    28         class DataPlot1D;
    29     }
     19namespace core {
     20class ImuData;
     21class OneAxisRotation;
     22}
     23namespace gui {
     24class Tab;
     25class TabWidget;
     26class GroupBox;
     27class Layout;
     28class DataPlot1D;
     29}
    3030}
    3131
    3232class Ahrs_impl;
    3333
    34 namespace flair { namespace sensor {
    35     /*! \class Imu
    36     *
    37     * \brief Base class for Imu
    38     *
    39     * Use this class to define a custom Imu.
    40     *
    41     */
    42     class Imu : public core::IODevice {
    43         friend class ::Ahrs_impl;
     34namespace flair {
     35namespace sensor {
     36/*! \class Imu
     37*
     38* \brief Base class for Imu
     39*
     40* Use this class to define a custom Imu.
     41*
     42*/
     43class Imu : public core::IODevice {
     44  friend class ::Ahrs_impl;
    4445
    45         public:
    46             /*!
    47             * \brief Constructor
    48             *
    49             * Construct an Imu.
    50             *
    51             * \param parent parent
    52             * \param name name
    53             */
    54             Imu(const core::FrameworkManager *parent,std::string name);
     46public:
     47  /*!
     48  * \brief Constructor
     49  *
     50  * Construct an Imu.
     51  *
     52  * \param parent parent
     53  * \param name name
     54  */
     55  Imu(const core::FrameworkManager *parent, std::string name);
    5556
    56             /*!
    57             * \brief Constructor
    58             *
    59             * Construct an Imu. \n
    60             * This contructor must only be called for a simulated device.
    61             *
    62             * \param parent parent
    63             * \param name name
    64             */
    65             Imu(const core::IODevice *parent,std::string name);
     57  /*!
     58  * \brief Constructor
     59  *
     60  * Construct an Imu. \n
     61  * This contructor must only be called for a simulated device.
     62  *
     63  * \param parent parent
     64  * \param name name
     65  */
     66  Imu(const core::IODevice *parent, std::string name);
    6667
    67             /*!
    68             * \brief Destructor
    69             *
    70             */
    71             ~Imu();
     68  /*!
     69  * \brief Destructor
     70  *
     71  */
     72  ~Imu();
    7273
    73             /*!
    74             * \brief Setup Layout
    75             *
    76             * \return setup Layout
    77             */
    78             gui::Layout *GetLayout(void) const;
     74  /*!
     75  * \brief Setup Layout
     76  *
     77  * \return setup Layout
     78  */
     79  gui::Layout *GetLayout(void) const;
    7980
    80             /*!
    81             * \brief Lock user interface
    82             *
    83             */
    84             void LockUserInterface(void) const;
     81  /*!
     82  * \brief Lock user interface
     83  *
     84  */
     85  void LockUserInterface(void) const;
    8586
    86             /*!
    87             * \brief Unlock user interface
    88             *
    89             */
    90             void UnlockUserInterface(void) const;
     87  /*!
     88  * \brief Unlock user interface
     89  *
     90  */
     91  void UnlockUserInterface(void) const;
    9192
    92             /*!
    93             * \brief Use default plot
    94             *
    95             */
    96             void UseDefaultPlot(void);
     93  /*!
     94  * \brief Use default plot
     95  *
     96  */
     97  void UseDefaultPlot(void);
    9798
    98             /*!
    99             * \brief Plot tab
    100             *
    101             * \return plot Tab
    102             */
    103             gui::Tab *GetPlotTab(void) const;
     99  /*!
     100  * \brief Plot tab
     101  *
     102  * \return plot Tab
     103  */
     104  gui::Tab *GetPlotTab(void) const;
    104105
    105         protected:
    106             /*!
    107             * \brief Setup GroupBox
    108             *
    109             * \return setup GroupBox
    110             */
    111             gui::GroupBox *GetGroupBox(void) const;
     106protected:
     107  /*!
     108  * \brief Setup GroupBox
     109  *
     110  * \return setup GroupBox
     111  */
     112  gui::GroupBox *GetGroupBox(void) const;
    112113
    113             /*!
    114             * \brief UpdateImu
    115             *
    116             * The reimplemented class must call this function as soon as IMU datas are available. \n
    117             * It handles the data rotation if it was defined.
    118             *
    119             */
    120             void UpdateImu();
     114  /*!
     115  * \brief UpdateImu
     116  *
     117  * The reimplemented class must call this function as soon as IMU datas are
     118  *available. \n
     119  * It handles the data rotation if it was defined.
     120  *
     121  */
     122  void UpdateImu();
    121123
    122             /*!
    123             * \brief Get imu datas
    124             *
    125             * \param imuData imu datas
    126             */
    127             void GetDatas(core::ImuData **imuData) const;
     124  /*!
     125  * \brief Get imu datas
     126  *
     127  * \param imuData imu datas
     128  */
     129  void GetDatas(core::ImuData **imuData) const;
    128130
     131private:
     132  gui::Tab *mainTab, *sensorTab, *plotTab;
     133  gui::TabWidget *tab;
     134  gui::GroupBox *setupGroupbox;
     135  core::OneAxisRotation *rotation;
     136  core::ImuData *imuData;
    129137
    130         private:
    131             gui::Tab *mainTab,*sensorTab,*plotTab;
    132             gui::TabWidget* tab;
    133             gui::GroupBox *setupGroupbox;
    134             core::OneAxisRotation* rotation;
    135             core::ImuData *imuData;
    136 
    137             gui::DataPlot1D *axPlot,*ayPlot,*azPlot;
    138             gui::DataPlot1D *gxPlot,*gyPlot,*gzPlot;
    139             gui::DataPlot1D *mxPlot,*myPlot,*mzPlot;
    140     };
     138  gui::DataPlot1D *axPlot, *ayPlot, *azPlot;
     139  gui::DataPlot1D *gxPlot, *gyPlot, *gzPlot;
     140  gui::DataPlot1D *mxPlot, *myPlot, *mzPlot;
     141};
    141142} // end namespace sensor
    142143} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp

    r3 r15  
    3232using namespace flair::gui;
    3333
    34 namespace flair { namespace sensor {
     34namespace flair {
     35namespace sensor {
    3536
    36 LaserRangeFinder::LaserRangeFinder(const FrameworkManager* parent,string name) : IODevice(parent,name) {
    37     cvmatrix_descriptor* desc=new cvmatrix_descriptor(360,1);
    38     output=new cvmatrix(this,desc,floatType);
    39     AddDataToLog(output);
     37LaserRangeFinder::LaserRangeFinder(const FrameworkManager *parent, string name)
     38    : IODevice(parent, name) {
     39  cvmatrix_descriptor *desc = new cvmatrix_descriptor(360, 1);
     40  output = new cvmatrix(this, desc, floatType);
     41  AddDataToLog(output);
    4042
    41     //station sol
    42     main_tab=new Tab(parent->GetTabWidget(),name);
    43     tab=new TabWidget(main_tab->NewRow(),name);
    44     sensor_tab=new Tab(tab,"Reglages");
    45     setup_groupbox=new GroupBox(sensor_tab->NewRow(),name);
    46 
     43  // station sol
     44  main_tab = new Tab(parent->GetTabWidget(), name);
     45  tab = new TabWidget(main_tab->NewRow(), name);
     46  sensor_tab = new Tab(tab, "Reglages");
     47  setup_groupbox = new GroupBox(sensor_tab->NewRow(), name);
    4748}
    4849
    49 LaserRangeFinder::LaserRangeFinder(const IODevice* parent,std::string name) : IODevice(parent,name)
    50 {
    51     plot_tab=NULL;
    52     main_tab=NULL;
    53     tab=NULL;
    54     sensor_tab=NULL;
    55     setup_groupbox=NULL;
     50LaserRangeFinder::LaserRangeFinder(const IODevice *parent, std::string name)
     51    : IODevice(parent, name) {
     52  plot_tab = NULL;
     53  main_tab = NULL;
     54  tab = NULL;
     55  sensor_tab = NULL;
     56  setup_groupbox = NULL;
    5657
    57     output=NULL;
     58  output = NULL;
    5859}
    5960
    60 LaserRangeFinder::~LaserRangeFinder()
    61 {
     61LaserRangeFinder::~LaserRangeFinder() {}
    6262
    63 }
     63GroupBox *LaserRangeFinder::GetGroupBox(void) const { return setup_groupbox; }
    6464
     65Layout *LaserRangeFinder::GetLayout(void) const { return sensor_tab; }
    6566
    66 GroupBox* LaserRangeFinder::GetGroupBox(void) const
    67 {
    68 return setup_groupbox;
    69 }
     67RangeFinderPlot *LaserRangeFinder::GetPlot(void) const { return plot; }
    7068
    71 Layout* LaserRangeFinder::GetLayout(void) const
    72 {
    73 return sensor_tab;
    74 }
     69void LaserRangeFinder::UseDefaultPlot(void) {
     70  if (tab == NULL) {
     71    Err("not applicable for simulation part.\n");
     72    return;
     73  }
    7574
    76 
    77 RangeFinderPlot* LaserRangeFinder::GetPlot(void) const
    78 {
    79 return plot;
    80 }
    81 
    82 void LaserRangeFinder::UseDefaultPlot(void)
    83 {
    84     if(tab==NULL)
    85     {
    86         Err("not applicable for simulation part.\n");
    87         return;
    88     }
    89 
    90     plot_tab=new Tab(tab,"Mesures");
    91     plot=new RangeFinderPlot(plot_tab->NewRow(),"plot","x",-30,30,"y",-30,30,output,0,359,output->Rows());
     75  plot_tab = new Tab(tab, "Mesures");
     76  plot = new RangeFinderPlot(plot_tab->NewRow(), "plot", "x", -30, 30, "y", -30,
     77                             30, output, 0, 359, output->Rows());
    9278}
    9379} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h

    r3 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class FrameworkManager;
    23         class cvmatrix;
    24 
    25     }
    26     namespace gui
    27     {
    28         class Tab;
    29         class TabWidget;
    30         class GroupBox;
    31         class Layout;
    32         class RangeFinderPlot;
    33     }
     18namespace flair {
     19namespace core {
     20class FrameworkManager;
     21class cvmatrix;
     22}
     23namespace gui {
     24class Tab;
     25class TabWidget;
     26class GroupBox;
     27class Layout;
     28class RangeFinderPlot;
     29}
    3430}
    3531
    36 namespace flair
    37 {
    38 namespace sensor
    39 {
    40     /*! \class LaserRangeFinder
    41     *
    42     * \brief Classe generique intégrant les telemetres laser
    43     */
    44     class LaserRangeFinder : public core::IODevice
    45     {
    46         public:
    47             /*!
    48             * \brief Constructor
    49             *
    50             * Construct a Laser Range Finder.
    51             *
    52             * \param parent parent
    53             * \param name name
    54             */
    55             LaserRangeFinder(const core::FrameworkManager* parent,std::string name);
    56             /*!
    57             * \brief Constructor
    58             *
    59             * Construct a UsRangeFinder. Simulation part.
    60             *
    61             * \param parent parent
    62             * \param name name
    63             */
    64             LaserRangeFinder(const core::IODevice* parent,std::string name);
    65             /*!
    66             * \brief Destructor
    67             *
    68             */
    69             ~LaserRangeFinder();
     32namespace flair {
     33namespace sensor {
     34/*! \class LaserRangeFinder
     35*
     36* \brief Classe generique intégrant les telemetres laser
     37*/
     38class LaserRangeFinder : public core::IODevice {
     39public:
     40  /*!
     41  * \brief Constructor
     42  *
     43  * Construct a Laser Range Finder.
     44  *
     45  * \param parent parent
     46  * \param name name
     47  */
     48  LaserRangeFinder(const core::FrameworkManager *parent, std::string name);
     49  /*!
     50  * \brief Constructor
     51  *
     52  * Construct a UsRangeFinder. Simulation part.
     53  *
     54  * \param parent parent
     55  * \param name name
     56  */
     57  LaserRangeFinder(const core::IODevice *parent, std::string name);
     58  /*!
     59  * \brief Destructor
     60  *
     61  */
     62  ~LaserRangeFinder();
    7063
    71             /*!
    72             * \brief Use default plot
    73             *
    74             */
    75             void UseDefaultPlot(void);
     64  /*!
     65  * \brief Use default plot
     66  *
     67  */
     68  void UseDefaultPlot(void);
    7669
    77             /*!
    78             * \brief Plot
    79             *
    80             * \return DataPlot1D
    81             */
    82             gui::RangeFinderPlot* GetPlot(void) const;
     70  /*!
     71  * \brief Plot
     72  *
     73  * \return DataPlot1D
     74  */
     75  gui::RangeFinderPlot *GetPlot(void) const;
    8376
    84             /*!
    85             * \brief Setup Layout
    86             *
    87             * \return a Layout available
    88             */
    89             gui::Layout* GetLayout(void) const;
     77  /*!
     78  * \brief Setup Layout
     79  *
     80  * \return a Layout available
     81  */
     82  gui::Layout *GetLayout(void) const;
    9083
    91             /*!
    92             * \brief Plot tab
    93             *
    94             * \return plot Tab
    95             */
    96             gui::Tab* GetPlotTab(void) const;
     84  /*!
     85  * \brief Plot tab
     86  *
     87  * \return plot Tab
     88  */
     89  gui::Tab *GetPlotTab(void) const;
    9790
    98             /*!
    99             * \brief Value
    100             *
    101             * \return output value
    102             */
    103             float Value(void) const;
     91  /*!
     92  * \brief Value
     93  *
     94  * \return output value
     95  */
     96  float Value(void) const;
    10497
    105         protected:
    106         /*!
    107             * \brief Output matrix
    108             *
    109             * \return output matrix
    110             */
    111             core::cvmatrix *output;
     98protected:
     99  /*!
     100      * \brief Output matrix
     101      *
     102      * \return output matrix
     103      */
     104  core::cvmatrix *output;
    112105
    113             /*!
    114             * \brief Setup GroupBox
    115             *
    116             * \return a GroupBox available
    117             */
    118             gui::GroupBox* GetGroupBox(void) const;
     106  /*!
     107  * \brief Setup GroupBox
     108  *
     109  * \return a GroupBox available
     110  */
     111  gui::GroupBox *GetGroupBox(void) const;
    119112
    120         private:
    121             /*!
    122             * \brief Update using provided datas
    123             *
    124             * Reimplemented from IODevice.
    125             *
    126             * \param data data from the parent to process
    127             */
    128             void UpdateFrom(const core::io_data *data){};
     113private:
     114  /*!
     115  * \brief Update using provided datas
     116  *
     117  * Reimplemented from IODevice.
     118  *
     119  * \param data data from the parent to process
     120  */
     121  void UpdateFrom(const core::io_data *data){};
    129122
    130             gui::Tab* main_tab;
    131             gui::TabWidget *tab;
    132             gui::GroupBox* setup_groupbox;
    133             gui::Tab* sensor_tab;
    134             gui::RangeFinderPlot* plot;
    135             gui::Tab* plot_tab;
    136 
    137     };
     123  gui::Tab *main_tab;
     124  gui::TabWidget *tab;
     125  gui::GroupBox *setup_groupbox;
     126  gui::Tab *sensor_tab;
     127  gui::RangeFinderPlot *plot;
     128  gui::Tab *plot_tab;
     129};
    138130} // end namespace sensor
    139131} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/Mb800.cpp

    r3 r15  
    2424using namespace flair::core;
    2525
    26 namespace flair
    27 {
    28 namespace sensor
    29 {
     26namespace flair {
     27namespace sensor {
    3028
    31 Mb800::Mb800(const FrameworkManager* parent,string name,SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Gps(parent,name,NMEAFlags), Thread(parent,name,priority)
    32 {
    33     this->serialport=serialport;
    34     serialport->SetRxTimeout(100000000);
     29Mb800::Mb800(const FrameworkManager *parent, string name,
     30             SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     31             uint8_t priority)
     32    : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) {
     33  this->serialport = serialport;
     34  serialport->SetRxTimeout(100000000);
    3535}
    3636
    37 Mb800::~Mb800()
    38 {
    39     SafeStop();
    40     Join();
     37Mb800::~Mb800() {
     38  SafeStop();
     39  Join();
    4140}
    4241
    43 void Mb800::Run(void)
    44 {
    45     /** Debut init **/
    46     char response[200] = {0};
    47     int size;
     42void Mb800::Run(void) {
     43  /** Debut init **/
     44  char response[200] = {0};
     45  int size;
    4846
    49     /** Fin init **/
     47  /** Fin init **/
    5048
    51     /** Debut config **/
    52     char to_send[]="$PASHS,NME,ALL,A,OFF\r\n";
     49  /** Debut config **/
     50  char to_send[] = "$PASHS,NME,ALL,A,OFF\r\n";
     51  serialport->Write(to_send, sizeof(to_send));
     52
     53  {
     54    char to_send[] = "$PASHS,CPD,AFP,95.0\r\n";
    5355    serialport->Write(to_send, sizeof(to_send));
     56  }
     57  {
     58    char to_send[] = "$PASHS,DIF,PRT,C,RT3\r\n";
     59    serialport->Write(to_send, sizeof(to_send));
     60  }
    5461
    55     {
    56         char to_send[]="$PASHS,CPD,AFP,95.0\r\n";
    57         serialport->Write(to_send, sizeof(to_send));
     62  if ((NMEAFlags & GGA) != 0) {
     63    char to_send[] = "$PASHS,NME,GGA,A,ON,0.05\r\n";
     64    size = serialport->Write(to_send, sizeof(to_send));
     65    // Printf("ecrit %i\n",size);
     66  }
     67  if ((NMEAFlags & VTG) != 0) {
     68    char to_send[] = "$PASHS,NME,VTG,A,ON,0.05\r\n";
     69    size = serialport->Write(to_send, sizeof(to_send));
     70    // Printf("%i\n",size);
     71  }
     72  if ((NMEAFlags & GST) != 0) {
     73    char to_send[] = "$PASHS,NME,GST,A,ON,0.05\r\n";
     74    size = serialport->Write(to_send, sizeof(to_send));
     75    // Printf("%i\n",size);
     76  }
     77
     78  Sync();
     79
     80  /** Fin config **/
     81
     82  /** Debut running loop **/
     83  WarnUponSwitches(true);
     84
     85  while (!ToBeStopped()) {
     86    SleepMS(10);
     87    size = 0;
     88    while (!ToBeStopped()) {
     89      ssize_t read = serialport->Read(&response[size], 1);
     90      if (read < 0) {
     91        Thread::Err("erreur Read (%s)\n", strerror(-read));
     92      }
     93
     94      if (response[size] == 0x0a)
     95        break;
     96      size++;
    5897    }
    59     {
    60         char to_send[]="$PASHS,DIF,PRT,C,RT3\r\n";
    61         serialport->Write(to_send, sizeof(to_send));
    62     }
    63 
    64     if((NMEAFlags&GGA)!=0)
    65     {
    66         char to_send[]="$PASHS,NME,GGA,A,ON,0.05\r\n";
    67         size=serialport->Write(to_send, sizeof(to_send));
    68         //Printf("ecrit %i\n",size);
    69     }
    70     if((NMEAFlags&VTG)!=0)
    71     {
    72         char to_send[]="$PASHS,NME,VTG,A,ON,0.05\r\n";
    73         size=serialport->Write(to_send, sizeof(to_send));
    74         //Printf("%i\n",size);
    75     }
    76     if((NMEAFlags&GST)!=0)
    77     {
    78         char to_send[]="$PASHS,NME,GST,A,ON,0.05\r\n";
    79         size=serialport->Write(to_send, sizeof(to_send));
    80         //Printf("%i\n",size);
    81     }
    82 
    83     Sync();
    84 
    85     /** Fin config **/
    86 
    87     /** Debut running loop **/
    88     WarnUponSwitches(true);
    89 
    90     while(!ToBeStopped())
    91     {
    92         SleepMS(10);
    93         size=0;
    94         while(!ToBeStopped())
    95         {
    96             ssize_t read = serialport->Read(&response[size],1);
    97             if(read<0)
    98             {
    99                 Thread::Err("erreur Read (%s)\n",strerror(-read));
    100             }
    101 
    102             if(response[size]==0x0a) break;
    103             size++;
    104         }
    105         size++;
    106         parseFrame(response, size);
    107     }
    108     /** fin running loop **/
    109     WarnUponSwitches(false);
     98    size++;
     99    parseFrame(response, size);
     100  }
     101  /** fin running loop **/
     102  WarnUponSwitches(false);
    110103}
    111104
    112 void Mb800::Sync(void)
    113 {
    114     char data=0;
    115     ssize_t read = 0;
     105void Mb800::Sync(void) {
     106  char data = 0;
     107  ssize_t read = 0;
    116108
    117     //attente fin trame
    118     while(data!=0x0a)
    119     {
    120         read = serialport->Read(&data,1);
    121         if(read<0)
    122         {
    123             Thread::Err("erreur Read (%s)\n",strerror(-read));
    124         }
     109  // attente fin trame
     110  while (data != 0x0a) {
     111    read = serialport->Read(&data, 1);
     112    if (read < 0) {
     113      Thread::Err("erreur Read (%s)\n", strerror(-read));
    125114    }
     115  }
    126116}
    127117

  • trunk/lib/FlairSensorActuator/src/Mb800.h

    r3 r15  
    1717#include <Gps.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class SerialPort;
    25     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class SerialPort;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30 namespace sensor
    31 {
    32     /*! \class Mb800
    33     *
    34     * \brief Class for mb800 gps receiver
    35     */
    36     class Mb800 : public core::Thread, public Gps
    37     {
    38         public:
    39             /*!
    40             * \brief Constructor
    41             *
    42             * Construct a Mb800.
    43             *
    44             * \param parent parent
    45             * \param name name
    46             * \param serialport serialport
    47             * \param NMEAFlags NMEA sentances to enable
    48             * \param priority priority of the Thread
    49             */
    50             Mb800(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
     26namespace flair {
     27namespace sensor {
     28/*! \class Mb800
     29*
     30* \brief Class for mb800 gps receiver
     31*/
     32class Mb800 : public core::Thread, public Gps {
     33public:
     34  /*!
     35  * \brief Constructor
     36  *
     37  * Construct a Mb800.
     38  *
     39  * \param parent parent
     40  * \param name name
     41  * \param serialport serialport
     42  * \param NMEAFlags NMEA sentances to enable
     43  * \param priority priority of the Thread
     44  */
     45  Mb800(const core::FrameworkManager *parent, std::string name,
     46        core::SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     47        uint8_t priority);
    5148
    52             /*!
    53             * \brief Destructor
    54             *
    55             */
    56             ~Mb800();
     49  /*!
     50  * \brief Destructor
     51  *
     52  */
     53  ~Mb800();
    5754
    58         private:
    59             /*!
    60             * \brief Update using provided datas
    61             *
    62             * Reimplemented from IODevice.
    63             *
    64             * \param data data from the parent to process
    65             */
    66             void UpdateFrom(const core::io_data *data){};
     55private:
     56  /*!
     57  * \brief Update using provided datas
     58  *
     59  * Reimplemented from IODevice.
     60  *
     61  * \param data data from the parent to process
     62  */
     63  void UpdateFrom(const core::io_data *data){};
    6764
    68             /*!
    69             * \brief Run function
    70             *
    71             * Reimplemented from Thread.
    72             *
    73             */
    74             void Run(void);
     65  /*!
     66  * \brief Run function
     67  *
     68  * Reimplemented from Thread.
     69  *
     70  */
     71  void Run(void);
    7572
    76             void Sync(void);
    77             core::SerialPort *serialport;
    78     };
     73  void Sync(void);
     74  core::SerialPort *serialport;
     75};
    7976} // end namespace sensor
    8077} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/Novatel.cpp

    r3 r15  
    2424using namespace flair::core;
    2525
    26 namespace flair
    27 {
    28 namespace sensor
    29 {
     26namespace flair {
     27namespace sensor {
    3028
    31 Novatel::Novatel(const FrameworkManager* parent,string name,SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Gps(parent,name,NMEAFlags), Thread(parent,name,priority)
    32 {
    33     this->serialport=serialport;
     29Novatel::Novatel(const FrameworkManager *parent, string name,
     30                 SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     31                 uint8_t priority)
     32    : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) {
     33  this->serialport = serialport;
    3434}
    3535
    36 Novatel::~Novatel()
    37 {
    38     SafeStop();
    39     Join();
     36Novatel::~Novatel() {
     37  SafeStop();
     38  Join();
    4039}
    4140
    42 void Novatel::Run(void)
    43 {
    44     char response[200] = {0};
    45     int size;
    46     ssize_t written;
     41void Novatel::Run(void) {
     42  char response[200] = {0};
     43  int size;
     44  ssize_t written;
    4745
    48     WarnUponSwitches(true);
     46  WarnUponSwitches(true);
    4947
    50     char to_send[]="log gpggalong ontime 0.05\n";
    51     written=serialport->Write(to_send, sizeof(to_send));
    52     if(written<0)
    53     {
    54         Thread::Err("erreur write (%s)\n",strerror(-written));
     48  char to_send[] = "log gpggalong ontime 0.05\n";
     49  written = serialport->Write(to_send, sizeof(to_send));
     50  if (written < 0) {
     51    Thread::Err("erreur write (%s)\n", strerror(-written));
     52  }
     53  char to_send2[] = "log gpvtg ontime 0.05\n";
     54  written = serialport->Write(to_send2, sizeof(to_send2));
     55  if (written < 0) {
     56    Thread::Err("erreur write (%s)\n", strerror(-written));
     57  }
     58
     59  Sync();
     60
     61  while (!ToBeStopped()) {
     62    size = 0;
     63    while (1) {
     64      // ssize_t read = rt_dev_read(uart_fd, &response[size],1);
     65      ssize_t Read = serialport->Read(&response[size], 1);
     66      if (Read < 0) {
     67        Thread::Err("erreur Read (%s)\n", strerror(-Read));
     68      }
     69      if (response[size] == 0x0a)
     70        break;
     71      size++;
    5572    }
    56     char to_send2[]="log gpvtg ontime 0.05\n";
    57     written=serialport->Write(to_send2, sizeof(to_send2));
    58     if(written<0)
    59     {
    60         Thread::Err("erreur write (%s)\n",strerror(-written));
    61     }
     73    size++;
     74    parseFrame(response, size);
     75  }
    6276
    63     Sync();
    64 
    65     while(!ToBeStopped())
    66     {
    67         size=0;
    68         while(1)
    69         {
    70             //ssize_t read = rt_dev_read(uart_fd, &response[size],1);
    71             ssize_t Read = serialport->Read(&response[size],1);
    72             if(Read<0)
    73             {
    74                 Thread::Err("erreur Read (%s)\n",strerror(-Read));
    75             }
    76             if(response[size]==0x0a) break;
    77             size++;
    78         }
    79         size++;
    80         parseFrame(response, size);
    81     }
    82 
    83     WarnUponSwitches(false);
     77  WarnUponSwitches(false);
    8478}
    8579
    86 void Novatel::Sync(void)
    87 {
    88     char data=0;
    89     ssize_t Read = 0;
     80void Novatel::Sync(void) {
     81  char data = 0;
     82  ssize_t Read = 0;
    9083
    91     //attente fin trame
    92     while(data!=0x0a)
    93     {
    94         Read = serialport->Read(&data,1);
    95         if(Read<0)
    96         {
    97             Thread::Err("erreur Read (%s)\n",strerror(-Read));
    98         }
     84  // attente fin trame
     85  while (data != 0x0a) {
     86    Read = serialport->Read(&data, 1);
     87    if (Read < 0) {
     88      Thread::Err("erreur Read (%s)\n", strerror(-Read));
    9989    }
     90  }
    10091}
    10192

  • trunk/lib/FlairSensorActuator/src/Novatel.h

    r3 r15  
    1717#include <Gps.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class SerialPort;
    25     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class SerialPort;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30 namespace sensor
    31 {
    32     /*! \class Novatel
    33     *
    34     * \brief Class for Novatel gps receiver
    35     */
    36     class Novatel : public core::Thread, public Gps
    37     {
    38         public:
    39             /*!
    40             * \brief Constructor
    41             *
    42             * Construct a Novatel.
    43             *
    44             * \param parent parent
    45             * \param name name
    46             * \param serialport serialport
    47             * \param NMEAFlags NMEA sentances to enable
    48             * \param priority priority of the Thread
    49             */
    50             Novatel(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
     26namespace flair {
     27namespace sensor {
     28/*! \class Novatel
     29*
     30* \brief Class for Novatel gps receiver
     31*/
     32class Novatel : public core::Thread, public Gps {
     33public:
     34  /*!
     35  * \brief Constructor
     36  *
     37  * Construct a Novatel.
     38  *
     39  * \param parent parent
     40  * \param name name
     41  * \param serialport serialport
     42  * \param NMEAFlags NMEA sentances to enable
     43  * \param priority priority of the Thread
     44  */
     45  Novatel(const core::FrameworkManager *parent, std::string name,
     46          core::SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,
     47          uint8_t priority);
    5148
    52             /*!
    53             * \brief Destructor
    54             *
    55             */
    56             ~Novatel();
     49  /*!
     50  * \brief Destructor
     51  *
     52  */
     53  ~Novatel();
    5754
    58         private:
    59             /*!
    60             * \brief Update using provided datas
    61             *
    62             * Reimplemented from IODevice.
    63             *
    64             * \param data data from the parent to process
    65             */
    66             void UpdateFrom(const core::io_data *data){};
     55private:
     56  /*!
     57  * \brief Update using provided datas
     58  *
     59  * Reimplemented from IODevice.
     60  *
     61  * \param data data from the parent to process
     62  */
     63  void UpdateFrom(const core::io_data *data){};
    6764
    68             /*!
    69             * \brief Run function
    70             *
    71             * Reimplemented from Thread.
    72             *
    73             */
    74             void Run(void);
     65  /*!
     66  * \brief Run function
     67  *
     68  * Reimplemented from Thread.
     69  *
     70  */
     71  void Run(void);
    7572
    76             core::SerialPort *serialport;
    77             void Sync(void);
    78     };
     73  core::SerialPort *serialport;
     74  void Sync(void);
     75};
    7976} // end namespace sensor
    8077} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/Ps3Eye.cpp

    r3 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair
    32 {
    33 namespace sensor
    34 {
     31namespace flair {
     32namespace sensor {
    3533
    36 Ps3Eye::Ps3Eye(const FrameworkManager* parent,string name,int camera_index,uint8_t priority)
    37     : V4LCamera(parent,name,camera_index,320,240,cvimage::Type::Format::YUYV,priority) {
     34Ps3Eye::Ps3Eye(const FrameworkManager *parent, string name, int camera_index,
     35               uint8_t priority)
     36    : V4LCamera(parent, name, camera_index, 320, 240,
     37                cvimage::Type::Format::YUYV, priority) {}
    3838
    39 }
    40 
    41 Ps3Eye::~Ps3Eye() {
    42 }
     39Ps3Eye::~Ps3Eye() {}
    4340
    4441} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/Ps3Eye.h

    r3 r15  
    1616#include "V4LCamera.h"
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvimage;
    23         class FrameworkManager;
    24     }
    25     namespace gui
    26     {
    27         class GridLayout;
    28         class DoubleSpinBox;
    29         class CheckBox;
    30     }
     18namespace flair {
     19namespace core {
     20class cvimage;
     21class FrameworkManager;
     22}
     23namespace gui {
     24class GridLayout;
     25class DoubleSpinBox;
     26class CheckBox;
     27}
    3128}
    3229
    33 namespace flair
    34 {
    35 namespace sensor
    36 {
    37     /*! \class Ps3Eye
    38     *
    39     * \brief Class for Ps3Eye camera
    40     */
    41     class Ps3Eye : public V4LCamera
    42     {
     30namespace flair {
     31namespace sensor {
     32/*! \class Ps3Eye
     33*
     34* \brief Class for Ps3Eye camera
     35*/
     36class Ps3Eye : public V4LCamera {
    4337
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct a Ps3Eye.
    49             *
    50             * \param parent parent
    51             * \param name name
    52             * \param camera_index index of the camera, ie /dev/videox
    53             * \param priority priority of the Thread
    54             */
    55             Ps3Eye(const core::FrameworkManager* parent,std::string name,int camera_index,uint8_t priority);
     38public:
     39  /*!
     40  * \brief Constructor
     41  *
     42  * Construct a Ps3Eye.
     43  *
     44  * \param parent parent
     45  * \param name name
     46  * \param camera_index index of the camera, ie /dev/videox
     47  * \param priority priority of the Thread
     48  */
     49  Ps3Eye(const core::FrameworkManager *parent, std::string name,
     50         int camera_index, uint8_t priority);
    5651
    57             /*!
    58             * \brief Destructor
    59             *
    60             */
    61             ~Ps3Eye();
     52  /*!
     53  * \brief Destructor
     54  *
     55  */
     56  ~Ps3Eye();
    6257
    63         private:
    64     };
     58private:
     59};
    6560} // end namespace sensor
    6661} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp

    r3 r15  
    2828using namespace flair::gui;
    2929
    30 namespace flair
    31 {
    32 namespace sensor
    33 {
     30namespace flair {
     31namespace sensor {
    3432
    35 RadioReceiver::RadioReceiver(const FrameworkManager* parent,string name,unsigned int nb_channels) : IODevice(parent,name)
    36 {
    37     is_connected=false;
    38     this->nb_channels=nb_channels;
     33RadioReceiver::RadioReceiver(const FrameworkManager *parent, string name,
     34                             unsigned int nb_channels)
     35    : IODevice(parent, name) {
     36  is_connected = false;
     37  this->nb_channels = nb_channels;
    3938
    40     //init matrix
    41     cvmatrix_descriptor* desc=new cvmatrix_descriptor(nb_channels,1);
    42     for(int i=0;i<nb_channels;i++)
    43     {
    44         ostringstream channel_name;
    45         channel_name << "channel " << i;
    46         desc->SetElementName(i,0,channel_name.str());
    47     }
    48     output=new cvmatrix(this,desc,floatType,name);
     39  // init matrix
     40  cvmatrix_descriptor *desc = new cvmatrix_descriptor(nb_channels, 1);
     41  for (int i = 0; i < nb_channels; i++) {
     42    ostringstream channel_name;
     43    channel_name << "channel " << i;
     44    desc->SetElementName(i, 0, channel_name.str());
     45  }
     46  output = new cvmatrix(this, desc, floatType, name);
    4947
    50      //station sol
    51     main_tab=new Tab(parent->GetTabWidget(),name);
    52     tab=new TabWidget(main_tab->NewRow(),name);
    53     setup_tab=new Tab(tab,"Setup");
     48  // station sol
     49  main_tab = new Tab(parent->GetTabWidget(), name);
     50  tab = new TabWidget(main_tab->NewRow(), name);
     51  setup_tab = new Tab(tab, "Setup");
    5452
    55     AddDataToLog(output);
     53  AddDataToLog(output);
    5654}
    5755
    58 RadioReceiver::~RadioReceiver()
    59 {
    60     delete main_tab;
     56RadioReceiver::~RadioReceiver() { delete main_tab; }
     57
     58Layout *RadioReceiver::GetLayout(void) const { return setup_tab; }
     59
     60float RadioReceiver::ChannelValue(unsigned int id) const {
     61  if (id < nb_channels) {
     62    return output->Value(id, 0);
     63  } else {
     64    Err("channel %i/%i is out of bound\n", id, nb_channels);
     65    return -1;
     66  }
    6167}
    6268
    63 Layout* RadioReceiver::GetLayout(void) const
    64 {
    65     return setup_tab;
    66 }
    67 
    68 float RadioReceiver::ChannelValue(unsigned int id) const
    69 {
    70     if(id<nb_channels)
    71     {
    72         return output->Value(id,0);
    73     }
    74     else
    75     {
    76         Err("channel %i/%i is out of bound\n",id,nb_channels);
    77         return -1;
    78     }
    79 }
    80 
    81 bool RadioReceiver::IsConnected(void) const
    82 {
    83     return is_connected;
    84 }
     69bool RadioReceiver::IsConnected(void) const { return is_connected; }
    8570
    8671} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/RadioReceiver.h

    r3 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class cvmatrix;
    25     }
    26     namespace gui
    27     {
    28         class Tab;
    29         class TabWidget;
    30         class Layout;
    31     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class cvmatrix;
     23}
     24namespace gui {
     25class Tab;
     26class TabWidget;
     27class Layout;
     28}
    3229}
    3330
    34 namespace flair
    35 {
    36 namespace sensor
    37 {
    38     /*! \class RadioReceiver
    39     *
    40     * \brief Base class for radio receiver
    41     */
    42     class RadioReceiver : public core::IODevice
    43     {
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct a RadioReceiver.
    49             *
    50             * \param parent parent
    51             * \param name name
    52             * \param nb_channels number of supported channels
    53             */
    54             RadioReceiver(const core::FrameworkManager* parent,std::string name,unsigned int nb_channels);
     31namespace flair {
     32namespace sensor {
     33/*! \class RadioReceiver
     34*
     35* \brief Base class for radio receiver
     36*/
     37class RadioReceiver : public core::IODevice {
     38public:
     39  /*!
     40  * \brief Constructor
     41  *
     42  * Construct a RadioReceiver.
     43  *
     44  * \param parent parent
     45  * \param name name
     46  * \param nb_channels number of supported channels
     47  */
     48  RadioReceiver(const core::FrameworkManager *parent, std::string name,
     49                unsigned int nb_channels);
    5550
    56             /*!
    57             * \brief Destructor
    58             *
    59             */
    60             ~RadioReceiver();
     51  /*!
     52  * \brief Destructor
     53  *
     54  */
     55  ~RadioReceiver();
    6156
    62             /*!
    63             * \brief get channel value
    64             *
    65             * \param id channel id
    66             * \return value of the channel, between 0 and 1.
    67             *  Returns -1 if channel is out of bound
    68             */
    69             float ChannelValue(unsigned int id) const;
     57  /*!
     58  * \brief get channel value
     59  *
     60  * \param id channel id
     61  * \return value of the channel, between 0 and 1.
     62  *  Returns -1 if channel is out of bound
     63  */
     64  float ChannelValue(unsigned int id) const;
    7065
    71             /*!
    72             * \brief Is transmitter connected?
    73             *
    74             * \return true if transmitter is connected
    75             */
    76             bool IsConnected(void) const;
     66  /*!
     67  * \brief Is transmitter connected?
     68  *
     69  * \return true if transmitter is connected
     70  */
     71  bool IsConnected(void) const;
    7772
    78             /*!
    79             * \brief Setup Layout
    80             *
    81             * \return a Layout available
    82             */
    83             gui::Layout* GetLayout(void) const;
     73  /*!
     74  * \brief Setup Layout
     75  *
     76  * \return a Layout available
     77  */
     78  gui::Layout *GetLayout(void) const;
    8479
    85         private:
    86             /*!
    87             * \brief Update using provided datas
    88             *
    89             * Reimplemented from IODevice.
    90             *
    91             * \param data data from the parent to process
    92             */
    93             void UpdateFrom(const core::io_data *data){};
     80private:
     81  /*!
     82  * \brief Update using provided datas
     83  *
     84  * Reimplemented from IODevice.
     85  *
     86  * \param data data from the parent to process
     87  */
     88  void UpdateFrom(const core::io_data *data){};
    9489
    95             core::cvmatrix *output;
    96             bool is_connected;
    97             unsigned int nb_channels;
    98             gui::Tab* main_tab;
    99             gui::TabWidget* tab;
    100             gui::Tab* setup_tab;
    101     };
     90  core::cvmatrix *output;
     91  bool is_connected;
     92  unsigned int nb_channels;
     93  gui::Tab *main_tab;
     94  gui::TabWidget *tab;
     95  gui::Tab *setup_tab;
     96};
    10297} // end namespace sensor
    10398} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/SimuBldc.cpp

    r3 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair
    32 {
    33 namespace actuator
    34 {
     31namespace flair {
     32namespace actuator {
    3533
    36 SimuBldc::SimuBldc(const IODevice* parent,Layout* layout,string name,uint8_t motors_count,uint32_t dev_id) :Bldc(parent,layout,name,motors_count)
    37 {
    38     ostringstream dev_name;
    39     dev_name << "simu_bldc_" << dev_id;
    40     shmem=new SharedMem(this,dev_name.str().c_str(),motors_count*sizeof(float));
     34SimuBldc::SimuBldc(const IODevice *parent, Layout *layout, string name,
     35                   uint8_t motors_count, uint32_t dev_id)
     36    : Bldc(parent, layout, name, motors_count) {
     37  ostringstream dev_name;
     38  dev_name << "simu_bldc_" << dev_id;
     39  shmem =
     40      new SharedMem(this, dev_name.str().c_str(), motors_count * sizeof(float));
    4141
    42     GroupBox *groupbox=new GroupBox(layout->NewRow(),"simubldc");
    43     k=new DoubleSpinBox(groupbox->NewRow(),"k driver:",0,10000,1);
     42  GroupBox *groupbox = new GroupBox(layout->NewRow(), "simubldc");
     43  k = new DoubleSpinBox(groupbox->NewRow(), "k driver:", 0, 10000, 1);
    4444}
    4545
    46 SimuBldc::SimuBldc(const Object* parent,string name,uint8_t motors_count,uint32_t dev_id) :Bldc(parent,name,motors_count) {
    47     ostringstream dev_name;
    48     dev_name << "simu_bldc_" << dev_id;
    49     shmem=new SharedMem(this,dev_name.str().c_str(),motors_count*sizeof(float));
     46SimuBldc::SimuBldc(const Object *parent, string name, uint8_t motors_count,
     47                   uint32_t dev_id)
     48    : Bldc(parent, name, motors_count) {
     49  ostringstream dev_name;
     50  dev_name << "simu_bldc_" << dev_id;
     51  shmem =
     52      new SharedMem(this, dev_name.str().c_str(), motors_count * sizeof(float));
    5053
    51     //reset values
    52     float values[motors_count];
    53     for(int i=0;i<motors_count;i++) values[i]=0;
     54  // reset values
     55  float values[motors_count];
     56  for (int i = 0; i < motors_count; i++)
     57    values[i] = 0;
    5458
    55     shmem->Write((char*)&values,motors_count*sizeof(float));
     59  shmem->Write((char *)&values, motors_count * sizeof(float));
    5660}
    5761
    58 SimuBldc::~SimuBldc() {
     62SimuBldc::~SimuBldc() {}
     63
     64void SimuBldc::SetMotors(float *value) {
     65  float values[MotorsCount()];
     66
     67  for (int i = 0; i < MotorsCount(); i++)
     68    values[i] = k->Value() * value[i];
     69
     70  shmem->Write((char *)&values, MotorsCount() * sizeof(float));
     71
     72  // on prend une fois pour toute le mutex et on fait des accès directs
     73  output->GetMutex();
     74  for (int i = 0; i < MotorsCount(); i++)
     75    output->SetValueNoMutex(i, 0, values[i]);
     76  output->ReleaseMutex();
    5977}
    6078
    61 void SimuBldc::SetMotors(float* value) {
    62     float values[MotorsCount()];
     79void SimuBldc::GetSpeeds(float *value) const {
     80  float values[MotorsCount()];
     81  shmem->Read((char *)&values, MotorsCount() * sizeof(float));
    6382
    64     for(int i=0;i<MotorsCount();i++) values[i]=k->Value()*value[i];
    65 
    66     shmem->Write((char*)&values,MotorsCount()*sizeof(float));
    67 
    68     //on prend une fois pour toute le mutex et on fait des accès directs
    69     output->GetMutex();
    70     for(int i=0;i<MotorsCount();i++) output->SetValueNoMutex(i,0,values[i]);
    71     output->ReleaseMutex();
    72 }
    73 
    74 void SimuBldc::GetSpeeds(float* value) const {
    75     float values[MotorsCount()];
    76     shmem->Read((char*)&values,MotorsCount()*sizeof(float));
    77 
    78     for(int i=0;i<MotorsCount();i++) value[i]=values[i];
     83  for (int i = 0; i < MotorsCount(); i++)
     84    value[i] = values[i];
    7985}
    8086

  • trunk/lib/FlairSensorActuator/src/SimuBldc.h

    r3 r15  
    1616#include <Bldc.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class SharedMem;
    23         class IODevice;
    24         class cvmatrix;
    25     }
    26     namespace gui
    27     {
    28         class DoubleSpinBox;
    29         class Layout;
    30     }
     18namespace flair {
     19namespace core {
     20class SharedMem;
     21class IODevice;
     22class cvmatrix;
     23}
     24namespace gui {
     25class DoubleSpinBox;
     26class Layout;
     27}
    3128}
    3229
    33 namespace flair
    34 {
    35 namespace actuator
    36 {
    37     /*! \class SimuBldc
    38     *
    39     * \brief Class for a simulation bldc
    40     *
    41     */
    42     class SimuBldc : public Bldc
    43     {
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct a SimuBldc. Control part.
    49             *
    50             * \param parent parent
    51             * \param layout layout
    52             * \param name name
    53             * \param motors_count number of motors
    54             * \param dev_id device id
    55             */
    56             SimuBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,uint8_t motors_count,uint32_t dev_id);
     30namespace flair {
     31namespace actuator {
     32/*! \class SimuBldc
     33*
     34* \brief Class for a simulation bldc
     35*
     36*/
     37class SimuBldc : public Bldc {
     38public:
     39  /*!
     40  * \brief Constructor
     41  *
     42  * Construct a SimuBldc. Control part.
     43  *
     44  * \param parent parent
     45  * \param layout layout
     46  * \param name name
     47  * \param motors_count number of motors
     48  * \param dev_id device id
     49  */
     50  SimuBldc(const core::IODevice *parent, gui::Layout *layout, std::string name,
     51           uint8_t motors_count, uint32_t dev_id);
    5752
    58             /*!
    59             * \brief Constructor
    60             *
    61             * Construct a SimuBldc. Simulation part.
    62             *
    63             * \param parent parent
    64             * \param name name
    65             * \param motors_count number of motors
    66             * \param dev_id device id
    67             */
    68             SimuBldc(const core::Object* parent,std::string name,uint8_t motors_count,uint32_t dev_id);
     53  /*!
     54  * \brief Constructor
     55  *
     56  * Construct a SimuBldc. Simulation part.
     57  *
     58  * \param parent parent
     59  * \param name name
     60  * \param motors_count number of motors
     61  * \param dev_id device id
     62  */
     63  SimuBldc(const core::Object *parent, std::string name, uint8_t motors_count,
     64           uint32_t dev_id);
    6965
    70             /*!
    71             * \brief Destructor
    72             *
    73             */
    74             ~SimuBldc();
     66  /*!
     67  * \brief Destructor
     68  *
     69  */
     70  ~SimuBldc();
    7571
    76             /*!
    77             * \brief Get motors speeds.
    78             *
    79             * This function should only be used for the simulation part.
    80             *
    81             * \param value array to store motors speeds
    82             */
    83             void GetSpeeds(float* value) const;
     72  /*!
     73  * \brief Get motors speeds.
     74  *
     75  * This function should only be used for the simulation part.
     76  *
     77  * \param value array to store motors speeds
     78  */
     79  void GetSpeeds(float *value) const;
    8480
    85             /*!
    86             * \brief Has speed measurement
    87             *
    88             * Reimplemented from Bldc. \n
    89             *
    90             * \return true if it has speed measurement
    91             */
    92             bool HasSpeedMeasurement(void) const{return false;};
     81  /*!
     82  * \brief Has speed measurement
     83  *
     84  * Reimplemented from Bldc. \n
     85  *
     86  * \return true if it has speed measurement
     87  */
     88  bool HasSpeedMeasurement(void) const { return false; };
    9389
    94             /*!
    95             * \brief Has current measurement
    96             *
    97             * Reimplemented from Bldc. \n
    98             *
    99             * \return true if it has current measurement
    100             */
    101             bool HasCurrentMeasurement(void) const{return false;};
     90  /*!
     91  * \brief Has current measurement
     92  *
     93  * Reimplemented from Bldc. \n
     94  *
     95  * \return true if it has current measurement
     96  */
     97  bool HasCurrentMeasurement(void) const { return false; };
    10298
    103         private:
    104             /*!
    105             * \brief Set motors values
    106             *
    107             * Reimplemented from Bldc. \n
    108             * Values size must be the same as MotorsCount()
    109             *
    110             * \param values motor values
    111             */
    112             void SetMotors(float* value);
     99private:
     100  /*!
     101  * \brief Set motors values
     102  *
     103  * Reimplemented from Bldc. \n
     104  * Values size must be the same as MotorsCount()
     105  *
     106  * \param values motor values
     107  */
     108  void SetMotors(float *value);
    113109
    114             core::SharedMem *shmem;
    115             gui::DoubleSpinBox *k;
    116     };
     110  core::SharedMem *shmem;
     111  gui::DoubleSpinBox *k;
     112};
    117113} // end namespace actuator
    118114} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/SimuCamera.cpp

    r3 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair { namespace sensor {
     31namespace flair {
     32namespace sensor {
    3233
    33 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) {
    34     data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,100,1,50);
     34SimuCamera::SimuCamera(const FrameworkManager *parent, string name,
     35                       uint16_t width, uint16_t height, uint8_t channels,
     36                       uint32_t dev_id, uint8_t priority)
     37    : Thread(parent, name, priority),
     38      Camera(parent, name, width, height, cvimage::Type::Format::BGR) {
     39  data_rate =
     40      new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 100, 1, 50);
    3541
    36     buf_size=width*height*channels;
     42  buf_size = width * height * channels;
    3743
    38     img=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,3);
    39     output->img->imageData=img->imageData;
     44  img = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
     45  output->img->imageData = img->imageData;
    4046
    41     ostringstream dev_name;
    42     dev_name << "simu_cam_" << dev_id;
    43     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),buf_size);
     47  ostringstream dev_name;
     48  dev_name << "simu_cam_" << dev_id;
     49  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(), buf_size);
    4450}
    4551
    46 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) {
    47     data_rate=NULL;
     52SimuCamera::SimuCamera(const IODevice *parent, string name, uint16_t width,
     53                       uint16_t height, uint8_t channels, uint32_t dev_id)
     54    : Thread(parent, name, 0), Camera(parent, name) {
     55  data_rate = NULL;
    4856
    49     ostringstream dev_name;
    50     dev_name << "simu_cam_" << dev_id;
    51     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),width*height*channels);
     57  ostringstream dev_name;
     58  dev_name << "simu_cam_" << dev_id;
     59  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     60                        width * height * channels);
    5261}
    5362
    5463SimuCamera::~SimuCamera() {
    55     SafeStop();
    56     Join();
     64  SafeStop();
     65  Join();
    5766}
    5867
    5968void SimuCamera::Run(void) {
    60     if(data_rate==NULL) {
    61         Thread::Err("not applicable for simulation part.\n");
    62         return;
     69  if (data_rate == NULL) {
     70    Thread::Err("not applicable for simulation part.\n");
     71    return;
     72  }
     73
     74  SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
     75
     76  while (!ToBeStopped()) {
     77    WaitPeriod();
     78
     79    if (data_rate->ValueChanged() == true) {
     80      SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
    6381    }
    6482
    65     SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
     83    output->GetMutex();
     84    shmem->Read((char *)img->imageData, buf_size); // remplacer copie par
     85                                                   // échange de pointeur sur
     86                                                   // double buffering
     87    output->ReleaseMutex();
    6688
    67     while(!ToBeStopped()) {
    68         WaitPeriod();
     89    output->SetDataTime(GetTime());
    6990
    70         if(data_rate->ValueChanged()==true) {
    71             SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
    72         }
    73 
    74         output->GetMutex();
    75         shmem->Read((char*)img->imageData,buf_size); // remplacer copie par échange de pointeur sur double buffering
    76         output->ReleaseMutex();
    77 
    78         output->SetDataTime(GetTime());
    79 
    80         ProcessUpdate(output);
    81     }
     91    ProcessUpdate(output);
     92  }
    8293}
    8394

  • trunk/lib/FlairSensorActuator/src/SimuCamera.h

    r3 r15  
    1818#include <cxcore.h>
    1919
    20 namespace flair
    21 {
    22     namespace core
    23     {
    24         class SharedMem;
    25     }
    26     namespace gui
    27     {
    28         class SpinBox;
    29     }
     20namespace flair {
     21namespace core {
     22class SharedMem;
     23}
     24namespace gui {
     25class SpinBox;
     26}
    3027}
    3128
    32 namespace flair
    33 {
    34 namespace sensor
    35 {
    36     /*! \class SimuCamera
    37     *
    38     * \brief Class for a simulation camera
    39     */
    40     class SimuCamera : public core::Thread, public Camera
    41     {
    42         public:
    43             /*!
    44             * \brief Constructor
    45             *
    46             * Construct a SimuCamera. Control part.
    47             *
    48             * \param parent parent
    49             * \param name name
    50             * \param width width
    51             * \param height height
    52             * \param channels number of channels
    53             * \param dev_id device id
    54             * \param priority priority of the Thread
    55             */
    56             SimuCamera(const core::FrameworkManager* parent,std::string name,uint16_t width,uint16_t height,uint8_t channels,uint32_t dev_id,uint8_t priority);
     29namespace flair {
     30namespace sensor {
     31/*! \class SimuCamera
     32*
     33* \brief Class for a simulation camera
     34*/
     35class SimuCamera : public core::Thread, public Camera {
     36public:
     37  /*!
     38  * \brief Constructor
     39  *
     40  * Construct a SimuCamera. Control part.
     41  *
     42  * \param parent parent
     43  * \param name name
     44  * \param width width
     45  * \param height height
     46  * \param channels number of channels
     47  * \param dev_id device id
     48  * \param priority priority of the Thread
     49  */
     50  SimuCamera(const core::FrameworkManager *parent, std::string name,
     51             uint16_t width, uint16_t height, uint8_t channels, uint32_t dev_id,
     52             uint8_t priority);
    5753
    58             /*!
    59             * \brief Constructor
    60             *
    61             * Construct a SimuCamera. Simulation part.\n
    62             * The Thread of this class should not be run.
    63             *
    64             * \param parent parent
    65             * \param name name
    66             * \param width width
    67             * \param height height
    68             * \param channels number of channels
    69             * \param dev_id device id
    70             */
    71             SimuCamera(const core::IODevice* parent,std::string name,uint16_t width,uint16_t height,uint8_t channels,uint32_t dev_id);
     54  /*!
     55  * \brief Constructor
     56  *
     57  * Construct a SimuCamera. Simulation part.\n
     58  * The Thread of this class should not be run.
     59  *
     60  * \param parent parent
     61  * \param name name
     62  * \param width width
     63  * \param height height
     64  * \param channels number of channels
     65  * \param dev_id device id
     66  */
     67  SimuCamera(const core::IODevice *parent, std::string name, uint16_t width,
     68             uint16_t height, uint8_t channels, uint32_t dev_id);
    7269
    73             /*!
    74             * \brief Destructor
    75             *
    76             */
    77             ~SimuCamera();
     70  /*!
     71  * \brief Destructor
     72  *
     73  */
     74  ~SimuCamera();
    7875
    79         protected:
    80             /*!
    81             * \brief SharedMem to access datas
    82             *
    83             */
    84             core::SharedMem *shmem;
     76protected:
     77  /*!
     78  * \brief SharedMem to access datas
     79  *
     80  */
     81  core::SharedMem *shmem;
    8582
    86         private:
    87             /*!
    88             * \brief Run function
    89             *
    90             * Reimplemented from Thread.
    91             *
    92             */
    93             void Run(void);
     83private:
     84  /*!
     85  * \brief Run function
     86  *
     87  * Reimplemented from Thread.
     88  *
     89  */
     90  void Run(void);
    9491
    95             /*!
    96             * \brief Update using provided datas
    97             *
    98             * Reimplemented from IODevice.
    99             *
    100             * \param data data from the parent to process
    101             */
    102             void UpdateFrom(const core::io_data *data){};
     92  /*!
     93  * \brief Update using provided datas
     94  *
     95  * Reimplemented from IODevice.
     96  *
     97  * \param data data from the parent to process
     98  */
     99  void UpdateFrom(const core::io_data *data){};
    103100
    104             gui::SpinBox *data_rate;
    105             size_t buf_size;
    106             IplImage* img;
    107     };
     101  gui::SpinBox *data_rate;
     102  size_t buf_size;
     103  IplImage *img;
     104};
    108105} // end namespace sensor
    109106} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/SimuGps.cpp

    r3 r15  
    2424using namespace flair::core;
    2525
    26 namespace flair
    27 {
    28 namespace sensor
    29 {
     26namespace flair {
     27namespace sensor {
    3028
    31 SimuGps::SimuGps(const FrameworkManager* parent,string name,Gps::NMEAFlags_t NMEAFlags,uint8_t priority) : Thread(parent,name,priority), Gps(parent,name,NMEAFlags)
    32 {
     29SimuGps::SimuGps(const FrameworkManager *parent, string name,
     30                 Gps::NMEAFlags_t NMEAFlags, uint8_t priority)
     31    : Thread(parent, name, priority), Gps(parent, name, NMEAFlags) {}
     32
     33SimuGps::~SimuGps() {
     34  SafeStop();
     35  Join();
    3336}
    3437
    35 SimuGps::~SimuGps()
    36 {
    37     SafeStop();
    38     Join();
     38void SimuGps::Run(void) {
     39  char response[200] = {0};
     40  int size, result;
     41  // double lat=0;
     42  SetPeriodMS(500);
     43  WarnUponSwitches(true);
     44
     45  while (!ToBeStopped()) {
     46    WaitPeriod();
     47    position->SetCoordinates(49.402313, 2.795463, 0);
     48    //      lat+=.5;
     49  }
     50
     51  WarnUponSwitches(false);
    3952}
    40 
    41 void SimuGps::Run(void)
    42 {
    43     char response[200] = {0};
    44     int size,result;
    45 //double lat=0;
    46     SetPeriodMS(500);
    47     WarnUponSwitches(true);
    48 
    49     while(!ToBeStopped())
    50     {
    51         WaitPeriod();
    52         position->SetCoordinates(49.402313, 2.795463,0);
    53   //      lat+=.5;
    54     }
    55 
    56 
    57     WarnUponSwitches(false);
    58 }
    59 
    6053
    6154} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/SimuGps.h

    r3 r15  
    1717#include <Gps.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22}
    2523}
    2624
    27 namespace flair
    28 {
    29 namespace sensor
    30 {
    31     /*! \class SimuGps
    32     *
    33     * \brief Class for a simulation GPS
    34     */
    35     class SimuGps : public core::Thread, public Gps
    36     {
    37         public:
    38             /*!
    39             * \brief Constructor
    40             *
    41             * Construct a Novatel.
    42             *
    43             * \param parent parent
    44             * \param name name
    45             * \param NMEAFlags NMEA sentances to enable
    46             * \param priority priority of the Thread
    47             */
    48             SimuGps(const core::FrameworkManager* parent,std::string name,Gps::NMEAFlags_t NMEAFlags,uint8_t priority);
     25namespace flair {
     26namespace sensor {
     27/*! \class SimuGps
     28*
     29* \brief Class for a simulation GPS
     30*/
     31class SimuGps : public core::Thread, public Gps {
     32public:
     33  /*!
     34  * \brief Constructor
     35  *
     36  * Construct a Novatel.
     37  *
     38  * \param parent parent
     39  * \param name name
     40  * \param NMEAFlags NMEA sentances to enable
     41  * \param priority priority of the Thread
     42  */
     43  SimuGps(const core::FrameworkManager *parent, std::string name,
     44          Gps::NMEAFlags_t NMEAFlags, uint8_t priority);
    4945
    50             /*!
    51             * \brief Destructor
    52             *
    53             */
    54             ~SimuGps();
     46  /*!
     47  * \brief Destructor
     48  *
     49  */
     50  ~SimuGps();
    5551
    56         private:
    57             /*!
    58             * \brief Update using provided datas
    59             *
    60             * Reimplemented from IODevice.
    61             *
    62             * \param data data from the parent to process
    63             */
    64             void UpdateFrom(const core::io_data *data){};
     52private:
     53  /*!
     54  * \brief Update using provided datas
     55  *
     56  * Reimplemented from IODevice.
     57  *
     58  * \param data data from the parent to process
     59  */
     60  void UpdateFrom(const core::io_data *data){};
    6561
    66             /*!
    67             * \brief Run function
    68             *
    69             * Reimplemented from Thread.
    70             *
    71             */
    72             void Run(void);
    73     };
     62  /*!
     63  * \brief Run function
     64  *
     65  * Reimplemented from Thread.
     66  *
     67  */
     68  void Run(void);
     69};
    7470} // end namespace sensor
    7571} // end namespace framewor

  • trunk/lib/FlairSensorActuator/src/SimuImu.cpp

    r3 r15  
    3131using namespace flair::gui;
    3232
    33 namespace flair { namespace sensor {
     33namespace flair {
     34namespace sensor {
    3435
    35 SimuImu::SimuImu(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) :Imu(parent,name),Thread(parent,name,priority) {
    36     data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,200);
    37     ahrsData=new AhrsData((Imu*)this);
     36SimuImu::SimuImu(const FrameworkManager *parent, string name, uint32_t dev_id,
     37                 uint8_t priority)
     38    : Imu(parent, name), Thread(parent, name, priority) {
     39  data_rate =
     40      new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
     41  ahrsData = new AhrsData((Imu *)this);
    3842
    39     ostringstream dev_name;
    40     dev_name << "simu_imu_" << dev_id;
    41     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(imu_states_t));
     43  ostringstream dev_name;
     44  dev_name << "simu_imu_" << dev_id;
     45  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     46                        sizeof(imu_states_t));
    4247}
    4348
    44 SimuImu::SimuImu(const IODevice* parent,string name,uint32_t dev_id) :Imu(parent,name),Thread(parent,name,0) {
    45     data_rate=NULL;
     49SimuImu::SimuImu(const IODevice *parent, string name, uint32_t dev_id)
     50    : Imu(parent, name), Thread(parent, name, 0) {
     51  data_rate = NULL;
    4652
    47     ostringstream dev_name;
    48     dev_name << "simu_imu_" << dev_id;
    49     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(imu_states_t));
     53  ostringstream dev_name;
     54  dev_name << "simu_imu_" << dev_id;
     55  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     56                        sizeof(imu_states_t));
    5057}
    5158
    5259SimuImu::~SimuImu() {
    53     SafeStop();
    54     Join();
     60  SafeStop();
     61  Join();
    5562}
    5663
    5764void SimuImu::UpdateFrom(const io_data *data) {
    58     if(data!=NULL) {
    59         cvmatrix *input=(cvmatrix*)data;
    60         imu_states_t state;
     65  if (data != NULL) {
     66    cvmatrix *input = (cvmatrix *)data;
     67    imu_states_t state;
    6168
    62         input->GetMutex();
    63         state.q0=input->ValueNoMutex(0,0);
    64         state.q1=input->ValueNoMutex(1,0);
    65         state.q2=input->ValueNoMutex(2,0);
    66         state.q3=input->ValueNoMutex(3,0);
    67         state.wx=input->ValueNoMutex(7,0);
    68         state.wy=input->ValueNoMutex(8,0);
    69         state.wz=input->ValueNoMutex(9,0);
    70         input->ReleaseMutex();
     69    input->GetMutex();
     70    state.q0 = input->ValueNoMutex(0, 0);
     71    state.q1 = input->ValueNoMutex(1, 0);
     72    state.q2 = input->ValueNoMutex(2, 0);
     73    state.q3 = input->ValueNoMutex(3, 0);
     74    state.wx = input->ValueNoMutex(7, 0);
     75    state.wy = input->ValueNoMutex(8, 0);
     76    state.wz = input->ValueNoMutex(9, 0);
     77    input->ReleaseMutex();
    7178
    72         shmem->Write((char*)&state,sizeof(imu_states_t));
    73     }
     79    shmem->Write((char *)&state, sizeof(imu_states_t));
     80  }
    7481}
    7582
    7683void SimuImu::Run(void) {
    77     imu_states_t state;
    78     ImuData* imuData;
    79     GetDatas(&imuData);
     84  imu_states_t state;
     85  ImuData *imuData;
     86  GetDatas(&imuData);
    8087
    81     if(data_rate==NULL) {
    82         Thread::Err("not applicable for simulation part.\n");
    83         return;
     88  if (data_rate == NULL) {
     89    Thread::Err("not applicable for simulation part.\n");
     90    return;
     91  }
     92
     93  SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
     94
     95  while (!ToBeStopped()) {
     96    WaitPeriod();
     97
     98    if (data_rate->ValueChanged() == true) {
     99      SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
    84100    }
    85101
    86     SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
     102    shmem->Read((char *)&state, sizeof(imu_states_t));
     103    ahrsData->SetQuaternionAndAngularRates(
     104        Quaternion(state.q0, state.q1, state.q2, state.q3),
     105        Vector3D(state.wx, state.wy, state.wz));
    87106
    88     while(!ToBeStopped()) {
    89         WaitPeriod();
     107    imuData->SetDataTime(GetTime());
     108    ahrsData->SetDataTime(GetTime());
    90109
    91         if(data_rate->ValueChanged()==true) {
    92             SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
    93         }
    94 
    95         shmem->Read((char*)&state,sizeof(imu_states_t));
    96         ahrsData->SetQuaternionAndAngularRates(Quaternion(state.q0,state.q1,state.q2,state.q3)
    97                                                ,Vector3D(state.wx,state.wy,state.wz));
    98 
    99         imuData->SetDataTime(GetTime());
    100         ahrsData->SetDataTime(GetTime());
    101 
    102         UpdateImu();
    103         ProcessUpdate(ahrsData);
    104     }
     110    UpdateImu();
     111    ProcessUpdate(ahrsData);
     112  }
    105113}
    106114

  • trunk/lib/FlairSensorActuator/src/SimuImu.h

    r3 r15  
    1818
    1919namespace flair {
    20     namespace core    {
    21         class SharedMem;
    22         class AhrsData;
    23     }
    24     namespace gui {
    25         class SpinBox;
    26     }
     20namespace core {
     21class SharedMem;
     22class AhrsData;
     23}
     24namespace gui {
     25class SpinBox;
     26}
    2727}
    2828
    29 namespace flair { namespace sensor {
    30     /*! \class SimuImu
    31     *
    32     * \brief Class for a simulation Imu
    33     */
    34     class SimuImu : public Imu, public core::Thread {
    35         public:
    36             /*!
    37             * \brief Constructor
    38             *
    39             * Construct a SimuImu. Control part.
    40             *
    41             * \param parent parent
    42             * \param name name
    43             * \param dev_id device id
    44             * \param priority priority of the Thread
    45             */
    46             SimuImu(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
     29namespace flair {
     30namespace sensor {
     31/*! \class SimuImu
     32*
     33* \brief Class for a simulation Imu
     34*/
     35class SimuImu : public Imu, public core::Thread {
     36public:
     37  /*!
     38  * \brief Constructor
     39  *
     40  * Construct a SimuImu. Control part.
     41  *
     42  * \param parent parent
     43  * \param name name
     44  * \param dev_id device id
     45  * \param priority priority of the Thread
     46  */
     47  SimuImu(const core::FrameworkManager *parent, std::string name,
     48          uint32_t dev_id, uint8_t priority);
    4749
    48             /*!
    49             * \brief Constructor
    50             *
    51             * Construct a SimuImu. Simulation part.\n
    52             * The Thread of this class should not be run.
    53             *
    54             * \param parent parent
    55             * \param name name
    56             * \param dev_id device id
    57             */
    58             SimuImu(const core::IODevice* parent,std::string name,uint32_t dev_id);
     50  /*!
     51  * \brief Constructor
     52  *
     53  * Construct a SimuImu. Simulation part.\n
     54  * The Thread of this class should not be run.
     55  *
     56  * \param parent parent
     57  * \param name name
     58  * \param dev_id device id
     59  */
     60  SimuImu(const core::IODevice *parent, std::string name, uint32_t dev_id);
    5961
    60             /*!
    61             * \brief Destructor
    62             *
    63             */
    64             ~SimuImu();
     62  /*!
     63  * \brief Destructor
     64  *
     65  */
     66  ~SimuImu();
    6567
    66         private:
    67             /*!
    68             * \brief Run function
    69             *
    70             * Reimplemented from Thread.
    71             *
    72             */
    73             void Run(void);
     68private:
     69  /*!
     70  * \brief Run function
     71  *
     72  * Reimplemented from Thread.
     73  *
     74  */
     75  void Run(void);
    7476
    75             /*!
    76             * \brief Update using provided datas
    77             *
    78             * Reimplemented from IODevice.
    79             *
    80             * \param data data from the parent to process
    81             */
    82             void UpdateFrom(const core::io_data *data);
     77  /*!
     78  * \brief Update using provided datas
     79  *
     80  * Reimplemented from IODevice.
     81  *
     82  * \param data data from the parent to process
     83  */
     84  void UpdateFrom(const core::io_data *data);
    8385
    84             typedef struct {
    85                 float q0;
    86                 float q1;
    87                 float q2;
    88                 float q3;
    89                 float wx;
    90                 float wy;
    91                 float wz;
    92             } imu_states_t;
    93             gui::SpinBox *data_rate;
    94             core::SharedMem *shmem;
    95             core::AhrsData *ahrsData;
    96     };
     86  typedef struct {
     87    float q0;
     88    float q1;
     89    float q2;
     90    float q3;
     91    float wx;
     92    float wy;
     93    float wz;
     94  } imu_states_t;
     95  gui::SpinBox *data_rate;
     96  core::SharedMem *shmem;
     97  core::AhrsData *ahrsData;
     98};
    9799} // end namespace sensor
    98100} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/SimuLaser.cpp

    r3 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair
    32 {
    33 namespace sensor
    34 {
     31namespace flair {
     32namespace sensor {
    3533
    36 SimuLaser::SimuLaser(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) : Thread(parent,name,priority), LaserRangeFinder(parent,name)
    37 {
    38     data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,50);
     34SimuLaser::SimuLaser(const FrameworkManager *parent, string name,
     35                     uint32_t dev_id, uint8_t priority)
     36    : Thread(parent, name, priority), LaserRangeFinder(parent, name) {
     37  data_rate =
     38      new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 50);
    3939
    40     ostringstream dev_name;
    41     dev_name << "simu_Laser_" << dev_id;
    42     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),360*sizeof(float)); //****
     40  ostringstream dev_name;
     41  dev_name << "simu_Laser_" << dev_id;
     42  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     43                        360 * sizeof(float)); //****
    4344}
    4445
    45 SimuLaser::SimuLaser(const IODevice* parent,string name,uint32_t dev_id) : Thread(parent,name,0), LaserRangeFinder(parent,name)
    46 {
    47     data_rate=NULL;
     46SimuLaser::SimuLaser(const IODevice *parent, string name, uint32_t dev_id)
     47    : Thread(parent, name, 0), LaserRangeFinder(parent, name) {
     48  data_rate = NULL;
    4849
    49     ostringstream dev_name;
    50     dev_name << "simu_Laser_" << dev_id;
    51     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),360*sizeof(float));
     50  ostringstream dev_name;
     51  dev_name << "simu_Laser_" << dev_id;
     52  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(),
     53                        360 * sizeof(float));
    5254}
    5355
    54 SimuLaser::~SimuLaser()
    55 {
    56     SafeStop();
    57     Join();
     56SimuLaser::~SimuLaser() {
     57  SafeStop();
     58  Join();
    5859}
    5960
    60 void SimuLaser::Run(void)
    61 {
    62     float z[360];
     61void SimuLaser::Run(void) {
     62  float z[360];
    6363
    64     if(data_rate==NULL)
    65     {
    66         Thread::Err("not applicable for simulation part.\n");
    67         return;
     64  if (data_rate == NULL) {
     65    Thread::Err("not applicable for simulation part.\n");
     66    return;
     67  }
     68
     69  SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
     70
     71  while (!ToBeStopped()) {
     72    WaitPeriod();
     73
     74    shmem->Read((char *)z, 360 * sizeof(float));
     75
     76    if (data_rate->ValueChanged() == true) {
     77      SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
    6878    }
    69 
    70     SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
    71 
    72     while(!ToBeStopped())
    73     {
    74         WaitPeriod();
    75 
    76         shmem->Read((char*)z,360*sizeof(float));
    77 
    78         if(data_rate->ValueChanged()==true)
    79         {
    80             SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
    81         }
    82         for(int i=0; i<360; i++)
    83         {
    84             output->SetValue(i,0,z[i]); //*******
    85         }
    86         output->SetDataTime(GetTime());
    87         ProcessUpdate(output);
     79    for (int i = 0; i < 360; i++) {
     80      output->SetValue(i, 0, z[i]); //*******
    8881    }
     82    output->SetDataTime(GetTime());
     83    ProcessUpdate(output);
     84  }
    8985}
    9086

  • trunk/lib/FlairSensorActuator/src/SimuLaser.h

    r3 r15  
    1717#include <Thread.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class SharedMem;
    24     }
    25     namespace gui
    26     {
    27         class SpinBox;
    28     }
     19namespace flair {
     20namespace core {
     21class SharedMem;
     22}
     23namespace gui {
     24class SpinBox;
     25}
    2926}
    3027
    31 namespace flair
    32 {
    33 namespace sensor
    34 {
    35     /*! \class SimuUs
    36     *
    37     * \brief Class for a simulation UsRangeFinder
    38     */
    39     class SimuLaser : public core::Thread, public LaserRangeFinder
    40     {
    41         public:
    42             /*!
    43             * \brief Constructor
    44             *
    45             * Construct a SimuUs. Control part.
    46             *
    47             * \param parent parent
    48             * \param name name
    49             * \param dev_id device id
    50             * \param priority priority of the Thread
    51             */
    52             SimuLaser(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
     28namespace flair {
     29namespace sensor {
     30/*! \class SimuUs
     31*
     32* \brief Class for a simulation UsRangeFinder
     33*/
     34class SimuLaser : public core::Thread, public LaserRangeFinder {
     35public:
     36  /*!
     37  * \brief Constructor
     38  *
     39  * Construct a SimuUs. Control part.
     40  *
     41  * \param parent parent
     42  * \param name name
     43  * \param dev_id device id
     44  * \param priority priority of the Thread
     45  */
     46  SimuLaser(const core::FrameworkManager *parent, std::string name,
     47            uint32_t dev_id, uint8_t priority);
    5348
    54             /*!
    55             * \brief Constructor
    56             *
    57             * Construct a SimuUs. Simulation part.\n
    58             * The Thread of this class should not be run.
    59             *
    60             * \param parent parent
    61             * \param name name
    62             * \param dev_id device id
    63             */
    64             SimuLaser(const core::IODevice* parent,std::string name,uint32_t dev_id);
     49  /*!
     50  * \brief Constructor
     51  *
     52  * Construct a SimuUs. Simulation part.\n
     53  * The Thread of this class should not be run.
     54  *
     55  * \param parent parent
     56  * \param name name
     57  * \param dev_id device id
     58  */
     59  SimuLaser(const core::IODevice *parent, std::string name, uint32_t dev_id);
    6560
    66             /*!
    67             * \brief Destructor
    68             *
    69             */
    70             ~SimuLaser();
     61  /*!
     62  * \brief Destructor
     63  *
     64  */
     65  ~SimuLaser();
    7166
    72         protected:
    73             /*!
    74             * \brief SharedMem to access datas
    75             *
    76             */
    77             core::SharedMem *shmem;
     67protected:
     68  /*!
     69  * \brief SharedMem to access datas
     70  *
     71  */
     72  core::SharedMem *shmem;
    7873
    79         private:
    80             /*!
    81             * \brief Update using provided datas
    82             *
    83             * Reimplemented from IODevice.
    84             *
    85             * \param data data from the parent to process
    86             */
    87             void UpdateFrom(const core::io_data *data){};
     74private:
     75  /*!
     76  * \brief Update using provided datas
     77  *
     78  * Reimplemented from IODevice.
     79  *
     80  * \param data data from the parent to process
     81  */
     82  void UpdateFrom(const core::io_data *data){};
    8883
    89             /*!
    90             * \brief Run function
    91             *
    92             * Reimplemented from Thread.
    93             *
    94             */
    95             void Run(void);
     84  /*!
     85  * \brief Run function
     86  *
     87  * Reimplemented from Thread.
     88  *
     89  */
     90  void Run(void);
    9691
    97             gui::SpinBox *data_rate;
    98     };
     92  gui::SpinBox *data_rate;
     93};
    9994} // end namespace sensor
    10095} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/SimuUs.cpp

    r3 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair
    32 {
    33 namespace sensor
    34 {
     31namespace flair {
     32namespace sensor {
    3533
    36 SimuUs::SimuUs(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) : Thread(parent,name,priority), UsRangeFinder(parent,name)
    37 {
    38     data_rate=new SpinBox(GetGroupBox()->NewRow(),"data rate"," Hz",1,500,1,50);
     34SimuUs::SimuUs(const FrameworkManager *parent, string name, uint32_t dev_id,
     35               uint8_t priority)
     36    : Thread(parent, name, priority), UsRangeFinder(parent, name) {
     37  data_rate =
     38      new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 50);
    3939
    40     ostringstream dev_name;
    41     dev_name << "simu_us_" << dev_id;
    42     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(float));
     40  ostringstream dev_name;
     41  dev_name << "simu_us_" << dev_id;
     42  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(), sizeof(float));
    4343}
    4444
    45 SimuUs::SimuUs(const IODevice* parent,string name,uint32_t dev_id) : Thread(parent,name,0), UsRangeFinder(parent,name)
    46 {
    47     data_rate=NULL;
     45SimuUs::SimuUs(const IODevice *parent, string name, uint32_t dev_id)
     46    : Thread(parent, name, 0), UsRangeFinder(parent, name) {
     47  data_rate = NULL;
    4848
    49     ostringstream dev_name;
    50     dev_name << "simu_us_" << dev_id;
    51     shmem=new SharedMem((Thread*)this,dev_name.str().c_str(),sizeof(float));
     49  ostringstream dev_name;
     50  dev_name << "simu_us_" << dev_id;
     51  shmem = new SharedMem((Thread *)this, dev_name.str().c_str(), sizeof(float));
    5252}
    5353
    54 SimuUs::~SimuUs()
    55 {
    56     SafeStop();
    57     Join();
     54SimuUs::~SimuUs() {
     55  SafeStop();
     56  Join();
    5857}
    5958
    60 void SimuUs::Run(void)
    61 {
    62     float z;
     59void SimuUs::Run(void) {
     60  float z;
    6361
    64     if(data_rate==NULL)
    65     {
    66         Thread::Err("not applicable for simulation part.\n");
    67         return;
     62  if (data_rate == NULL) {
     63    Thread::Err("not applicable for simulation part.\n");
     64    return;
     65  }
     66
     67  SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
     68
     69  while (!ToBeStopped()) {
     70    WaitPeriod();
     71
     72    shmem->Read((char *)&z, sizeof(float));
     73
     74    if (data_rate->ValueChanged() == true) {
     75      SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
    6876    }
    6977
    70     SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
    71 
    72     while(!ToBeStopped())
    73     {
    74         WaitPeriod();
    75 
    76         shmem->Read((char*)&z,sizeof(float));
    77 
    78         if(data_rate->ValueChanged()==true)
    79         {
    80             SetPeriodUS((uint32_t)(1000000./data_rate->Value()));
    81         }
    82 
    83         output->SetValue(0,0,z);
    84         output->SetDataTime(GetTime());
    85         ProcessUpdate(output);
    86     }
     78    output->SetValue(0, 0, z);
     79    output->SetDataTime(GetTime());
     80    ProcessUpdate(output);
     81  }
    8782}
    8883

  • trunk/lib/FlairSensorActuator/src/SimuUs.h

    r3 r15  
    1717#include <Thread.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class SharedMem;
    24     }
    25     namespace gui
    26     {
    27         class SpinBox;
    28     }
     19namespace flair {
     20namespace core {
     21class SharedMem;
     22}
     23namespace gui {
     24class SpinBox;
     25}
    2926}
    3027
    31 namespace flair
    32 {
    33 namespace sensor
    34 {
    35     /*! \class SimuUs
    36     *
    37     * \brief Class for a simulation UsRangeFinder
    38     */
    39     class SimuUs : public core::Thread, public UsRangeFinder
    40     {
    41         public:
    42             /*!
    43             * \brief Constructor
    44             *
    45             * Construct a SimuUs. Control part.
    46             *
    47             * \param parent parent
    48             * \param name name
    49             * \param dev_id device id
    50             * \param priority priority of the Thread
    51             */
    52             SimuUs(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
     28namespace flair {
     29namespace sensor {
     30/*! \class SimuUs
     31*
     32* \brief Class for a simulation UsRangeFinder
     33*/
     34class SimuUs : public core::Thread, public UsRangeFinder {
     35public:
     36  /*!
     37  * \brief Constructor
     38  *
     39  * Construct a SimuUs. Control part.
     40  *
     41  * \param parent parent
     42  * \param name name
     43  * \param dev_id device id
     44  * \param priority priority of the Thread
     45  */
     46  SimuUs(const core::FrameworkManager *parent, std::string name,
     47         uint32_t dev_id, uint8_t priority);
    5348
    54             /*!
    55             * \brief Constructor
    56             *
    57             * Construct a SimuUs. Simulation part.\n
    58             * The Thread of this class should not be run.
    59             *
    60             * \param parent parent
    61             * \param name name
    62             * \param dev_id device id
    63             */
    64             SimuUs(const core::IODevice* parent,std::string name,uint32_t dev_id);
     49  /*!
     50  * \brief Constructor
     51  *
     52  * Construct a SimuUs. Simulation part.\n
     53  * The Thread of this class should not be run.
     54  *
     55  * \param parent parent
     56  * \param name name
     57  * \param dev_id device id
     58  */
     59  SimuUs(const core::IODevice *parent, std::string name, uint32_t dev_id);
    6560
    66             /*!
    67             * \brief Destructor
    68             *
    69             */
    70             ~SimuUs();
     61  /*!
     62  * \brief Destructor
     63  *
     64  */
     65  ~SimuUs();
    7166
    72         protected:
    73             /*!
    74             * \brief SharedMem to access datas
    75             *
    76             */
    77             core::SharedMem *shmem;
     67protected:
     68  /*!
     69  * \brief SharedMem to access datas
     70  *
     71  */
     72  core::SharedMem *shmem;
    7873
    79         private:
    80             /*!
    81             * \brief Update using provided datas
    82             *
    83             * Reimplemented from IODevice.
    84             *
    85             * \param data data from the parent to process
    86             */
    87             void UpdateFrom(const core::io_data *data){};
     74private:
     75  /*!
     76  * \brief Update using provided datas
     77  *
     78  * Reimplemented from IODevice.
     79  *
     80  * \param data data from the parent to process
     81  */
     82  void UpdateFrom(const core::io_data *data){};
    8883
    89             /*!
    90             * \brief Run function
    91             *
    92             * Reimplemented from Thread.
    93             *
    94             */
    95             void Run(void);
     84  /*!
     85  * \brief Run function
     86  *
     87  * Reimplemented from Thread.
     88  *
     89  */
     90  void Run(void);
    9691
    97             gui::SpinBox *data_rate;
    98     };
     92  gui::SpinBox *data_rate;
     93};
    9994} // end namespace sensor
    10095} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/Srf08.cpp

    r3 r15  
    3030using namespace flair::gui;
    3131
    32 namespace flair
    33 {
    34 namespace sensor
    35 {
    36 
    37 Srf08::Srf08(const FrameworkManager* parent,string name,I2cPort* i2cport,uint16_t address,uint8_t priority) : Thread(parent,name,priority), UsRangeFinder(parent,name)
    38 {
    39     is_init=false;
    40 
    41     //default values
    42     //range 46*43+43=2021mm max (2m)
    43     //7:gain=118
    44     //range=46;
    45     //gain=7;
    46 
    47     this->i2cport=i2cport;
    48         this->address=address;
    49 
    50     //station sol
    51     gain=new SpinBox(GetGroupBox()->NewRow(),"gain:",0,255,1,8);
    52     range=new SpinBox(GetGroupBox()->LastRowLastCol(),"range:",0,255,1,46);
    53 
    54 
    55     SetRange();
    56     SetMaxGain();
    57 }
    58 
    59 Srf08::~Srf08()
    60 {
    61     SafeStop();
    62     Join();
    63 }
    64 
    65 void Srf08::Run(void)
    66 {
    67     WarnUponSwitches(true);
    68 
    69     //init srf
     32namespace flair {
     33namespace sensor {
     34
     35Srf08::Srf08(const FrameworkManager *parent, string name, I2cPort *i2cport,
     36             uint16_t address, uint8_t priority)
     37    : Thread(parent, name, priority), UsRangeFinder(parent, name) {
     38  is_init = false;
     39
     40  // default values
     41  // range 46*43+43=2021mm max (2m)
     42  // 7:gain=118
     43  // range=46;
     44  // gain=7;
     45
     46  this->i2cport = i2cport;
     47  this->address = address;
     48
     49  // station sol
     50  gain = new SpinBox(GetGroupBox()->NewRow(), "gain:", 0, 255, 1, 8);
     51  range = new SpinBox(GetGroupBox()->LastRowLastCol(), "range:", 0, 255, 1, 46);
     52
     53  SetRange();
     54  SetMaxGain();
     55}
     56
     57Srf08::~Srf08() {
     58  SafeStop();
     59  Join();
     60}
     61
     62void Srf08::Run(void) {
     63  WarnUponSwitches(true);
     64
     65  // init srf
     66  SendEcho();
     67
     68  SetPeriodMS(20);
     69
     70  while (!ToBeStopped()) {
     71    WaitPeriod();
     72
     73    if (range->ValueChanged() == true)
     74      SetRange();
     75    if (gain->ValueChanged() == true)
     76      SetMaxGain();
     77
     78    GetEcho();
    7079    SendEcho();
    71 
    72     SetPeriodMS(20);
    73 
    74     while(!ToBeStopped())
    75     {
    76         WaitPeriod();
    77 
    78         if(range->ValueChanged()==true) SetRange();
    79         if(gain->ValueChanged()==true) SetMaxGain();
    80 
    81         GetEcho();
    82         SendEcho();
     80  }
     81
     82  WarnUponSwitches(false);
     83}
     84
     85void Srf08::SendEcho(void) {
     86  ssize_t written;
     87  uint8_t tx[2];
     88
     89  tx[0] = 0;  // command register
     90  tx[1] = 82; // ranging mode in usec
     91
     92  i2cport->GetMutex();
     93
     94  i2cport->SetSlave(address);
     95  written = i2cport->Write(tx, 2);
     96  echo_time = GetTime();
     97
     98  i2cport->ReleaseMutex();
     99
     100  if (written < 0) {
     101    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     102  } else if (written != 2) {
     103    Thread::Err("erreur rt_dev_write %i/2\n", written);
     104  }
     105}
     106
     107void Srf08::GetEcho(void) {
     108  float z = 0;
     109  ssize_t written, read;
     110  uint8_t tx, rx[2];
     111  tx = 2;
     112  uint8_t nb_err = 0;
     113
     114  // si l'us est bloqué, on attend 1ms de plus
     115  while (1) {
     116    i2cport->GetMutex();
     117    i2cport->SetSlave(address);
     118    written = i2cport->Write(&tx, 1);
     119
     120    if (written < 0) {
     121      i2cport->ReleaseMutex();
     122      Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     123      nb_err++;
     124      if (nb_err == 20) {
     125        Thread::Err("erreur rt_dev_write (%s), too many errors\n",
     126                    strerror(-written));
     127        return;
     128      }
     129      SleepMS(1);
     130    } else {
     131      read = i2cport->Read(rx, 2);
     132      i2cport->ReleaseMutex();
     133      // rt_printf("%i %i\n",rx[0],rx[1]);
     134      if (read < 0) {
     135        if (read != -ETIMEDOUT)
     136          Thread::Err("erreur rt_dev_read (%s) %i\n", strerror(-written), read);
     137        nb_err++;
     138        if (nb_err == 20) {
     139          Thread::Err("erreur rt_dev_read (%s), too many errors\n",
     140                      strerror(-written));
     141          return;
     142        }
     143        SleepMS(1);
     144      } else if (read != 2) {
     145        Thread::Err("erreur rt_dev_read %i/2\n", read);
     146        return;
     147      } else if (read == 2) {
     148        break;
     149      }
    83150    }
    84 
    85     WarnUponSwitches(false);
    86 }
    87 
    88 void Srf08::SendEcho(void)
    89 {
    90     ssize_t written;
    91     uint8_t tx[2];
    92 
    93     tx[0]=0;//command register
    94     tx[1]=82;//ranging mode in usec
    95 
    96     i2cport->GetMutex();
    97 
    98     i2cport->SetSlave(address);
    99     written = i2cport->Write(tx, 2);
    100     echo_time=GetTime();
    101 
    102     i2cport->ReleaseMutex();
    103 
    104     if(written<0)
    105     {
    106         Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
     151  }
     152
     153  // if(z!=-1)
     154  // revoir ce filtrage!!!
     155  {
     156    z = 0.000001 * (float)(rx[0] * 256 + rx[1]) * 344 /
     157        2; // d=v*t; v=344m/s, t=t usec/2 (aller retour)
     158    // if(z>1) rt_printf("%i %i %f\n",rx[0],rx[1],z);
     159    // on ne permet pas 2 mesures consecutives + grandes de 10cm
     160    if (fabs(z - z_1) > 0.5 && is_init == true) {
     161      Printf("z %f (anc %f) %lld\n", z, z_1, echo_time);
     162      Printf("a revoir on suppose le sol plan\n");
     163      z = z_1 + (z_1 - z_2); // on suppose que l'on continue a la meme vitesse
    107164    }
    108     else if (written != 2)
    109     {
    110         Thread::Err("erreur rt_dev_write %i/2\n",written);
    111     }
    112 
    113 }
    114 
    115 void Srf08::GetEcho(void)
    116 {
    117     float z=0;
    118     ssize_t written,read;
    119     uint8_t tx,rx[2];
    120     tx=2;
    121     uint8_t nb_err=0;
    122 
    123     //si l'us est bloqué, on attend 1ms de plus
    124     while(1)
    125     {
    126         i2cport->GetMutex();
    127         i2cport->SetSlave(address);
    128         written = i2cport->Write(&tx, 1);
    129 
    130         if(written<0)
    131         {
    132             i2cport->ReleaseMutex();
    133             Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
    134             nb_err++;
    135             if(nb_err==20)
    136             {
    137                 Thread::Err("erreur rt_dev_write (%s), too many errors\n",strerror(-written));
    138                 return;
    139             }
    140             SleepMS(1);
    141         }
    142         else
    143         {
    144             read = i2cport->Read(rx, 2);
    145             i2cport->ReleaseMutex();
    146         //rt_printf("%i %i\n",rx[0],rx[1]);
    147             if(read<0)
    148             {
    149                 if(read!=-ETIMEDOUT) Thread::Err("erreur rt_dev_read (%s) %i\n",strerror(-written),read);
    150                 nb_err++;
    151                 if(nb_err==20)
    152                 {
    153                     Thread::Err("erreur rt_dev_read (%s), too many errors\n",strerror(-written));
    154                     return;
    155                 }
    156                 SleepMS(1);
    157             }
    158             else if (read != 2)
    159             {
    160                 Thread::Err("erreur rt_dev_read %i/2\n",read);
    161                 return;
    162             }
    163             else if(read==2)
    164             {
    165                 break;
    166             }
    167         }
    168     }
    169 
    170     //if(z!=-1)
    171     //revoir ce filtrage!!!
    172     {
    173         z=0.000001*(float)(rx[0]*256+rx[1])*344/2;//d=v*t; v=344m/s, t=t usec/2 (aller retour)
    174 //if(z>1) rt_printf("%i %i %f\n",rx[0],rx[1],z);
    175         //on ne permet pas 2 mesures consecutives + grandes de 10cm
    176         if(fabs(z-z_1)>0.5 && is_init==true)
    177         {
    178             Printf("z %f (anc %f) %lld\n",z,z_1,echo_time);
    179             Printf("a revoir on suppose le sol plan\n");
    180             z=z_1+(z_1-z_2);//on suppose que l'on continue a la meme vitesse
    181         }
    182         output->SetValue(0,0,z);
    183         output->SetDataTime(echo_time+(rx[0]*256+rx[1])*1000);
    184         ProcessUpdate(output);
    185         z_2=z_1;
    186         z_1=z;
    187         is_init=true;
    188     }
    189 }
    190 
    191 
    192 void Srf08::SetRange(void)
    193 {
    194     ssize_t written;
    195     uint8_t tx[2];
    196 
    197     tx[0]=2;//range register
    198     tx[1]=range->Value();//range*43+43=dist max en mm
    199 
    200     i2cport->GetMutex();
    201 
    202     i2cport->SetSlave(address);
    203     written = i2cport->Write(tx, 2);
    204 
    205     i2cport->ReleaseMutex();
    206 
    207     if(written<0)
    208     {
    209         Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
    210     }
    211     else if (written != 2)
    212     {
    213         Thread::Err("erreur rt_dev_write %i/2\n",written);
    214     }
    215 
    216 }
    217 
    218 void Srf08::SetMaxGain(void)
    219 {
    220     ssize_t written;
    221     uint8_t tx[2];
    222 
    223     //rt_printf("Srf08::SetMaxGain: %s ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
    224 
    225     tx[0]=1;//max gain register
    226     tx[1]=gain->Value();
    227 
    228     i2cport->GetMutex();
    229 
    230     i2cport->SetSlave(address);
    231     written = i2cport->Write(tx, 2);
    232 
    233     i2cport->ReleaseMutex();
    234 
    235     if(written<0)
    236     {
    237         Thread::Err("erreur write (%s)\n",strerror(-written));
    238     }
    239     else if (written != 2)
    240     {
    241         Thread::Err("erreur write %i/2\n",written);
    242     }
     165    output->SetValue(0, 0, z);
     166    output->SetDataTime(echo_time + (rx[0] * 256 + rx[1]) * 1000);
     167    ProcessUpdate(output);
     168    z_2 = z_1;
     169    z_1 = z;
     170    is_init = true;
     171  }
     172}
     173
     174void Srf08::SetRange(void) {
     175  ssize_t written;
     176  uint8_t tx[2];
     177
     178  tx[0] = 2;              // range register
     179  tx[1] = range->Value(); // range*43+43=dist max en mm
     180
     181  i2cport->GetMutex();
     182
     183  i2cport->SetSlave(address);
     184  written = i2cport->Write(tx, 2);
     185
     186  i2cport->ReleaseMutex();
     187
     188  if (written < 0) {
     189    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     190  } else if (written != 2) {
     191    Thread::Err("erreur rt_dev_write %i/2\n", written);
     192  }
     193}
     194
     195void Srf08::SetMaxGain(void) {
     196  ssize_t written;
     197  uint8_t tx[2];
     198
     199  // rt_printf("Srf08::SetMaxGain: %s
     200  // ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
     201
     202  tx[0] = 1; // max gain register
     203  tx[1] = gain->Value();
     204
     205  i2cport->GetMutex();
     206
     207  i2cport->SetSlave(address);
     208  written = i2cport->Write(tx, 2);
     209
     210  i2cport->ReleaseMutex();
     211
     212  if (written < 0) {
     213    Thread::Err("erreur write (%s)\n", strerror(-written));
     214  } else if (written != 2) {
     215    Thread::Err("erreur write %i/2\n", written);
     216  }
    243217}
    244218

  • trunk/lib/FlairSensorActuator/src/Srf08.h

    r3 r15  
    1717#include <UsRangeFinder.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class I2cPort;
    25     }
    26     namespace gui
    27     {
    28         class SpinBox;;
    29     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class I2cPort;
     23}
     24namespace gui {
     25class SpinBox;
     26;
     27}
    3028}
    3129
    32 namespace flair
    33 {
    34 namespace sensor
    35 {
    36     /*! \class Srf08
    37     *
    38     * \brief Class for ultra sonic SRF08
    39     */
    40     class Srf08 : public core::Thread, public UsRangeFinder
    41     {
     30namespace flair {
     31namespace sensor {
     32/*! \class Srf08
     33*
     34* \brief Class for ultra sonic SRF08
     35*/
     36class Srf08 : public core::Thread, public UsRangeFinder {
    4237
    43         public:
    44             /*!
    45             * \brief Constructor
    46             *
    47             * Construct a SimuUs. Control part.
    48             *
    49             * \param parent parent
    50             * \param name name
    51             * \param i2cport i2c port
    52             * \param address i2c slave address
    53             * \param priority priority of the Thread
    54             */
    55             Srf08(const core::FrameworkManager* parent,std::string name,core::I2cPort* i2cport,uint16_t address,uint8_t priority);
     38public:
     39  /*!
     40  * \brief Constructor
     41  *
     42  * Construct a SimuUs. Control part.
     43  *
     44  * \param parent parent
     45  * \param name name
     46  * \param i2cport i2c port
     47  * \param address i2c slave address
     48  * \param priority priority of the Thread
     49  */
     50  Srf08(const core::FrameworkManager *parent, std::string name,
     51        core::I2cPort *i2cport, uint16_t address, uint8_t priority);
    5652
    57             /*!
    58             * \brief Destructor
    59             *
    60             */
    61             ~Srf08();
     53  /*!
     54  * \brief Destructor
     55  *
     56  */
     57  ~Srf08();
    6258
    63             /*!
    64             * \brief Set Range
    65             *
    66             * check datasheet for values
    67             */
    68             void SetRange(void);
     59  /*!
     60  * \brief Set Range
     61  *
     62  * check datasheet for values
     63  */
     64  void SetRange(void);
    6965
    70             /*!
    71             * \brief Set Max Gain
    72             *
    73             * check datasheet for values
    74             */
    75             void SetMaxGain(void);
     66  /*!
     67  * \brief Set Max Gain
     68  *
     69  * check datasheet for values
     70  */
     71  void SetMaxGain(void);
    7672
    77         private:
    78             /*!
    79             * \brief Update using provided datas
    80             *
    81             * Reimplemented from IODevice.
    82             *
    83             * \param data data from the parent to process
    84             */
    85             void UpdateFrom(const core::io_data *data){};
     73private:
     74  /*!
     75  * \brief Update using provided datas
     76  *
     77  * Reimplemented from IODevice.
     78  *
     79  * \param data data from the parent to process
     80  */
     81  void UpdateFrom(const core::io_data *data){};
    8682
    87             /*!
    88             * \brief Run function
    89             *
    90             * Reimplemented from Thread.
    91             *
    92             */
    93             void Run(void);
     83  /*!
     84  * \brief Run function
     85  *
     86  * Reimplemented from Thread.
     87  *
     88  */
     89  void Run(void);
    9490
    95             void SendEcho(void);
    96             void GetEcho(void);
     91  void SendEcho(void);
     92  void GetEcho(void);
    9793
    98             bool is_init;
    99             core::Time echo_time;
    100             float z_1,z_2;
    101             gui::SpinBox *gain,*range;
    102             uint16_t address;
    103             core::I2cPort* i2cport;
    104     };
     94  bool is_init;
     95  core::Time echo_time;
     96  float z_1, z_2;
     97  gui::SpinBox *gain, *range;
     98  uint16_t address;
     99  core::I2cPort *i2cport;
     100};
    105101} // end namespace sensor
    106102} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/TargetController.cpp

    r3 r15  
    3131using std::string;
    3232
    33 namespace flair
    34 {
    35 namespace sensor
    36 {
     33namespace flair {
     34namespace sensor {
    3735
    38 TargetController::TargetController(const FrameworkManager* parent,string name,uint8_t priority) :
    39     Thread(parent,name,priority),IODevice(parent,name) {
    40     main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
    41     TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
    42     setup_tab=new Tab(tab,"Reglages");
    43 
     36TargetController::TargetController(const FrameworkManager *parent, string name,
     37                                   uint8_t priority)
     38    : Thread(parent, name, priority), IODevice(parent, name) {
     39  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
     40  TabWidget *tab = new TabWidget(main_tab->NewRow(), name);
     41  setup_tab = new Tab(tab, "Reglages");
    4442}
    4543
    4644TargetController::~TargetController() {
    47     SafeStop();
    48     Join();
     45  SafeStop();
     46  Join();
    4947}
    5048
    5149std::string TargetController::GetAxisName(unsigned int axisId) const {
    52     return string("axis")+std::to_string(axisId);
     50  return string("axis") + std::to_string(axisId);
    5351}
    5452
    5553std::string TargetController::GetButtonName(unsigned int buttonId) const {
    56     return string("button")+std::to_string(buttonId);
     54  return string("button") + std::to_string(buttonId);
    5755}
    5856
    5957bool TargetController::SetLedOn(unsigned int ledId) {
    60     if (!IsControllerActionSupported(ControllerAction::SetLedOn)) return false;
    61     SwitchLedMessage *msgLed=new SwitchLedMessage(true,ledId);
    62     changeStateQueue.push(msgLed);
    63     return true;
     58  if (!IsControllerActionSupported(ControllerAction::SetLedOn))
     59    return false;
     60  SwitchLedMessage *msgLed = new SwitchLedMessage(true, ledId);
     61  changeStateQueue.push(msgLed);
     62  return true;
    6463}
    6564
    6665bool TargetController::SetLedOff(unsigned int ledId) {
    67     if (!IsControllerActionSupported(ControllerAction::SetLedOn)) return false;
    68     SwitchLedMessage *msgLed=new SwitchLedMessage(false,ledId);
    69     changeStateQueue.push(msgLed);
    70     return true;
     66  if (!IsControllerActionSupported(ControllerAction::SetLedOn))
     67    return false;
     68  SwitchLedMessage *msgLed = new SwitchLedMessage(false, ledId);
     69  changeStateQueue.push(msgLed);
     70  return true;
    7171}
    7272
    73 bool TargetController::Rumble(unsigned int leftForce,unsigned int leftTimeout,unsigned int rightForce,unsigned int rightTimeout) {
    74     if (!IsControllerActionSupported(ControllerAction::Rumble)) return false;
    75     RumbleMessage *msgRumble=new RumbleMessage(leftForce,leftTimeout,rightForce,rightTimeout);
    76     changeStateQueue.push(msgRumble);
    77     return true;
     73bool TargetController::Rumble(unsigned int leftForce, unsigned int leftTimeout,
     74                              unsigned int rightForce,
     75                              unsigned int rightTimeout) {
     76  if (!IsControllerActionSupported(ControllerAction::Rumble))
     77    return false;
     78  RumbleMessage *msgRumble =
     79      new RumbleMessage(leftForce, leftTimeout, rightForce, rightTimeout);
     80  changeStateQueue.push(msgRumble);
     81  return true;
    7882}
    7983
    80 bool TargetController::FlashLed(unsigned int ledId,unsigned int onTimeout,unsigned int offTimeout){
    81     if (!IsControllerActionSupported(ControllerAction::FlashLed)) return false;
    82     FlashLedMessage *msgFlashLed=new FlashLedMessage(ledId,onTimeout,offTimeout);
    83     changeStateQueue.push(msgFlashLed);
    84     return true;
     84bool TargetController::FlashLed(unsigned int ledId, unsigned int onTimeout,
     85                                unsigned int offTimeout) {
     86  if (!IsControllerActionSupported(ControllerAction::FlashLed))
     87    return false;
     88  FlashLedMessage *msgFlashLed =
     89      new FlashLedMessage(ledId, onTimeout, offTimeout);
     90  changeStateQueue.push(msgFlashLed);
     91  return true;
    8592}
    8693
    87 float TargetController::GetAxisValue(unsigned int axisId) const{
    88     //TODO we'd better throw an exception here
    89     if (axis==NULL) return 0;
     94float TargetController::GetAxisValue(unsigned int axisId) const {
     95  // TODO we'd better throw an exception here
     96  if (axis == NULL)
     97    return 0;
    9098
    91     axis->GetMutex();
    92     float axisValue=axis->Value(axisId, 0);
    93     axis->ReleaseMutex();
    94     return axisValue;
     99  axis->GetMutex();
     100  float axisValue = axis->Value(axisId, 0);
     101  axis->ReleaseMutex();
     102  return axisValue;
    95103}
    96104
    97 bool TargetController::IsButtonPressed(unsigned int buttonId) const{
    98     //TODO we'd better throw an exception here
    99     if (button==NULL) return false;
     105bool TargetController::IsButtonPressed(unsigned int buttonId) const {
     106  // TODO we'd better throw an exception here
     107  if (button == NULL)
     108    return false;
    100109
    101     button->GetMutex();
    102     bool buttonValue=button->Value(buttonId, 0);
    103     button->ReleaseMutex();
    104     return buttonValue;
     110  button->GetMutex();
     111  bool buttonValue = button->Value(buttonId, 0);
     112  button->ReleaseMutex();
     113  return buttonValue;
    105114}
    106115
    107116void TargetController::Run() {
    108     Message *message;
     117  Message *message;
    109118
    110     if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
     119  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
     120    SafeStop();
     121    Thread::Err("An error occured, we don't proceed with the loop.\n");
     122  } else {
     123
     124    axis = new cvmatrix((IODevice *)this, axisNumber, 1, floatType);
     125    button =
     126        new cvmatrix((IODevice *)this, buttonNumber, 1, SignedIntegerType(8));
     127
     128    while (!ToBeStopped()) {
     129      // Thread::Info("Debug: entering acquisition loop\n");
     130      if (getFrameworkManager()->ConnectionLost() == true)
    111131        SafeStop();
    112         Thread::Err("An error occured, we don't proceed with the loop.\n");
    113     } else {
    114132
    115         axis=new cvmatrix((IODevice *)this,axisNumber,1,floatType);
    116         button=new cvmatrix((IODevice *)this,buttonNumber,1,SignedIntegerType(8));
     133      if (IsDataFrameReady()) {
     134        // Thread::Info("Debug: data frame is ready\n");
    117135
    118         while(!ToBeStopped()) {
    119             //Thread::Info("Debug: entering acquisition loop\n");
    120             if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
     136        AcquireAxisData(*axis);
     137        AcquireButtonData(*button);
    121138
    122             if (IsDataFrameReady()) {
    123                 //Thread::Info("Debug: data frame is ready\n");
     139        // send the data
     140        axis->SetDataTime(GetTime());
     141        ProcessUpdate(axis);
    124142
    125                 AcquireAxisData(*axis);
    126                 AcquireButtonData(*button);
     143        // send pending controller state change request(s)
    127144
    128                 //send the data
    129                 axis->SetDataTime(GetTime());
    130                 ProcessUpdate(axis);
    131 
    132                 //send pending controller state change request(s)
    133 
    134                 while(changeStateQueue.size()!=0) {
    135                     message=changeStateQueue.front();
    136                     if (ProcessMessage(message)) {
    137                         changeStateQueue.pop();
    138                         delete message;
    139                     }
    140                 }
    141             } else {
    142                 //Thread::Info("Debug: relax...\n");
    143                 usleep(20000); //20ms
    144             }
     145        while (changeStateQueue.size() != 0) {
     146          message = changeStateQueue.front();
     147          if (ProcessMessage(message)) {
     148            changeStateQueue.pop();
     149            delete message;
     150          }
    145151        }
     152      } else {
     153        // Thread::Info("Debug: relax...\n");
     154        usleep(20000); // 20ms
     155      }
    146156    }
     157  }
    147158}
    148159
    149 Tab* TargetController::GetTab() const {
    150     return setup_tab;
    151 }
     160Tab *TargetController::GetTab() const { return setup_tab; }
    152161
    153162} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/TargetController.h

    r3 r15  
    2525
    2626namespace flair {
    27     namespace core {
    28         class FrameworkManager;
    29         class cvmatrix;
    30         class Socket;
    31         class io_data;
    32     }
    33     namespace gui {
    34         class Tab;
    35         class TabWidget;
    36         class DataPlot1D;
    37     }
     27namespace core {
     28class FrameworkManager;
     29class cvmatrix;
     30class Socket;
     31class io_data;
     32}
     33namespace gui {
     34class Tab;
     35class TabWidget;
     36class DataPlot1D;
     37}
    3838}
    3939
    40 namespace flair { namespace sensor {
    41     enum class ControllerAction;
     40namespace flair {
     41namespace sensor {
     42enum class ControllerAction;
    4243
    43     /*! \class TargetController
    44     *
    45     * \brief Base Class for target side remote controls
    46     *
    47     */
    48     class TargetController : public core::Thread, public core::IODevice {
    49     public:
    50         TargetController(const core::FrameworkManager* parent,std::string name,uint8_t priority=0);
    51         ~TargetController();
    52         //void DrawUserInterface();
    53         virtual bool IsConnected() const=0;
    54         virtual bool IsDataFrameReady()=0;
    55         //axis stuff
    56         unsigned int GetAxisNumber() const;
    57         virtual std::string GetAxisName(unsigned int axisId) const;
    58         float GetAxisValue(unsigned int axisId) const;// always in the range [-1.0,1.0]
    59         //button stuff
    60         unsigned int GetButtonNumber() const;
    61         bool IsButtonPressed(unsigned int buttonId) const;
    62         virtual std::string GetButtonName(unsigned int axisId) const;
    63         //controller state stuff
    64         virtual bool IsControllerActionSupported(ControllerAction action) const {return false;};
    65         bool SetLedOn(unsigned int ledId);
    66         bool SetLedOff(unsigned int ledId);
    67         bool Rumble(unsigned int left_force,unsigned int left_timeout,unsigned int right_force,unsigned int right_timeout);
    68         bool FlashLed(unsigned int ledId,unsigned int on_timeout,unsigned int off_timeout);
    69         void UpdateFrom(const core::io_data *data){}; //TODO
    70         gui::Tab* GetTab(void) const;
     44/*! \class TargetController
     45*
     46* \brief Base Class for target side remote controls
     47*
     48*/
     49class TargetController : public core::Thread, public core::IODevice {
     50public:
     51  TargetController(const core::FrameworkManager *parent, std::string name,
     52                   uint8_t priority = 0);
     53  ~TargetController();
     54  // void DrawUserInterface();
     55  virtual bool IsConnected() const = 0;
     56  virtual bool IsDataFrameReady() = 0;
     57  // axis stuff
     58  unsigned int GetAxisNumber() const;
     59  virtual std::string GetAxisName(unsigned int axisId) const;
     60  float
     61  GetAxisValue(unsigned int axisId) const; // always in the range [-1.0,1.0]
     62  // button stuff
     63  unsigned int GetButtonNumber() const;
     64  bool IsButtonPressed(unsigned int buttonId) const;
     65  virtual std::string GetButtonName(unsigned int axisId) const;
     66  // controller state stuff
     67  virtual bool IsControllerActionSupported(ControllerAction action) const {
     68    return false;
     69  };
     70  bool SetLedOn(unsigned int ledId);
     71  bool SetLedOff(unsigned int ledId);
     72  bool Rumble(unsigned int left_force, unsigned int left_timeout,
     73              unsigned int right_force, unsigned int right_timeout);
     74  bool FlashLed(unsigned int ledId, unsigned int on_timeout,
     75                unsigned int off_timeout);
     76  void UpdateFrom(const core::io_data *data){}; // TODO
     77  gui::Tab *GetTab(void) const;
    7178
    72     protected:
    73         virtual bool ProcessMessage(core::Message *msg)=0;
    74         void QueueMessage(core::Message msg);
    75         virtual bool ControllerInitialization()=0;// {return true;};
    76         //axis stuff
    77         unsigned int axisNumber;
    78         core::cvmatrix* axis=NULL;
    79         virtual void AcquireAxisData(core::cvmatrix &axis)=0; //responsible for getting the axis data from the hardware
    80         uint16_t bitsPerAxis;
    81         //button stuff
    82         unsigned int buttonNumber;
    83         core::cvmatrix* button=NULL;
    84         virtual void AcquireButtonData(core::cvmatrix &button)=0; //responsible for getting the button data from the hardware
    85         //controller state stuff
    86         unsigned int ledNumber;
    87     private:
    88         void Run();
    89         std::queue<core::Message *> changeStateQueue;
    90         flair::gui::Tab* main_tab;
    91         flair::gui::Tab* setup_tab;
    92     };
     79protected:
     80  virtual bool ProcessMessage(core::Message *msg) = 0;
     81  void QueueMessage(core::Message msg);
     82  virtual bool ControllerInitialization() = 0; // {return true;};
     83  // axis stuff
     84  unsigned int axisNumber;
     85  core::cvmatrix *axis = NULL;
     86  virtual void AcquireAxisData(core::cvmatrix &axis) = 0; // responsible for
     87                                                          // getting the axis
     88                                                          // data from the
     89                                                          // hardware
     90  uint16_t bitsPerAxis;
     91  // button stuff
     92  unsigned int buttonNumber;
     93  core::cvmatrix *button = NULL;
     94  virtual void AcquireButtonData(core::cvmatrix &button) = 0; // responsible for
     95                                                              // getting the
     96                                                              // button data
     97                                                              // from the
     98                                                              // hardware
     99  // controller state stuff
     100  unsigned int ledNumber;
    93101
    94 }}
     102private:
     103  void Run();
     104  std::queue<core::Message *> changeStateQueue;
     105  flair::gui::Tab *main_tab;
     106  flair::gui::Tab *setup_tab;
     107};
     108}
     109}
    95110
    96111#endif // TARGETCONTROLLER_H

  • trunk/lib/FlairSensorActuator/src/TargetEthController.cpp

    r3 r15  
    1212//
    1313//  purpose:    class that gets remote controls through an ethernet connection.
    14 //              Typical use case: a remote control is plugged in a workstation and sends remote control
     14//              Typical use case: a remote control is plugged in a workstation
     15//              and sends remote control
    1516//              data to a distant target (this class) through Wifi//
    1617//
    … …  
    3233using std::string;
    3334
    34 namespace flair
    35 {
    36 namespace sensor
    37 {
    38 
    39 TargetEthController::TargetEthController(const FrameworkManager* parent,string name,uint16_t _port,uint8_t priority) :
    40     TargetController(parent,name,priority),listeningPort(_port),receiveCurrentPosition(0) {
    41     const bool blocking=true;
    42     listeningSocket=new TcpSocket(parent,"TEC_listening_socket",blocking,blocking);
    43     dataSocket=new Socket(parent,"TEC_data_socket",_port+1); //receiving side
     35namespace flair {
     36namespace sensor {
     37
     38TargetEthController::TargetEthController(const FrameworkManager *parent,
     39                                         string name, uint16_t _port,
     40                                         uint8_t priority)
     41    : TargetController(parent, name, priority), listeningPort(_port),
     42      receiveCurrentPosition(0) {
     43  const bool blocking = true;
     44  listeningSocket =
     45      new TcpSocket(parent, "TEC_listening_socket", blocking, blocking);
     46  dataSocket =
     47      new Socket(parent, "TEC_data_socket", _port + 1); // receiving side
    4448}
    4549
    4650TargetEthController::~TargetEthController() {
    47     //We are (currently) the server side. We must ask the client side to initiate tcp connexion closing to avoid the server socket
    48     //to get stuck in TIME_WAIT state
    49     Message msg(32);
    50     if (controlSocket) {
    51         Message cancelAcquisition(sizeof(ControllerAction));
    52         ControllerAction exit=ControllerAction::Exit;
    53         memcpy(cancelAcquisition.buffer,&exit,sizeof(ControllerAction));
    54         controlSocket->SendMessage(cancelAcquisition.buffer,cancelAcquisition.bufferSize);
    55         //We don't expect any more data from the client, we're just waiting for the socket to be closed by the client
    56         controlSocket->RecvMessage(msg.buffer,msg.bufferSize);
    57     }
    58 
    59     //TargetController calls TargetEthController methods in its run
    60     //we must stop the thread now
    61     SafeStop();
    62     Join();
     51  // We are (currently) the server side. We must ask the client side to initiate
     52  // tcp connexion closing to avoid the server socket
     53  // to get stuck in TIME_WAIT state
     54  Message msg(32);
     55  if (controlSocket) {
     56    Message cancelAcquisition(sizeof(ControllerAction));
     57    ControllerAction exit = ControllerAction::Exit;
     58    memcpy(cancelAcquisition.buffer, &exit, sizeof(ControllerAction));
     59    controlSocket->SendMessage(cancelAcquisition.buffer,
     60                               cancelAcquisition.bufferSize);
     61    // We don't expect any more data from the client, we're just waiting for the
     62    // socket to be closed by the client
     63    controlSocket->RecvMessage(msg.buffer, msg.bufferSize);
     64  }
     65
     66  // TargetController calls TargetEthController methods in its run
     67  // we must stop the thread now
     68  SafeStop();
     69  Join();
    6370}
    6471
    6572bool TargetEthController::IsConnected() const {
    66     //TODO
    67 }
    68 
    69 bool TargetEthController::IsDataFrameReady(){
    70     //read up to the last data
    71     ssize_t received;
    72     size_t bytesToReceive=dataFrameSize-receiveCurrentPosition;
    73     bool fullDatagramReceived=false;
    74 
    75     do {
    76         received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
    77         if (received>0) {
    78             bytesToReceive-=received;
    79             if (bytesToReceive==0) {
    80                 //a full datagram has been read in receiveFrameBuffer
    81                 fullDatagramReceived=true;
    82                 receiveCurrentPosition=0;
    83                 //we swap the data and reception buffers to avoid copy
    84                 char *swapFrameBuffer=dataFrameBuffer;
    85                 dataFrameBuffer=receiveFrameBuffer;
    86                 receiveFrameBuffer=swapFrameBuffer;
    87             }
    88         }
    89     } while (!received<0);
    90 
    91     return fullDatagramReceived;
    92 }
    93 
    94 uint8_t TargetEthController::getByteOrNull(char *buffer,int byte,size_t bufferSize) {
    95     if (byte<bufferSize) return buffer[byte];
    96     else return 0;
    97 }
    98 
    99 uint32_t TargetEthController::charBufferToUint32(char *buffer, size_t bufferSize) {
    100     union {
    101         uint32_t int32;
    102         char byte[4];
    103     } bitField;
    104     if (!IsBigEndian()) {
    105         bitField.byte[0]=getByteOrNull(buffer,3,bufferSize);
    106         bitField.byte[1]=getByteOrNull(buffer,2,bufferSize);
    107         bitField.byte[2]=getByteOrNull(buffer,1,bufferSize);
    108         bitField.byte[3]=getByteOrNull(buffer,0,bufferSize);
    109     } else {
    110         bitField.byte[0]=getByteOrNull(buffer,0,bufferSize);
    111         bitField.byte[1]=getByteOrNull(buffer,1,bufferSize);
    112         bitField.byte[2]=getByteOrNull(buffer,2,bufferSize);
    113         bitField.byte[3]=getByteOrNull(buffer,3,bufferSize);
    114     }
    115     return bitField.int32;
    116 }
    117 
    118 //read _up_to_16_bits_ in a buffer
    119 uint16_t TargetEthController::readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize) {
    120     //parameters check
    121     if (valueSizeInBits>16) throw std::range_error("bitfield should be at max 16bits wide");
    122     size_t minBufferSize=(offsetInBits+valueSizeInBits)/8;
    123     if ((offsetInBits+valueSizeInBits)%8!=0) minBufferSize++;
    124     if (bufferSize<minBufferSize) throw std::range_error("buffer too small");
    125     //skip first bytes
    126     size_t bytesToSkip=offsetInBits/8;
    127     buffer+=bytesToSkip;
    128     bufferSize-=bytesToSkip;
    129     offsetInBits-=bytesToSkip*8;
    130     //take care of endianness
    131     uint32_t value=charBufferToUint32(buffer,bufferSize);
    132     value>>=32-offsetInBits-valueSizeInBits;
    133     value&=(1<<valueSizeInBits)-1;
    134     return (uint16_t)value;
     73  // TODO
     74}
     75
     76bool TargetEthController::IsDataFrameReady() {
     77  // read up to the last data
     78  ssize_t received;
     79  size_t bytesToReceive = dataFrameSize - receiveCurrentPosition;
     80  bool fullDatagramReceived = false;
     81
     82  do {
     83    received =
     84        dataSocket->RecvMessage(receiveFrameBuffer + receiveCurrentPosition,
     85                                bytesToReceive, TIME_NONBLOCK);
     86    if (received > 0) {
     87      bytesToReceive -= received;
     88      if (bytesToReceive == 0) {
     89        // a full datagram has been read in receiveFrameBuffer
     90        fullDatagramReceived = true;
     91        receiveCurrentPosition = 0;
     92        // we swap the data and reception buffers to avoid copy
     93        char *swapFrameBuffer = dataFrameBuffer;
     94        dataFrameBuffer = receiveFrameBuffer;
     95        receiveFrameBuffer = swapFrameBuffer;
     96      }
     97    }
     98  } while (!received < 0);
     99
     100  return fullDatagramReceived;
     101}
     102
     103uint8_t TargetEthController::getByteOrNull(char *buffer, int byte,
     104                                           size_t bufferSize) {
     105  if (byte < bufferSize)
     106    return buffer[byte];
     107  else
     108    return 0;
     109}
     110
     111uint32_t TargetEthController::charBufferToUint32(char *buffer,
     112                                                 size_t bufferSize) {
     113  union {
     114    uint32_t int32;
     115    char byte[4];
     116  } bitField;
     117  if (!IsBigEndian()) {
     118    bitField.byte[0] = getByteOrNull(buffer, 3, bufferSize);
     119    bitField.byte[1] = getByteOrNull(buffer, 2, bufferSize);
     120    bitField.byte[2] = getByteOrNull(buffer, 1, bufferSize);
     121    bitField.byte[3] = getByteOrNull(buffer, 0, bufferSize);
     122  } else {
     123    bitField.byte[0] = getByteOrNull(buffer, 0, bufferSize);
     124    bitField.byte[1] = getByteOrNull(buffer, 1, bufferSize);
     125    bitField.byte[2] = getByteOrNull(buffer, 2, bufferSize);
     126    bitField.byte[3] = getByteOrNull(buffer, 3, bufferSize);
     127  }
     128  return bitField.int32;
     129}
     130
     131// read _up_to_16_bits_ in a buffer
     132uint16_t TargetEthController::readBits(uint8_t offsetInBits,
     133                                       uint8_t valueSizeInBits, char *buffer,
     134                                       size_t bufferSize) {
     135  // parameters check
     136  if (valueSizeInBits > 16)
     137    throw std::range_error("bitfield should be at max 16bits wide");
     138  size_t minBufferSize = (offsetInBits + valueSizeInBits) / 8;
     139  if ((offsetInBits + valueSizeInBits) % 8 != 0)
     140    minBufferSize++;
     141  if (bufferSize < minBufferSize)
     142    throw std::range_error("buffer too small");
     143  // skip first bytes
     144  size_t bytesToSkip = offsetInBits / 8;
     145  buffer += bytesToSkip;
     146  bufferSize -= bytesToSkip;
     147  offsetInBits -= bytesToSkip * 8;
     148  // take care of endianness
     149  uint32_t value = charBufferToUint32(buffer, bufferSize);
     150  value >>= 32 - offsetInBits - valueSizeInBits;
     151  value &= (1 << valueSizeInBits) - 1;
     152  return (uint16_t)value;
    135153}
    136154
    137155void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
    138     axis.GetMutex();
    139 //char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
    140     for (unsigned int i=0;i<axisNumber;i++) {
    141         uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,dataFrameBuffer,dataFrameSize);
    142 //        uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
    143         uint16_t scale=1<<(bitsPerAxis-1);
    144 //Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
    145         axis.SetValueNoMutex(i,0,(rawAxisValue-scale)/(float)scale);
    146     }
    147     axis.ReleaseMutex();
     156  axis.GetMutex();
     157  // char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
     158  for (unsigned int i = 0; i < axisNumber; i++) {
     159    uint16_t rawAxisValue =
     160        readBits(i * bitsPerAxis, bitsPerAxis, dataFrameBuffer, dataFrameSize);
     161    //        uint16_t
     162    //        rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
     163    uint16_t scale = 1 << (bitsPerAxis - 1);
     164    // Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float
     165    // value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
     166    axis.SetValueNoMutex(i, 0, (rawAxisValue - scale) / (float)scale);
     167  }
     168  axis.ReleaseMutex();
    148169}
    149170
    150171void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
    151     uint8_t buttonValue;
    152     int currentButton=0;
    153     button.GetMutex();
    154     /*
    155     for (unsigned int i=0;i<buttonNumber;i++) {
    156         memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
    157         dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
    158         button.SetValueNoMutex(i,0,buttonValue);
    159 //        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n", GetButtonName(i).c_str());
    160     }*/
    161     for (unsigned int i=0;i<buttonNumber/8;i++) {
    162         memcpy(&buttonValue,dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),sizeof(uint8_t));
    163 //        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
    164         for(unsigned int j=0;j<8;j++) {
    165             button.SetValueNoMutex(currentButton,0,(buttonValue>>j)&0x01);
    166             currentButton++;
    167         }
    168     }
    169     button.ReleaseMutex();
    170 }
    171 
    172 string TargetEthController::GetAxisName(unsigned int axisId) const{
    173     //TODO: should throw an exception if axisName==NULL or axisId>axisNumber
    174     return axisName[axisId];
    175 }
    176 
    177 string TargetEthController::GetButtonName(unsigned int buttonId) const{
    178     //TODO: should throw an exception if buttonName==NULL or buttonId>buttonNumber
    179     return buttonName[buttonId];
     172  uint8_t buttonValue;
     173  int currentButton = 0;
     174  button.GetMutex();
     175  /*
     176  for (unsigned int i=0;i<buttonNumber;i++) {
     177      memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
     178      dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
     179      button.SetValueNoMutex(i,0,buttonValue);
     180//        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n",
     181GetButtonName(i).c_str());
     182  }*/
     183  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
     184    memcpy(&buttonValue, dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
     185           sizeof(uint8_t));
     186    //        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
     187    for (unsigned int j = 0; j < 8; j++) {
     188      button.SetValueNoMutex(currentButton, 0, (buttonValue >> j) & 0x01);
     189      currentButton++;
     190    }
     191  }
     192  button.ReleaseMutex();
     193}
     194
     195string TargetEthController::GetAxisName(unsigned int axisId) const {
     196  // TODO: should throw an exception if axisName==NULL or axisId>axisNumber
     197  return axisName[axisId];
     198}
     199
     200string TargetEthController::GetButtonName(unsigned int buttonId) const {
     201  // TODO: should throw an exception if buttonName==NULL or
     202  // buttonId>buttonNumber
     203  return buttonName[buttonId];
    180204}
    181205
    182206bool TargetEthController::ProcessMessage(Message *msg) {
    183     return !(controlSocket->SendMessage(msg->buffer,msg->bufferSize,0)<0);
    184 }
    185 
    186 bool TargetEthController::IsControllerActionSupported(ControllerAction action) const {
    187     //TODO: here we should ask the remote side (host). Probably through the control socket
    188     switch(action) {
    189     case ControllerAction::SetLedOn:
    190         return true;
    191     case ControllerAction::SetLedOff:
    192         return true;
    193     case ControllerAction::Rumble:
    194         return true;
    195     case ControllerAction::FlashLed:
    196         return true;
    197     default:
     207  return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, 0) < 0);
     208}
     209
     210bool TargetEthController::IsControllerActionSupported(
     211    ControllerAction action) const {
     212  // TODO: here we should ask the remote side (host). Probably through the
     213  // control socket
     214  switch (action) {
     215  case ControllerAction::SetLedOn:
     216    return true;
     217  case ControllerAction::SetLedOff:
     218    return true;
     219  case ControllerAction::Rumble:
     220    return true;
     221  case ControllerAction::FlashLed:
     222    return true;
     223  default:
     224    return false;
     225  }
     226}
     227
     228bool TargetEthController::ControllerInitialization() {
     229  char message[1024];
     230  ssize_t received;
     231  bool connected = false;
     232  bool mustReadMore;
     233
     234  listeningSocket->Listen(listeningPort);
     235  Thread::Info("Debug: Listening to port %d\n", listeningPort);
     236  // accept incoming connection
     237  bool connectionAccepted = false;
     238  while (!connectionAccepted) {
     239    controlSocket = listeningSocket->Accept(10);
     240    if (controlSocket == nullptr) {
     241      // Timeout (or error btw)
     242      if (ToBeStopped())
    198243        return false;
    199     }
    200 }
    201 
    202 bool TargetEthController::ControllerInitialization() {
    203     char message[1024];
    204     ssize_t received;
    205     bool connected=false;
    206     bool mustReadMore;
    207 
    208     listeningSocket->Listen(listeningPort);
    209     Thread::Info("Debug: Listening to port %d\n",listeningPort);
    210                  //accept incoming connection
    211     bool connectionAccepted=false;
    212     while (!connectionAccepted) {
    213         controlSocket=listeningSocket->Accept(10);
    214         if (controlSocket==nullptr) {
    215             //Timeout (or error btw)
    216             if (ToBeStopped()) return false;
    217         } else connectionAccepted=true;
    218     }
    219     Thread::Info("Debug: Connexion accepted\n");
    220 
    221     //get axis stuff
    222     bool axisNumberRead=false;
    223     while (!axisNumberRead) {
    224         try {
    225             axisNumber=controlSocket->ReadUInt32(10);
    226             //Thread::Info("Debug: axisNumber %d\n", axisNumber);
    227             axisNumberRead=true;
    228         }
    229         catch (std::runtime_error e) {
    230             //timeout
    231             if (ToBeStopped()) return false;
    232         }
    233     }
    234     bool bitsPerAxisRead=false;
    235     while (!bitsPerAxisRead) {
    236         try {
    237             bitsPerAxis=controlSocket->ReadUInt32(10);
    238             //Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
    239             bitsPerAxisRead=true;
    240         }
    241         catch (std::runtime_error e) {
    242             //timeout
    243             if (ToBeStopped()) return false;
    244         }
    245     }
    246     axisName=new string[axisNumber];
    247     for (unsigned int i=0;i<axisNumber;i++) {
    248         //read string size
    249         int stringSize;
    250         bool stringSizeRead=false;
    251         while (!stringSizeRead) {
    252             try {
    253                 stringSize=controlSocket->ReadUInt32(10);
    254                 stringSizeRead=true;
    255             }
    256             catch (std::runtime_error e) {
    257                 //timeout
    258                 if (ToBeStopped()) return false;
    259             }
    260         }
    261         //read string
    262         bool axisNameRead=false;
    263         while (!axisNameRead) {
    264             try {
    265                 axisName[i]=controlSocket->ReadString(stringSize,10);
    266                 axisNameRead=true;
    267             }
    268             catch (std::runtime_error e) {
    269                 //timeout
    270                 if (ToBeStopped()) return false;
    271             }
    272         }
    273         //Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
    274     }
    275 
    276     //get button stuff
    277     bool buttonNumberRead=false;
    278     while (!buttonNumberRead) {
    279         try {
    280             buttonNumber=controlSocket->ReadUInt32(10);
    281             buttonNumberRead=true;
    282         }
    283         catch (std::runtime_error e) {
    284             //timeout
    285             if (ToBeStopped()) return false;
    286         }
    287     }
    288     //Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
    289     buttonName=new string[buttonNumber];
    290     for (unsigned int i=0;i<buttonNumber;i++) {
    291         //read string size
    292         int stringSize;
    293         bool stringSizeRead=false;
    294         while (!stringSizeRead) {
    295             try {
    296                 stringSize=controlSocket->ReadUInt32(10);
    297                 stringSizeRead=true;
    298             }
    299             catch (std::runtime_error e) {
    300                 //timeout
    301                 if (ToBeStopped()) return false;
    302             }
    303         }
    304         //read string
    305         bool buttonNameRead=false;
    306         while (!buttonNameRead) {
    307             try {
    308                 buttonName[i]=controlSocket->ReadString(stringSize,10);
    309                 buttonNameRead=true;
    310             }
    311             catch (std::runtime_error e) {
    312                 //timeout
    313                 if (ToBeStopped()) return false;
    314             }
    315         }
    316         //Thread::Info("Debug: buttonName for button %d %s\n", i, buttonName[i].c_str());
    317     }
    318 
    319 //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
    320     buttonOffset=(axisNumber*bitsPerAxis)/8;
    321     if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
    322     dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
    323     if ((buttonNumber%8)!=0) dataFrameSize++;
    324     dataFrameBuffer=new char[dataFrameSize];
    325     receiveFrameBuffer=new char[dataFrameSize];
    326 
    327     Thread::Info("Controller connected with host side\n");
    328     if(buttonNumber%8!=0) Thread::Err("Button number is not multiple of 8\n");
    329     return true;
     244    } else
     245      connectionAccepted = true;
     246  }
     247  Thread::Info("Debug: Connexion accepted\n");
     248
     249  // get axis stuff
     250  bool axisNumberRead = false;
     251  while (!axisNumberRead) {
     252    try {
     253      axisNumber = controlSocket->ReadUInt32(10);
     254      // Thread::Info("Debug: axisNumber %d\n", axisNumber);
     255      axisNumberRead = true;
     256    } catch (std::runtime_error e) {
     257      // timeout
     258      if (ToBeStopped())
     259        return false;
     260    }
     261  }
     262  bool bitsPerAxisRead = false;
     263  while (!bitsPerAxisRead) {
     264    try {
     265      bitsPerAxis = controlSocket->ReadUInt32(10);
     266      // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
     267      bitsPerAxisRead = true;
     268    } catch (std::runtime_error e) {
     269      // timeout
     270      if (ToBeStopped())
     271        return false;
     272    }
     273  }
     274  axisName = new string[axisNumber];
     275  for (unsigned int i = 0; i < axisNumber; i++) {
     276    // read string size
     277    int stringSize;
     278    bool stringSizeRead = false;
     279    while (!stringSizeRead) {
     280      try {
     281        stringSize = controlSocket->ReadUInt32(10);
     282        stringSizeRead = true;
     283      } catch (std::runtime_error e) {
     284        // timeout
     285        if (ToBeStopped())
     286          return false;
     287      }
     288    }
     289    // read string
     290    bool axisNameRead = false;
     291    while (!axisNameRead) {
     292      try {
     293        axisName[i] = controlSocket->ReadString(stringSize, 10);
     294        axisNameRead = true;
     295      } catch (std::runtime_error e) {
     296        // timeout
     297        if (ToBeStopped())
     298          return false;
     299      }
     300    }
     301    // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
     302  }
     303
     304  // get button stuff
     305  bool buttonNumberRead = false;
     306  while (!buttonNumberRead) {
     307    try {
     308      buttonNumber = controlSocket->ReadUInt32(10);
     309      buttonNumberRead = true;
     310    } catch (std::runtime_error e) {
     311      // timeout
     312      if (ToBeStopped())
     313        return false;
     314    }
     315  }
     316  // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
     317  buttonName = new string[buttonNumber];
     318  for (unsigned int i = 0; i < buttonNumber; i++) {
     319    // read string size
     320    int stringSize;
     321    bool stringSizeRead = false;
     322    while (!stringSizeRead) {
     323      try {
     324        stringSize = controlSocket->ReadUInt32(10);
     325        stringSizeRead = true;
     326      } catch (std::runtime_error e) {
     327        // timeout
     328        if (ToBeStopped())
     329          return false;
     330      }
     331    }
     332    // read string
     333    bool buttonNameRead = false;
     334    while (!buttonNameRead) {
     335      try {
     336        buttonName[i] = controlSocket->ReadString(stringSize, 10);
     337        buttonNameRead = true;
     338      } catch (std::runtime_error e) {
     339        // timeout
     340        if (ToBeStopped())
     341          return false;
     342      }
     343    }
     344    // Thread::Info("Debug: buttonName for button %d %s\n", i,
     345    // buttonName[i].c_str());
     346  }
     347
     348  //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
     349  buttonOffset = (axisNumber * bitsPerAxis) / 8;
     350  if ((axisNumber * bitsPerAxis) % 8 != 0)
     351    buttonOffset++;
     352  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
     353  if ((buttonNumber % 8) != 0)
     354    dataFrameSize++;
     355  dataFrameBuffer = new char[dataFrameSize];
     356  receiveFrameBuffer = new char[dataFrameSize];
     357
     358  Thread::Info("Controller connected with host side\n");
     359  if (buttonNumber % 8 != 0)
     360    Thread::Err("Button number is not multiple of 8\n");
     361  return true;
    330362}
    331363

  • trunk/lib/FlairSensorActuator/src/TargetEthController.h

    r3 r15  
    1212//
    1313//  purpose:    class that gets remote controls through an ethernet connection.
    14 //              Typical use case: a remote control is plugged in a workstation and sends remote control
     14//              Typical use case: a remote control is plugged in a workstation
     15//              and sends remote control
    1516//              data to a distant target (this class) through Wifi
    1617//
    … …  
    2425
    2526namespace flair {
    26     namespace core {
    27         class FrameworkManager;
    28         class cvmatrix;
    29         class TcpSocket;
    30         class Socket;
    31     }
    32     namespace gui {
    33         class Tab;
    34         class TabWidget;
    35         class DataPlot1D;
    36     }
     27namespace core {
     28class FrameworkManager;
     29class cvmatrix;
     30class TcpSocket;
     31class Socket;
     32}
     33namespace gui {
     34class Tab;
     35class TabWidget;
     36class DataPlot1D;
     37}
    3738}
    3839
    39 namespace flair { namespace sensor {
    40     /*! \class TargetController
    41     *
    42     * \brief Base Class for target side remote controls
    43     *
    44     */
    45     class TargetEthController : public TargetController {
    46     public:
    47         TargetEthController(const core::FrameworkManager* parent,std::string name,uint16_t port,uint8_t priority=0);
    48         ~TargetEthController();
    49         //void DrawUserInterface();
    50     protected:
    51         bool IsConnected() const;
    52         //axis stuff
    53         std::string GetAxisName(unsigned int axisId) const;
    54         //button stuff
    55         std::string GetButtonName(unsigned int axisId) const;
    56         //controller state stuff
    57         bool ProcessMessage(core::Message *msg);
    58         bool IsControllerActionSupported(ControllerAction action) const;
     40namespace flair {
     41namespace sensor {
     42/*! \class TargetController
     43*
     44* \brief Base Class for target side remote controls
     45*
     46*/
     47class TargetEthController : public TargetController {
     48public:
     49  TargetEthController(const core::FrameworkManager *parent, std::string name,
     50                      uint16_t port, uint8_t priority = 0);
     51  ~TargetEthController();
     52  // void DrawUserInterface();
     53protected:
     54  bool IsConnected() const;
     55  // axis stuff
     56  std::string GetAxisName(unsigned int axisId) const;
     57  // button stuff
     58  std::string GetButtonName(unsigned int axisId) const;
     59  // controller state stuff
     60  bool ProcessMessage(core::Message *msg);
     61  bool IsControllerActionSupported(ControllerAction action) const;
    5962
    60         bool IsDataFrameReady();
    61         void AcquireAxisData(core::cvmatrix &axis); //responsible for getting the axis data from the hardware
    62         void AcquireButtonData(core::cvmatrix &button); //responsible for getting the button data from the hardware
     63  bool IsDataFrameReady();
     64  void AcquireAxisData(core::cvmatrix &axis); // responsible for getting the
     65                                              // axis data from the hardware
     66  void AcquireButtonData(core::cvmatrix &button); // responsible for getting the
     67                                                  // button data from the
     68                                                  // hardware
    6369
    64         bool ControllerInitialization();
     70  bool ControllerInitialization();
    6571
    66     private:
    67         uint16_t readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize);
    68         uint8_t getByteOrNull(char *buffer,int byte,size_t bufferSize);
    69         uint32_t charBufferToUint32(char *buffer, size_t bufferSize);
    70         core::TcpSocket *listeningSocket;
    71         int listeningPort;
    72         core::TcpSocket *controlSocket=NULL;
    73         core::Socket *dataSocket;
    74         std::string *axisName=NULL;
    75         std::string *buttonName=NULL;
    76         size_t dataFrameSize;
    77         char *dataFrameBuffer;
    78         char *receiveFrameBuffer;
    79         size_t receiveCurrentPosition;
    80         uint8_t buttonOffset;
    81     };
    82 
    83 }}
     72private:
     73  uint16_t readBits(uint8_t offsetInBits, uint8_t valueSizeInBits, char *buffer,
     74                    size_t bufferSize);
     75  uint8_t getByteOrNull(char *buffer, int byte, size_t bufferSize);
     76  uint32_t charBufferToUint32(char *buffer, size_t bufferSize);
     77  core::TcpSocket *listeningSocket;
     78  int listeningPort;
     79  core::TcpSocket *controlSocket = NULL;
     80  core::Socket *dataSocket;
     81  std::string *axisName = NULL;
     82  std::string *buttonName = NULL;
     83  size_t dataFrameSize;
     84  char *dataFrameBuffer;
     85  char *receiveFrameBuffer;
     86  size_t receiveCurrentPosition;
     87  uint8_t buttonOffset;
     88};
     89}
     90}
    8491
    8592#endif // TARGETCONTROLLER_H

  • trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp

    r3 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair
    32 {
    33 namespace sensor
    34 {
     31namespace flair {
     32namespace sensor {
    3533
    36 UsRangeFinder::UsRangeFinder(const FrameworkManager* parent,string name) : IODevice(parent,name)
    37 {
    38     plot_tab=NULL;
     34UsRangeFinder::UsRangeFinder(const FrameworkManager *parent, string name)
     35    : IODevice(parent, name) {
     36  plot_tab = NULL;
    3937
    40     cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
    41     desc->SetElementName(0,0,name);
    42     output=new cvmatrix(this,desc,floatType);
    43     AddDataToLog(output);
     38  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     39  desc->SetElementName(0, 0, name);
     40  output = new cvmatrix(this, desc, floatType);
     41  AddDataToLog(output);
    4442
    45     //station sol
    46     main_tab=new Tab(parent->GetTabWidget(),name);
    47     tab=new TabWidget(main_tab->NewRow(),name);
    48         sensor_tab=new Tab(tab,"Reglages");
    49         setup_groupbox=new GroupBox(sensor_tab->NewRow(),name);
     43  // station sol
     44  main_tab = new Tab(parent->GetTabWidget(), name);
     45  tab = new TabWidget(main_tab->NewRow(), name);
     46  sensor_tab = new Tab(tab, "Reglages");
     47  setup_groupbox = new GroupBox(sensor_tab->NewRow(), name);
    5048}
    5149
    52 UsRangeFinder::UsRangeFinder(const IODevice* parent,std::string name) : IODevice(parent,name)
    53 {
    54     plot_tab=NULL;
    55     main_tab=NULL;
    56     tab=NULL;
    57         sensor_tab=NULL;
    58         setup_groupbox=NULL;
     50UsRangeFinder::UsRangeFinder(const IODevice *parent, std::string name)
     51    : IODevice(parent, name) {
     52  plot_tab = NULL;
     53  main_tab = NULL;
     54  tab = NULL;
     55  sensor_tab = NULL;
     56  setup_groupbox = NULL;
    5957
    60     output=NULL;
     58  output = NULL;
    6159}
    6260
    63 UsRangeFinder::~UsRangeFinder()
    64 {
    65     if(main_tab!=NULL) delete main_tab;
     61UsRangeFinder::~UsRangeFinder() {
     62  if (main_tab != NULL)
     63    delete main_tab;
    6664}
    6765
    68 GroupBox* UsRangeFinder::GetGroupBox(void) const
    69 {
    70     return setup_groupbox;
     66GroupBox *UsRangeFinder::GetGroupBox(void) const { return setup_groupbox; }
     67
     68Layout *UsRangeFinder::GetLayout(void) const { return sensor_tab; }
     69
     70DataPlot1D *UsRangeFinder::GetPlot(void) const { return plot; }
     71
     72Tab *UsRangeFinder::GetPlotTab(void) const { return plot_tab; }
     73
     74void UsRangeFinder::UseDefaultPlot(void) {
     75  if (tab == NULL) {
     76    Err("not applicable for simulation part.\n");
     77    return;
     78  }
     79
     80  plot_tab = new Tab(tab, "Mesures");
     81  plot = new DataPlot1D(plot_tab->NewRow(), ObjectName(), 0, 2);
     82  plot->AddCurve(output->Element(0));
    7183}
    7284
    73 Layout* UsRangeFinder::GetLayout(void) const
    74 {
    75     return sensor_tab;
     85void UsRangeFinder::LockUserInterface(void) const {
     86  setup_groupbox->setEnabled(false);
    7687}
    7788
    78 DataPlot1D* UsRangeFinder::GetPlot(void) const
    79 {
    80     return plot;
     89void UsRangeFinder::UnlockUserInterface(void) const {
     90  setup_groupbox->setEnabled(true);
    8191}
    8292
    83 Tab* UsRangeFinder::GetPlotTab(void) const
    84 {
    85     return plot_tab;
    86 }
    87 
    88 void UsRangeFinder::UseDefaultPlot(void)
    89 {
    90     if(tab==NULL)
    91     {
    92         Err("not applicable for simulation part.\n");
    93         return;
    94     }
    95 
    96     plot_tab=new Tab(tab,"Mesures");
    97         plot=new DataPlot1D(plot_tab->NewRow(),ObjectName(),0,2);
    98             plot->AddCurve(output->Element(0));
    99 }
    100 
    101 void UsRangeFinder::LockUserInterface(void) const
    102 {
    103     setup_groupbox->setEnabled(false);
    104 }
    105 
    106 void UsRangeFinder::UnlockUserInterface(void) const
    107 {
    108     setup_groupbox->setEnabled(true);
    109 }
    110 
    111 float UsRangeFinder::Value(void) const
    112 {
    113     return output->Value(0,0);
    114 }
     93float UsRangeFinder::Value(void) const { return output->Value(0, 0); }
    11594
    11695} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/UsRangeFinder.h

    r3 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23     }
    24     namespace gui
    25     {
    26         class Tab;
    27         class TabWidget;
    28         class GroupBox;
    29         class Layout;
    30         class DataPlot1D;
    31     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class Tab;
     24class TabWidget;
     25class GroupBox;
     26class Layout;
     27class DataPlot1D;
     28}
    3229}
    3330
    34 namespace flair
    35 {
    36 namespace sensor
    37 {
    38     /*! \class UsRangeFinder
    39     *
    40     * \brief Base class for UsRangeFinder
    41     *
    42     * Use this class to define a custom UsRangeFinder.
    43     *
    44     */
    45     class UsRangeFinder : public core::IODevice
    46     {
    47         public:
    48             /*!
    49             * \brief Constructor
    50             *
    51             * Construct a UsRangeFinder. Control part.
    52             *
    53             * \param parent parent
    54             * \param name name
    55             */
    56             UsRangeFinder(const core::FrameworkManager* parent,std::string name);
     31namespace flair {
     32namespace sensor {
     33/*! \class UsRangeFinder
     34*
     35* \brief Base class for UsRangeFinder
     36*
     37* Use this class to define a custom UsRangeFinder.
     38*
     39*/
     40class UsRangeFinder : public core::IODevice {
     41public:
     42  /*!
     43  * \brief Constructor
     44  *
     45  * Construct a UsRangeFinder. Control part.
     46  *
     47  * \param parent parent
     48  * \param name name
     49  */
     50  UsRangeFinder(const core::FrameworkManager *parent, std::string name);
    5751
    58             /*!
    59             * \brief Constructor
    60             *
    61             * Construct a UsRangeFinder. Simulation part.
    62             *
    63             * \param parent parent
    64             * \param name name
    65             */
    66             UsRangeFinder(const core::IODevice* parent,std::string name);
     52  /*!
     53  * \brief Constructor
     54  *
     55  * Construct a UsRangeFinder. Simulation part.
     56  *
     57  * \param parent parent
     58  * \param name name
     59  */
     60  UsRangeFinder(const core::IODevice *parent, std::string name);
    6761
    68             /*!
    69             * \brief Destructor
    70             *
    71             */
    72             ~UsRangeFinder();
     62  /*!
     63  * \brief Destructor
     64  *
     65  */
     66  ~UsRangeFinder();
    7367
    74             /*!
    75             * \brief Lock user interface
    76             *
    77             */
    78             void LockUserInterface(void) const;
     68  /*!
     69  * \brief Lock user interface
     70  *
     71  */
     72  void LockUserInterface(void) const;
    7973
    80             /*!
    81             * \brief Unlock user interface
    82             *
    83             */
    84             void UnlockUserInterface(void) const;
     74  /*!
     75  * \brief Unlock user interface
     76  *
     77  */
     78  void UnlockUserInterface(void) const;
    8579
    86             /*!
    87             * \brief Use default plot
    88             *
    89             * Should no be used for the simulation part.
    90             */
    91             void UseDefaultPlot(void);
     80  /*!
     81  * \brief Use default plot
     82  *
     83  * Should no be used for the simulation part.
     84  */
     85  void UseDefaultPlot(void);
    9286
    93             /*!
    94             * \brief Plot
    95             *
    96             * \return DataPlot1D
    97             */
    98             gui::DataPlot1D* GetPlot(void) const;
     87  /*!
     88  * \brief Plot
     89  *
     90  * \return DataPlot1D
     91  */
     92  gui::DataPlot1D *GetPlot(void) const;
    9993
    100             /*!
    101             * \brief Setup Layout
    102             *
    103             * \return a Layout available
    104             */
    105             gui::Layout* GetLayout(void) const;
     94  /*!
     95  * \brief Setup Layout
     96  *
     97  * \return a Layout available
     98  */
     99  gui::Layout *GetLayout(void) const;
    106100
    107             /*!
    108             * \brief Plot tab
    109             *
    110             * \return plot Tab
    111             */
    112             gui::Tab* GetPlotTab(void) const;
     101  /*!
     102  * \brief Plot tab
     103  *
     104  * \return plot Tab
     105  */
     106  gui::Tab *GetPlotTab(void) const;
    113107
    114             /*!
    115             * \brief Value
    116             *
    117             * \return output value
    118             */
    119             float Value(void) const;
     108  /*!
     109  * \brief Value
     110  *
     111  * \return output value
     112  */
     113  float Value(void) const;
    120114
    121         protected:
    122             /*!
    123             * \brief Output matrix
    124             *
    125             * \return output matrix
    126             */
    127             core::cvmatrix *output;
     115protected:
     116  /*!
     117  * \brief Output matrix
     118  *
     119  * \return output matrix
     120  */
     121  core::cvmatrix *output;
    128122
    129             /*!
    130             * \brief Setup GroupBox
    131             *
    132             * \return a GroupBox available
    133             */
    134             gui::GroupBox* GetGroupBox(void) const;
     123  /*!
     124  * \brief Setup GroupBox
     125  *
     126  * \return a GroupBox available
     127  */
     128  gui::GroupBox *GetGroupBox(void) const;
    135129
    136         private:
    137             /*!
    138             * \brief Update using provided datas
    139             *
    140             * Reimplemented from IODevice.
    141             *
    142             * \param data data from the parent to process
    143             */
    144             void UpdateFrom(const core::io_data *data){};
     130private:
     131  /*!
     132  * \brief Update using provided datas
     133  *
     134  * Reimplemented from IODevice.
     135  *
     136  * \param data data from the parent to process
     137  */
     138  void UpdateFrom(const core::io_data *data){};
    145139
    146             gui::Tab* main_tab;
    147             gui::TabWidget *tab;
    148             gui::GroupBox* setup_groupbox;
    149             gui::Tab* sensor_tab;
    150             gui::DataPlot1D* plot;
    151             gui::Tab* plot_tab;
    152     };
     140  gui::Tab *main_tab;
     141  gui::TabWidget *tab;
     142  gui::GroupBox *setup_groupbox;
     143  gui::Tab *sensor_tab;
     144  gui::DataPlot1D *plot;
     145  gui::Tab *plot_tab;
     146};
    153147} // end namespace sensor
    154148} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/V4LCamera.cpp

    r3 r15  
    3030using namespace flair::gui;
    3131
    32 namespace flair
    33 {
    34 namespace sensor
    35 {
     32namespace flair {
     33namespace sensor {
    3634
    37 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)
    38 {
    39     capture = cvCaptureFromCAM(camera_index);//a mettre apres l'init dsp
     35V4LCamera::V4LCamera(const FrameworkManager *parent, string name,
     36                     uint8_t camera_index, uint16_t width, uint16_t height,
     37                     cvimage::Type::Format format, uint8_t priority)
     38    : Thread(parent, name, priority),
     39      Camera(parent, name, width, height, format) {
     40  capture = cvCaptureFromCAM(camera_index); // a mettre apres l'init dsp
    4041
    41     if(capture<0) Thread::Err("cvCaptureFromCAM error\n");
     42  if (capture < 0)
     43    Thread::Err("cvCaptureFromCAM error\n");
    4244
    43     if(cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,width)<0) Thread::Err("cvSetCaptureProperty error\n");
    44     if(cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,height)<0) Thread::Err("cvSetCaptureProperty error\n");
     45  if (cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, width) < 0)
     46    Thread::Err("cvSetCaptureProperty error\n");
     47  if (cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, height) < 0)
     48    Thread::Err("cvSetCaptureProperty error\n");
    4549
    46     if(format==cvimage::Type::Format::UYVY)
    47     {
    48         if(cvSetCaptureProperty(capture,CV_CAP_PROP_FORMAT,V4L2_PIX_FMT_UYVY)<0) Thread::Err("cvSetCaptureProperty error\n");
    49     }
    50     else if(format==cvimage::Type::Format::YUYV)
    51     {
    52         if(cvSetCaptureProperty(capture,CV_CAP_PROP_FORMAT,V4L2_PIX_FMT_YUYV)<0) Thread::Err("cvSetCaptureProperty error\n");
    53     }
    54     else
    55     {
    56         Thread::Err("format not supported\n");
     50  if (format == cvimage::Type::Format::UYVY) {
     51    if (cvSetCaptureProperty(capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_UYVY) <
     52        0)
     53      Thread::Err("cvSetCaptureProperty error\n");
     54  } else if (format == cvimage::Type::Format::YUYV) {
     55    if (cvSetCaptureProperty(capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_YUYV) <
     56        0)
     57      Thread::Err("cvSetCaptureProperty error\n");
     58  } else {
     59    Thread::Err("format not supported\n");
     60  }
     61
     62  // station sol
     63  gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
     64  exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,
     65                               1, 0.1);
     66  bright =
     67      new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
     68  contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,
     69                               1, 0.1);
     70  hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
     71  sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:",
     72                                0, 1, 0.1);
     73  sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,
     74                          0.1);
     75  autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
     76  autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
     77  awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
     78  fps = new Label(GetGroupBox()->NewRow(), "fps");
     79}
     80
     81V4LCamera::~V4LCamera() {
     82  SafeStop();
     83  Join();
     84}
     85
     86void V4LCamera::Run(void) {
     87  Time cam_time, new_time, fpsNow, fpsPrev;
     88  IplImage *img; // raw image
     89  int fpsCounter = 0;
     90
     91  // init image old
     92  if (!cvGrabFrame(capture)) {
     93    Printf("Could not grab a frame\n");
     94  }
     95  cam_time = GetTime();
     96  fpsPrev = cam_time;
     97
     98  while (!ToBeStopped()) {
     99    // fps counter
     100    fpsCounter++;
     101    if (fpsCounter == 100) {
     102      fpsNow = GetTime();
     103      fps->SetText("fps: %.1f",
     104                   fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
     105      fpsCounter = 0;
     106      fpsPrev = fpsNow;
    57107    }
    58108
    59     //station sol
    60     gain=new DoubleSpinBox(GetGroupBox()->NewRow(),"gain:",0,1,0.1);
    61     exposure=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"exposure:",0,1,0.1);
    62     bright=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"bright:",0,1,0.1);
    63     contrast=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"contrast:",0,1,0.1);
    64     hue=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"hue:",0,1,0.1);
    65     sharpness=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"sharpness:",0,1,0.1);
    66     sat=new DoubleSpinBox(GetGroupBox()->LastRowLastCol(),"saturation:",0,1,0.1);
    67     autogain=new CheckBox(GetGroupBox()->NewRow(),"autogain:");
    68     autoexposure=new CheckBox(GetGroupBox()->LastRowLastCol(),"autoexposure:");
    69     awb=new CheckBox(GetGroupBox()->LastRowLastCol(),"awb:");
    70     fps=new Label(GetGroupBox()->NewRow(),"fps");
     109    // cam properties
     110    if (gain->ValueChanged() == true && autogain->Value() == false)
     111      SetGain(gain->Value());
     112    if (exposure->ValueChanged() == true && autoexposure->Value() == false)
     113      SetExposure(exposure->Value());
     114    if (bright->ValueChanged() == true)
     115      SetBrightness(bright->Value());
     116    if (sat->ValueChanged() == true)
     117      SetSaturation(sat->Value());
     118    if (contrast->ValueChanged() == true)
     119      SetContrast(contrast->Value());
     120    if (hue->ValueChanged() == true)
     121      SetHue(hue->Value());
     122    if (sharpness->ValueChanged() == true)
     123      cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value());
     124    if (autogain->ValueChanged() == true) {
     125      if (autogain->Value() == true) {
     126        gain->setEnabled(false);
     127      } else {
     128        gain->setEnabled(true);
     129        SetGain(gain->Value());
     130      }
     131      SetAutoGain(autogain->Value());
     132    }
     133    if (autoexposure->ValueChanged() == true) {
     134      if (autoexposure->Value() == true) {
     135        exposure->setEnabled(false);
     136      } else {
     137        exposure->setEnabled(true);
     138        SetExposure(exposure->Value());
     139      }
     140      SetAutoExposure(autoexposure->Value());
     141    }
     142    if (awb->ValueChanged() == true)
     143      cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value());
     144
     145    // cam pictures
     146    img = cvRetrieveRawFrame(capture);
     147    if (!cvGrabFrame(capture)) {
     148      Printf("Could not grab a frame\n");
     149    }
     150    new_time = GetTime();
     151
     152    output->GetMutex();
     153    output->img->imageData = img->imageData;
     154    output->ReleaseMutex();
     155
     156    output->SetDataTime(cam_time);
     157    ProcessUpdate(output);
     158
     159    cam_time = new_time;
     160  }
     161
     162  cvReleaseCapture(&capture);
    71163}
    72164
    73 V4LCamera::~V4LCamera()
    74 {
    75     SafeStop();
    76     Join();
     165void V4LCamera::SetAutoGain(bool value) {
     166  cvSetCaptureProperty(capture, CV_CAP_PROP_AUTOGAIN, value);
    77167}
    78168
    79 void V4LCamera::Run(void)
    80 {
    81     Time cam_time,new_time,fpsNow,fpsPrev;
    82     IplImage* img;//raw image
    83     int fpsCounter=0;
    84 
    85     //init image old
    86     if(!cvGrabFrame(capture))
    87     {
    88         Printf("Could not grab a frame\n");
    89     }
    90     cam_time=GetTime();
    91     fpsPrev = cam_time;
    92 
    93     while(!ToBeStopped())
    94     {
    95         //fps counter
    96         fpsCounter++;
    97         if(fpsCounter==100) {
    98             fpsNow = GetTime();
    99             fps->SetText("fps: %.1f",fpsCounter/((float)(fpsNow - fpsPrev)/1000000000.));
    100             fpsCounter=0;
    101             fpsPrev=fpsNow;
    102         }
    103 
    104         //cam properties
    105         if(gain->ValueChanged()==true && autogain->Value()==false) SetGain(gain->Value());
    106         if(exposure->ValueChanged()==true && autoexposure->Value()==false) SetExposure(exposure->Value());
    107         if(bright->ValueChanged()==true) SetBrightness(bright->Value());
    108         if(sat->ValueChanged()==true) SetSaturation(sat->Value());
    109         if(contrast->ValueChanged()==true) SetContrast(contrast->Value());
    110         if(hue->ValueChanged()==true) SetHue(hue->Value());
    111         if(sharpness->ValueChanged()==true) cvSetCaptureProperty(capture,CV_CAP_PROP_SHARPNESS,sharpness->Value());
    112         if(autogain->ValueChanged()==true)
    113         {
    114             if(autogain->Value()==true)
    115             {
    116                 gain->setEnabled(false);
    117             }
    118             else
    119             {
    120                 gain->setEnabled(true);
    121                 SetGain(gain->Value());
    122             }
    123             SetAutoGain(autogain->Value());
    124         }
    125         if(autoexposure->ValueChanged()==true)
    126         {
    127             if(autoexposure->Value()==true)
    128             {
    129                 exposure->setEnabled(false);
    130             }
    131             else
    132             {
    133                 exposure->setEnabled(true);
    134                 SetExposure(exposure->Value());
    135             }
    136             SetAutoExposure(autoexposure->Value());
    137         }
    138         if(awb->ValueChanged()==true) cvSetCaptureProperty(capture,CV_CAP_PROP_AWB,awb->Value());
    139 
    140 
    141         //cam pictures
    142         img=cvRetrieveRawFrame(capture);
    143         if(!cvGrabFrame(capture))
    144         {
    145             Printf("Could not grab a frame\n");
    146         }
    147         new_time=GetTime();
    148 
    149         output->GetMutex();
    150         output->img->imageData=img->imageData;
    151         output->ReleaseMutex();
    152 
    153         output->SetDataTime(cam_time);
    154         ProcessUpdate(output);
    155 
    156         cam_time=new_time;
    157     }
    158 
    159     cvReleaseCapture(&capture);
     169void V4LCamera::SetAutoExposure(bool value) {
     170  Thread::Warn("not implemented in opencv\n");
    160171}
    161172
    162 void V4LCamera::SetAutoGain(bool value)
    163 {
    164     cvSetCaptureProperty(capture,CV_CAP_PROP_AUTOGAIN,value);
     173void V4LCamera::SetGain(float value) {
     174  cvSetCaptureProperty(capture, CV_CAP_PROP_GAIN, value);
    165175}
    166176
    167 void V4LCamera::SetAutoExposure(bool value)
    168 {
    169     Thread::Warn("not implemented in opencv\n");
     177void V4LCamera::SetExposure(float value) {
     178  cvSetCaptureProperty(capture, CV_CAP_PROP_EXPOSURE, value);
    170179}
    171180
    172 void V4LCamera::SetGain(float value)
    173 {
    174     cvSetCaptureProperty(capture,CV_CAP_PROP_GAIN,value);
     181void V4LCamera::SetBrightness(float value) {
     182  cvSetCaptureProperty(capture, CV_CAP_PROP_BRIGHTNESS, value);
    175183}
    176184
    177 void V4LCamera::SetExposure(float value)
    178 {
    179     cvSetCaptureProperty(capture,CV_CAP_PROP_EXPOSURE,value);
     185void V4LCamera::SetSaturation(float value) {
     186  cvSetCaptureProperty(capture, CV_CAP_PROP_SATURATION, value);
    180187}
    181188
    182 void V4LCamera::SetBrightness(float value)
    183 {
    184     cvSetCaptureProperty(capture,CV_CAP_PROP_BRIGHTNESS,value);
     189void V4LCamera::SetHue(float value) {
     190  cvSetCaptureProperty(capture, CV_CAP_PROP_HUE, value);
    185191}
    186192
    187 void V4LCamera::SetSaturation(float value)
    188 {
    189     cvSetCaptureProperty(capture,CV_CAP_PROP_SATURATION,value);
    190 }
    191 
    192 void V4LCamera::SetHue(float value)
    193 {
    194     cvSetCaptureProperty(capture,CV_CAP_PROP_HUE,value);
    195 }
    196 
    197 void V4LCamera::SetContrast(float value)
    198 {
    199     cvSetCaptureProperty(capture,CV_CAP_PROP_CONTRAST,value);
     193void V4LCamera::SetContrast(float value) {
     194  cvSetCaptureProperty(capture, CV_CAP_PROP_CONTRAST, value);
    200195}
    201196

  • trunk/lib/FlairSensorActuator/src/V4LCamera.h

    r3 r15  
    1818#include <highgui.h>
    1919
    20 namespace flair
    21 {
    22     namespace core
    23     {
    24         class cvimage;
    25         class FrameworkManager;
    26     }
    27     namespace gui
    28     {
    29         class GridLayout;
    30         class DoubleSpinBox;
    31         class CheckBox;
    32         class Label;
    33     }
     20namespace flair {
     21namespace core {
     22class cvimage;
     23class FrameworkManager;
     24}
     25namespace gui {
     26class GridLayout;
     27class DoubleSpinBox;
     28class CheckBox;
     29class Label;
     30}
    3431}
    3532
    36 namespace flair
    37 {
    38 namespace sensor
    39 {
    40     /*! \class V4LCamera
    41     *
    42     * \brief Base class for V4l camera
    43     */
    44     class V4LCamera : public core::Thread, public Camera
    45     {
    46         public:
    47             /*!
    48             * \brief Constructor
    49             *
    50             * Construct a Camera.
    51             *
    52             * \param parent parent
    53             * \param name name
    54             * \param camera_index camera index
    55             * \param width width
    56             * \param height height
    57             * \param format image format
    58             * \param priority priority of the Thread
    59             */
    60             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);
     33namespace flair {
     34namespace sensor {
     35/*! \class V4LCamera
     36*
     37* \brief Base class for V4l camera
     38*/
     39class V4LCamera : public core::Thread, public Camera {
     40public:
     41  /*!
     42  * \brief Constructor
     43  *
     44  * Construct a Camera.
     45  *
     46  * \param parent parent
     47  * \param name name
     48  * \param camera_index camera index
     49  * \param width width
     50  * \param height height
     51  * \param format image format
     52  * \param priority priority of the Thread
     53  */
     54  V4LCamera(const core::FrameworkManager *parent, std::string name,
     55            uint8_t camera_index, uint16_t width, uint16_t height,
     56            core::cvimage::Type::Format format, uint8_t priority);
    6157
    62             /*!
    63             * \brief Destructor
    64             *
    65             */
    66             ~V4LCamera();
     58  /*!
     59  * \brief Destructor
     60  *
     61  */
     62  ~V4LCamera();
    6763
    68         protected:
    69             /*!
    70             * \brief Set Gain
    71             *
    72             * \param value value between 0 and 1
    73             */
    74             virtual void SetGain(float value);
     64protected:
     65  /*!
     66  * \brief Set Gain
     67  *
     68  * \param value value between 0 and 1
     69  */
     70  virtual void SetGain(float value);
    7571
    76             /*!
    77             * \brief Set Auto Gain
    78             *
    79             * \param value value
    80             */
    81             virtual void SetAutoGain(bool value);
     72  /*!
     73  * \brief Set Auto Gain
     74  *
     75  * \param value value
     76  */
     77  virtual void SetAutoGain(bool value);
    8278
    83             /*!
    84             * \brief Set Exposure
    85             *
    86             * \param value value between 0 and 1
    87             */
    88             virtual void SetExposure(float value);
     79  /*!
     80  * \brief Set Exposure
     81  *
     82  * \param value value between 0 and 1
     83  */
     84  virtual void SetExposure(float value);
    8985
    90             /*!
    91             * \brief Set Auto Exposure
    92             *
    93             * \param value value
    94             */
    95             virtual void SetAutoExposure(bool value);
     86  /*!
     87  * \brief Set Auto Exposure
     88  *
     89  * \param value value
     90  */
     91  virtual void SetAutoExposure(bool value);
    9692
    97             /*!
    98             * \brief Set Brightness
    99             *
    100             * \param value value between 0 and 1
    101             */
    102             virtual void SetBrightness(float value);
     93  /*!
     94  * \brief Set Brightness
     95  *
     96  * \param value value between 0 and 1
     97  */
     98  virtual void SetBrightness(float value);
    10399
    104             /*!
    105             * \brief Set Saturation
    106             *
    107             * \param value value between 0 and 1
    108             */
    109             virtual void SetSaturation(float value);
     100  /*!
     101  * \brief Set Saturation
     102  *
     103  * \param value value between 0 and 1
     104  */
     105  virtual void SetSaturation(float value);
    110106
    111             /*!
    112             * \brief Set Hue
    113             *
    114             * \param value value between 0 and 1
    115             */
    116             virtual void SetHue(float value);
     107  /*!
     108  * \brief Set Hue
     109  *
     110  * \param value value between 0 and 1
     111  */
     112  virtual void SetHue(float value);
    117113
    118             /*!
    119             * \brief Set Contrast
    120             *
    121             * \param value value between 0 and 1
    122             */
    123             virtual void SetContrast(float value);
     114  /*!
     115  * \brief Set Contrast
     116  *
     117  * \param value value between 0 and 1
     118  */
     119  virtual void SetContrast(float value);
    124120
    125         private:
    126             /*!
    127             * \brief Update using provided datas
    128             *
    129             * Reimplemented from IODevice.
    130             *
    131             * \param data data from the parent to process
    132             */
    133             void UpdateFrom(const core::io_data *data){};
     121private:
     122  /*!
     123  * \brief Update using provided datas
     124  *
     125  * Reimplemented from IODevice.
     126  *
     127  * \param data data from the parent to process
     128  */
     129  void UpdateFrom(const core::io_data *data){};
    134130
    135             /*!
    136             * \brief Run function
    137             *
    138             * Reimplemented from Thread.
    139             *
    140             */
    141             void Run(void);
     131  /*!
     132  * \brief Run function
     133  *
     134  * Reimplemented from Thread.
     135  *
     136  */
     137  void Run(void);
    142138
    143             CvCapture* capture;
     139  CvCapture *capture;
    144140
    145             gui::Tab* sensor_tab;
    146             gui::DoubleSpinBox *bright,*exposure,*gain,*contrast,*hue,*sharpness,*sat;
    147             gui::CheckBox *autogain,*awb,*autoexposure;
    148             gui::Label *fps;
    149     };
     141  gui::Tab *sensor_tab;
     142  gui::DoubleSpinBox *bright, *exposure, *gain, *contrast, *hue, *sharpness,
     143      *sat;
     144  gui::CheckBox *autogain, *awb, *autoexposure;
     145  gui::Label *fps;
     146};
    150147} // end namespace sensor
    151148} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/VrpnClient.cpp

    r3 r15  
    2626using namespace flair::gui;
    2727
    28 namespace flair
    29 {
    30 namespace sensor
    31 {
     28namespace flair {
     29namespace sensor {
    3230
    33 VrpnClient::VrpnClient(const FrameworkManager* parent,string name,string address,uint16_t us_period,uint8_t priority): Thread(parent,name,priority)
    34 {
    35     pimpl_=new VrpnClient_impl(this,name,address,us_period);
     31VrpnClient::VrpnClient(const FrameworkManager *parent, string name,
     32                       string address, uint16_t us_period, uint8_t priority)
     33    : Thread(parent, name, priority) {
     34  pimpl_ = new VrpnClient_impl(this, name, address, us_period);
    3635}
    3736
    38 VrpnClient::VrpnClient(const FrameworkManager* parent,string name,SerialPort* serialport,uint16_t us_period,uint8_t priority): Thread(parent,name,priority)
    39 {
    40     pimpl_=new VrpnClient_impl(this,name,serialport,us_period);
     37VrpnClient::VrpnClient(const FrameworkManager *parent, string name,
     38                       SerialPort *serialport, uint16_t us_period,
     39                       uint8_t priority)
     40    : Thread(parent, name, priority) {
     41  pimpl_ = new VrpnClient_impl(this, name, serialport, us_period);
    4142}
    4243
    43 VrpnClient::~VrpnClient()
    44 {
    45     SafeStop();
    46     Join();
     44VrpnClient::~VrpnClient() {
     45  SafeStop();
     46  Join();
    4747
    48     delete pimpl_;
     48  delete pimpl_;
    4949}
    5050
    51 Layout* VrpnClient::GetLayout(void) const
    52 {
    53     return (Layout*)(pimpl_->setup_tab);
     51Layout *VrpnClient::GetLayout(void) const {
     52  return (Layout *)(pimpl_->setup_tab);
    5453}
    5554
    56 TabWidget* VrpnClient::GetTabWidget(void) const
    57 {
    58     return pimpl_->tab;
    59 }
     55TabWidget *VrpnClient::GetTabWidget(void) const { return pimpl_->tab; }
    6056
    61 bool VrpnClient::UseXbee(void) const
    62 {
    63     return pimpl_->UseXbee();
    64 }
     57bool VrpnClient::UseXbee(void) const { return pimpl_->UseXbee(); }
    6558
    66 void VrpnClient::Run(void)
    67 {
    68     pimpl_->Run();
    69 }
     59void VrpnClient::Run(void) { pimpl_->Run(); }
    7060
    7161} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/VrpnClient.h

    r3 r15  
    66 * \file VrpnClient.h
    77 * \brief Class to connect to a Vrpn server
    8  * \author César Richard, Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
     8 * \author César Richard, Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS
     9 * 7253
    910 * \date 2013/04/03
    1011 * \version 4.0
    … …  
    1617#include <Thread.h>
    1718
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class FrameworkManager;
    23         class SerialPort;
    24     }
    25     namespace gui
    26     {
    27         class TabWidget;
    28         class Layout;
    29     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class SerialPort;
     23}
     24namespace gui {
     25class TabWidget;
     26class Layout;
     27}
    3028}
    3129
    … …  
    3331class VrpnObject_impl;
    3432
    35 namespace flair
    36 {
    37 namespace sensor
    38 {
    39     /*! \class VrpnClient
    40     *
    41     * \brief Class to connect to a Vrpn server
    42     */
    43     class VrpnClient:public core::Thread
    44     {
    45         friend class ::VrpnObject_impl;
     33namespace flair {
     34namespace sensor {
     35/*! \class VrpnClient
     36*
     37* \brief Class to connect to a Vrpn server
     38*/
     39class VrpnClient : public core::Thread {
     40  friend class ::VrpnObject_impl;
    4641
    47         public:
    48             /*!
    49             * \brief Constructor
    50             *
    51             * Construct a VrpnClient. Connection is done by IP.
    52             *
    53             * \param parent parent
    54             * \param name name
    55             * \param address server address
    56             * \param us_period Thread period in us
    57             * \param priority priority of the Thread
    58             */
    59             VrpnClient(const core::FrameworkManager* parent,std::string name,std::string address,uint16_t us_period,uint8_t priority);
     42public:
     43  /*!
     44  * \brief Constructor
     45  *
     46  * Construct a VrpnClient. Connection is done by IP.
     47  *
     48  * \param parent parent
     49  * \param name name
     50  * \param address server address
     51  * \param us_period Thread period in us
     52  * \param priority priority of the Thread
     53  */
     54  VrpnClient(const core::FrameworkManager *parent, std::string name,
     55             std::string address, uint16_t us_period, uint8_t priority);
    6056
    61             /*!
    62             * \brief Constructor
    63             *
    64             * Construct a VrpnClient. Connection is done by XBee modem.
    65             *
    66             * \param parent parent
    67             * \param name name
    68             * \param serialport SerialPort for XBee modem
    69             * \param us_period Xbee RX timeout in us
    70             * \param priority priority of the Thread
    71             */
    72             VrpnClient(const core::FrameworkManager* parent,std::string name,core::SerialPort* serialport,uint16_t us_period,uint8_t priority);
     57  /*!
     58  * \brief Constructor
     59  *
     60  * Construct a VrpnClient. Connection is done by XBee modem.
     61  *
     62  * \param parent parent
     63  * \param name name
     64  * \param serialport SerialPort for XBee modem
     65  * \param us_period Xbee RX timeout in us
     66  * \param priority priority of the Thread
     67  */
     68  VrpnClient(const core::FrameworkManager *parent, std::string name,
     69             core::SerialPort *serialport, uint16_t us_period,
     70             uint8_t priority);
    7371
    74             /*!
    75             * \brief Destructor
    76             *
    77             */
    78             ~VrpnClient();
     72  /*!
     73  * \brief Destructor
     74  *
     75  */
     76  ~VrpnClient();
    7977
    80             /*!
    81             * \brief Setup Layout
    82             *
    83             * \return a Layout available
    84             */
    85             gui::Layout* GetLayout(void) const;
     78  /*!
     79  * \brief Setup Layout
     80  *
     81  * \return a Layout available
     82  */
     83  gui::Layout *GetLayout(void) const;
    8684
    87             /*!
    88             * \brief Setup Tab
    89             *
    90             * \return a Tab available
    91             */
    92             gui::TabWidget* GetTabWidget(void) const;
     85  /*!
     86  * \brief Setup Tab
     87  *
     88  * \return a Tab available
     89  */
     90  gui::TabWidget *GetTabWidget(void) const;
    9391
    94             /*!
    95             * \brief Is XBee used?
    96             *
    97             * \return true if connection is based on XBee modem
    98             */
    99             bool UseXbee(void) const;
     92  /*!
     93  * \brief Is XBee used?
     94  *
     95  * \return true if connection is based on XBee modem
     96  */
     97  bool UseXbee(void) const;
    10098
    101         private:
    102             /*!
    103             * \brief Run function
    104             *
    105             * Reimplemented from Thread.
    106             *
    107             */
    108             void Run(void);
     99private:
     100  /*!
     101  * \brief Run function
     102  *
     103  * Reimplemented from Thread.
     104  *
     105  */
     106  void Run(void);
    109107
    110             class VrpnClient_impl* pimpl_;
    111     };
     108  class VrpnClient_impl *pimpl_;
     109};
    112110} // end namespace sensor
    113111} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

    r3 r15  
    4141using namespace flair::sensor;
    4242
    43 VrpnClient_impl::VrpnClient_impl(VrpnClient* self,std::string name,std::string address,uint16_t us_period)
    44 {
    45     this->us_period=us_period;
    46     this->self=self;
    47     serialport=NULL;
    48 
    49     mutex=new Mutex(self,name);
    50 
    51     connection = vrpn_get_connection_by_name(address.c_str());
    52 
    53     //station sol
    54     main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
    55     tab=new TabWidget(main_tab->NewRow(),name);
    56     setup_tab=new Tab(tab,"Reglages");
    57 
    58     rotation_1=new OneAxisRotation(setup_tab->NewRow(),"post rotation 1");
    59     rotation_2=new OneAxisRotation(setup_tab->NewRow(),"post rotation 2");
    60 }
    61 
    62 VrpnClient_impl::VrpnClient_impl(VrpnClient* self,std::string name,SerialPort* serialport,uint16_t us_period)
    63 {
    64     this->us_period=us_period;
    65     this->self=self;
    66     this->serialport=serialport;
    67     connection=NULL;
    68     mutex=new Mutex(self,name);
    69 
    70     serialport->SetBaudrate(111111);
    71     serialport->SetRxTimeout(us_period*1000);
    72 
    73     //station sol
    74     main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
    75     tab=new TabWidget(main_tab->NewRow(),name);
    76     setup_tab=new Tab(tab,"Reglages");
    77 
    78     rotation_1=new OneAxisRotation(setup_tab->NewRow(),"post rotation 1");
    79     rotation_2=new OneAxisRotation(setup_tab->NewRow(),"post rotation 2");
    80 }
    81 
    82 VrpnClient_impl::~VrpnClient_impl()
    83 {
    84     if(!UseXbee())
    85     {
    86         //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
    87         vector<VrpnObject*> trackables_copy=trackables;
    88         for(unsigned int i=0;i<trackables_copy.size();i++) delete trackables_copy.at(i);
    89         //trackables.clear();
     43VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
     44                                 std::string address, uint16_t us_period) {
     45  this->us_period = us_period;
     46  this->self = self;
     47  serialport = NULL;
     48
     49  mutex = new Mutex(self, name);
     50
     51  connection = vrpn_get_connection_by_name(address.c_str());
     52
     53  // station sol
     54  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
     55  tab = new TabWidget(main_tab->NewRow(), name);
     56  setup_tab = new Tab(tab, "Reglages");
     57
     58  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1");
     59  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2");
     60}
     61
     62VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
     63                                 SerialPort *serialport, uint16_t us_period) {
     64  this->us_period = us_period;
     65  this->self = self;
     66  this->serialport = serialport;
     67  connection = NULL;
     68  mutex = new Mutex(self, name);
     69
     70  serialport->SetBaudrate(111111);
     71  serialport->SetRxTimeout(us_period * 1000);
     72
     73  // station sol
     74  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
     75  tab = new TabWidget(main_tab->NewRow(), name);
     76  setup_tab = new Tab(tab, "Reglages");
     77
     78  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1");
     79  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2");
     80}
     81
     82VrpnClient_impl::~VrpnClient_impl() {
     83  if (!UseXbee()) {
     84    // on fait une copie car le delete touche a xbee_objects_copy via
     85    // RemoveTrackable
     86    vector<VrpnObject *> trackables_copy = trackables;
     87    for (unsigned int i = 0; i < trackables_copy.size(); i++)
     88      delete trackables_copy.at(i);
     89    // trackables.clear();
     90  } else {
     91    // on fait une copie car le delete touche a xbee_objects_copy via
     92    // RemoveTrackable
     93    vector<xbee_object> xbee_objects_copy = xbee_objects;
     94    for (unsigned int i = 0; i < xbee_objects_copy.size(); i++)
     95      delete xbee_objects_copy.at(i).vrpnobject->self;
     96  }
     97
     98  delete main_tab;
     99
     100  if (!UseXbee()) {
     101    // it will automatically delete connection
     102    connection->removeReference();
     103  }
     104}
     105
     106void VrpnClient_impl::ComputeRotations(Vector3D &point) {
     107  rotation_1->ComputeRotation(point);
     108  rotation_2->ComputeRotation(point);
     109}
     110
     111void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
     112  rotation_1->ComputeRotation(quat);
     113  rotation_2->ComputeRotation(quat);
     114}
     115
     116void VrpnClient_impl::AddTrackable(VrpnObject *obj) {
     117  mutex->GetMutex();
     118  trackables.push_back(obj);
     119  mutex->ReleaseMutex();
     120}
     121
     122void VrpnClient_impl::RemoveTrackable(VrpnObject *obj) {
     123  mutex->GetMutex();
     124  for (vector<VrpnObject *>::iterator it = trackables.begin();
     125       it < trackables.end(); it++) {
     126    if (*it == obj) {
     127      trackables.erase(it);
     128      break;
    90129    }
    91     else
    92     {
    93         //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
    94         vector<xbee_object> xbee_objects_copy=xbee_objects;
    95         for(unsigned int i=0;i<xbee_objects_copy.size();i++) delete xbee_objects_copy.at(i).vrpnobject->self;
     130  }
     131  mutex->ReleaseMutex();
     132}
     133
     134void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
     135  xbee_object tmp;
     136  tmp.vrpnobject = obj;
     137  tmp.id = id;
     138  mutex->GetMutex();
     139  xbee_objects.push_back(tmp);
     140  mutex->ReleaseMutex();
     141}
     142
     143void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
     144  mutex->GetMutex();
     145  for (vector<xbee_object>::iterator it = xbee_objects.begin();
     146       it < xbee_objects.end(); it++) {
     147    if ((*it).vrpnobject == obj) {
     148      xbee_objects.erase(it);
     149      break;
    96150    }
    97 
    98     delete main_tab;
    99 
    100     if(!UseXbee())
    101     {
    102         //it will automatically delete connection
    103         connection->removeReference();
     151  }
     152  mutex->ReleaseMutex();
     153}
     154
     155bool VrpnClient_impl::UseXbee(void) {
     156  if (connection == NULL) {
     157    return true;
     158  } else {
     159    return false;
     160  }
     161}
     162
     163void VrpnClient_impl::Run(void) {
     164  while (!self->ToBeStopped()) {
     165    if (UseXbee()) {
     166      ssize_t read = 0;
     167      uint8_t response[38] = {0};
     168
     169      read = serialport->Read(response, sizeof(response));
     170      if (read > 0 && read != sizeof(response))
     171        read += serialport->Read(&response[read], sizeof(response) - read);
     172      // int temps=(float)self->GetTime()/(1000*1000);
     173      // self->Printf("%i %i %i\n",temps-last,temps,last);
     174      // last=temps;
     175      if (read < 0) {
     176        // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
     177      } else if (read != sizeof(response)) {
     178        self->Warn("erreur rt_dev_read %i/%i\n", read, sizeof(response));
     179      } else {
     180        // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
     181        // printf("\n");
     182        uint8_t checksum = 0;
     183        for (ssize_t i = 3; i < read; i++)
     184          checksum += response[i];
     185        if (checksum != 255) {
     186          self->Err("checksum error\n");
     187        } else {
     188          vrpn_TRACKERCB t;
     189          float pos[3];
     190          float quat[4];
     191          uint8_t id = response[8];
     192
     193          mutex->GetMutex();
     194          if (id < xbee_objects.size()) {
     195            memcpy(pos, &response[9], sizeof(pos));
     196            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
     197            for (int i = 0; i < 3; i++)
     198              t.pos[i] = pos[i];
     199            for (int i = 0; i < 4; i++)
     200              t.quat[i] = quat[i];
     201            if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
     202              printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
     203            } else {
     204              // self->Printf("%i %f %f %f
     205              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
     206              VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
     207            }
     208          }
     209          mutex->ReleaseMutex();
     210        }
     211      }
     212    } else {
     213      connection->mainloop();
     214      mutex->GetMutex();
     215      for (unsigned int i = 0; i < trackables.size(); i++)
     216        trackables.at(i)->mainloop();
     217      mutex->ReleaseMutex();
     218
     219      self->SleepUS(us_period);
    104220    }
    105 }
    106 
    107 void VrpnClient_impl::ComputeRotations(Vector3D& point)
    108 {
    109     rotation_1->ComputeRotation(point);
    110     rotation_2->ComputeRotation(point);
    111 }
    112 
    113 void VrpnClient_impl::ComputeRotations(Quaternion& quat)
    114 {
    115     rotation_1->ComputeRotation(quat);
    116     rotation_2->ComputeRotation(quat);
    117 }
    118 
    119 void VrpnClient_impl::AddTrackable(VrpnObject* obj)
    120 {
    121     mutex->GetMutex();
    122     trackables.push_back(obj);
    123     mutex->ReleaseMutex();
    124 }
    125 
    126 void VrpnClient_impl::RemoveTrackable(VrpnObject* obj)
    127 {
    128     mutex->GetMutex();
    129     for(vector<VrpnObject*>::iterator it=trackables.begin() ; it < trackables.end(); it++ )
    130     {
    131         if(*it==obj)
    132         {
    133             trackables.erase (it);
    134             break;
    135         }
    136     }
    137     mutex->ReleaseMutex();
    138 }
    139 
    140 void VrpnClient_impl::AddTrackable(VrpnObject_impl* obj,uint8_t id)
    141 {
    142     xbee_object tmp;
    143     tmp.vrpnobject=obj;
    144     tmp.id=id;
    145     mutex->GetMutex();
    146     xbee_objects.push_back(tmp);
    147     mutex->ReleaseMutex();
    148 }
    149 
    150 void VrpnClient_impl::RemoveTrackable(VrpnObject_impl* obj)
    151 {
    152     mutex->GetMutex();
    153     for(vector<xbee_object>::iterator it=xbee_objects.begin() ; it < xbee_objects.end(); it++ )
    154     {
    155         if((*it).vrpnobject==obj)
    156         {
    157             xbee_objects.erase (it);
    158             break;
    159         }
    160     }
    161     mutex->ReleaseMutex();
    162 }
    163 
    164 bool VrpnClient_impl::UseXbee(void)
    165 {
    166     if(connection==NULL)
    167     {
    168         return true;
    169     }
    170     else
    171     {
    172         return false;
    173     }
    174 }
    175 
    176 void VrpnClient_impl::Run(void)
    177 {
    178     while(!self->ToBeStopped())
    179     {
    180         if(UseXbee())
    181         {
    182             ssize_t read = 0;
    183             uint8_t response[38] = {0};
    184 
    185             read = serialport->Read(response,sizeof(response));
    186             if(read>0 && read!=sizeof(response)) read += serialport->Read(&response[read],sizeof(response)-read);
    187             //int temps=(float)self->GetTime()/(1000*1000);
    188 //self->Printf("%i %i %i\n",temps-last,temps,last);
    189 //last=temps;
    190             if(read<0)
    191             {
    192                 //self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
    193             }
    194             else if (read != sizeof(response))
    195             {
    196                 self->Warn("erreur rt_dev_read %i/%i\n",read,sizeof(response));
    197             }
    198             else
    199             {
    200                 //for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
    201                 //printf("\n");
    202                 uint8_t checksum=0;
    203                 for(ssize_t i=3;i<read;i++) checksum+=response[i];
    204                 if(checksum!=255)
    205                 {
    206                     self->Err("checksum error\n");
    207                 }
    208                 else
    209                 {
    210                     vrpn_TRACKERCB t;
    211                     float pos[3];
    212                     float quat[4];
    213                     uint8_t id=response[8];
    214 
    215                     mutex->GetMutex();
    216                     if(id<xbee_objects.size())
    217                     {
    218                         memcpy(pos,&response[9],sizeof(pos));
    219                         memcpy(quat,&response[9]+sizeof(pos),sizeof(quat));
    220                         for(int i=0;i<3;i++) t.pos[i]=pos[i];
    221                         for(int i=0;i<4;i++) t.quat[i]=quat[i];
    222                         if(fabs(pos[0]>10) || fabs(pos[1]>10) || fabs(pos[2]>10))
    223                         {
    224                             printf("prob pos %f %f %f\n",pos[0],pos[1],pos[2]);
    225                         }
    226                         else
    227                         {
    228                             //self->Printf("%i %f %f %f %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
    229                             VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject,t);
    230                         }
    231                     }
    232                     mutex->ReleaseMutex();
    233                 }
    234             }
    235         }
    236         else
    237         {
    238             connection->mainloop();
    239             mutex->GetMutex();
    240             for(unsigned int i=0;i<trackables.size();i++) trackables.at(i)->mainloop();
    241             mutex->ReleaseMutex();
    242 
    243             self->SleepUS(us_period);
    244         }
    245     }
    246 }
     221  }
     222}

  • trunk/lib/FlairSensorActuator/src/VrpnObject.cpp

    r3 r15  
    2727using namespace flair::gui;
    2828
    29 namespace flair
    30 {
    31 namespace sensor
    32 {
     29namespace flair {
     30namespace sensor {
    3331
    34 VrpnObject::VrpnObject(const VrpnClient *parent,string name,const TabWidget* tab): IODevice(parent,name)
    35 {
    36     pimpl_=new VrpnObject_impl(this,parent,name,-1,tab);
    37     AddDataToLog(pimpl_->output);
     32VrpnObject::VrpnObject(const VrpnClient *parent, string name,
     33                       const TabWidget *tab)
     34    : IODevice(parent, name) {
     35  pimpl_ = new VrpnObject_impl(this, parent, name, -1, tab);
     36  AddDataToLog(pimpl_->output);
    3837}
    3938
    40 VrpnObject::VrpnObject(const VrpnClient *parent,string name,uint8_t id,const TabWidget* tab): IODevice(parent,name)
    41 {
    42     Warn("Creation of object %s with id %i\n",name.c_str(),id);
    43     pimpl_=new VrpnObject_impl(this,parent,name,id,tab);
    44     AddDataToLog(pimpl_->output);
     39VrpnObject::VrpnObject(const VrpnClient *parent, string name, uint8_t id,
     40                       const TabWidget *tab)
     41    : IODevice(parent, name) {
     42  Warn("Creation of object %s with id %i\n", name.c_str(), id);
     43  pimpl_ = new VrpnObject_impl(this, parent, name, id, tab);
     44  AddDataToLog(pimpl_->output);
    4545}
    4646
    47 VrpnObject::~VrpnObject(void)
    48 {
    49         delete pimpl_;
     47VrpnObject::~VrpnObject(void) { delete pimpl_; }
     48
     49cvmatrix *VrpnObject::Output(void) const { return pimpl_->output; }
     50
     51cvmatrix *VrpnObject::State(void) const { return pimpl_->state; }
     52
     53Tab *VrpnObject::GetPlotTab(void) const { return pimpl_->plot_tab; }
     54
     55DataPlot1D *VrpnObject::xPlot(void) const { return pimpl_->x_plot; }
     56
     57DataPlot1D *VrpnObject::yPlot(void) const { return pimpl_->y_plot; }
     58
     59DataPlot1D *VrpnObject::zPlot(void) const { return pimpl_->z_plot; }
     60
     61Time VrpnObject::GetLastPacketTime(void) const {
     62  return pimpl_->output->DataTime();
    5063}
    5164
    52 cvmatrix *VrpnObject::Output(void) const
    53 {
    54     return pimpl_->output;
     65bool VrpnObject::IsTracked(unsigned int timeout_ms) const {
     66  return pimpl_->IsTracked(timeout_ms);
    5567}
    5668
    57 cvmatrix *VrpnObject::State(void) const
    58 {
    59     return pimpl_->state;
     69void VrpnObject::GetEuler(Euler &euler) const { pimpl_->GetEuler(euler); }
     70
     71void VrpnObject::GetQuaternion(Quaternion &quaternion) const {
     72  pimpl_->GetQuaternion(quaternion);
    6073}
    6174
    62 Tab* VrpnObject::GetPlotTab(void) const
    63 {
    64     return pimpl_->plot_tab;
     75void VrpnObject::GetPosition(Vector3D &point) const {
     76  pimpl_->GetPosition(point);
    6577}
    6678
    67 DataPlot1D* VrpnObject::xPlot(void) const
    68 {
    69     return pimpl_->x_plot;
    70 }
    71 
    72 DataPlot1D* VrpnObject::yPlot(void) const
    73 {
    74     return pimpl_->y_plot;
    75 }
    76 
    77 DataPlot1D* VrpnObject::zPlot(void) const
    78 {
    79     return pimpl_->z_plot;
    80 }
    81 
    82 Time VrpnObject::GetLastPacketTime(void) const
    83 {
    84         return pimpl_->output->DataTime();
    85 }
    86 
    87 bool VrpnObject::IsTracked(unsigned int timeout_ms) const
    88 {
    89     return pimpl_->IsTracked(timeout_ms);
    90 }
    91 
    92 void VrpnObject::GetEuler(Euler &euler) const
    93 {
    94     pimpl_->GetEuler(euler);
    95 }
    96 
    97 void VrpnObject::GetQuaternion(Quaternion &quaternion) const {
    98     pimpl_->GetQuaternion(quaternion);
    99 }
    100 
    101 void VrpnObject::GetPosition(Vector3D &point) const
    102 {
    103     pimpl_->GetPosition(point);
    104 }
    105 
    106 void VrpnObject::mainloop(void)
    107 {
    108     pimpl_->mainloop();
    109 }
     79void VrpnObject::mainloop(void) { pimpl_->mainloop(); }
    11080
    11181} // end namespace sensor

  • trunk/lib/FlairSensorActuator/src/VrpnObject.h

    r3 r15  
    66 * \file VrpnObject.h
    77 * \brief Class for VRPN object
    8  * \author César Richard, Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
     8 * \author César Richard, Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS
     9 * 7253
    910 * \date 2013/04/03
    1011 * \version 4.0
    … …  
    1718#include <stdint.h>
    1819
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class cvmatrix;
    24         class Vector3D;
    25         class Euler;
    26         class Quaternion;
    27     }
    28     namespace gui
    29     {
    30         class TabWidget;
    31         class Tab;
    32         class DataPlot1D;
    33     }
     20namespace flair {
     21namespace core {
     22class cvmatrix;
     23class Vector3D;
     24class Euler;
     25class Quaternion;
     26}
     27namespace gui {
     28class TabWidget;
     29class Tab;
     30class DataPlot1D;
     31}
    3432}
    3533
    … …  
    3735class VrpnClient_impl;
    3836
    39 namespace flair
    40 {
    41 namespace sensor
    42 {
    43     class VrpnClient;
     37namespace flair {
     38namespace sensor {
     39class VrpnClient;
    4440
    45     /*! \class VrpnObject
    46     *
    47     * \brief Class for VRPN object
    48     */
    49     class VrpnObject: public core::IODevice
    50     {
    51         friend class ::VrpnObject_impl;
    52         friend class ::VrpnClient_impl;
     41/*! \class VrpnObject
     42*
     43* \brief Class for VRPN object
     44*/
     45class VrpnObject : public core::IODevice {
     46  friend class ::VrpnObject_impl;
     47  friend class ::VrpnClient_impl;
    5348
    54         public:
    55             /*!
    56             * \brief Constructor
    57             *
    58             * Construct a VrpnObject. Connection is done by IP.
    59             *
    60             * \param parent parent
    61             * \param name VRPN object name, should be the same as defined in the server
    62             * \param tab Tab for the user interface
    63             */
    64             VrpnObject(const VrpnClient *parent,std::string name,const gui::TabWidget* tab);
     49public:
     50  /*!
     51  * \brief Constructor
     52  *
     53  * Construct a VrpnObject. Connection is done by IP.
     54  *
     55  * \param parent parent
     56  * \param name VRPN object name, should be the same as defined in the server
     57  * \param tab Tab for the user interface
     58  */
     59  VrpnObject(const VrpnClient *parent, std::string name,
     60             const gui::TabWidget *tab);
    6561
    66             /*!
    67             * \brief Constructor
    68             *
    69             * Construct a VrpnObject. Connection is done by IP.
    70             *
    71             * \param parent parent
    72             * \param name name
    73             * \param id VRPN object id, should be the same as defined in the server
    74             * \param tab Tab for the user interface
    75             */
    76             VrpnObject(const VrpnClient *parent,std::string name,uint8_t id,const gui::TabWidget* tab);
     62  /*!
     63  * \brief Constructor
     64  *
     65  * Construct a VrpnObject. Connection is done by IP.
     66  *
     67  * \param parent parent
     68  * \param name name
     69  * \param id VRPN object id, should be the same as defined in the server
     70  * \param tab Tab for the user interface
     71  */
     72  VrpnObject(const VrpnClient *parent, std::string name, uint8_t id,
     73             const gui::TabWidget *tab);
    7774
    78             /*!
    79             * \brief Destructor
    80             *
    81             */
    82             ~VrpnObject(void);
     75  /*!
     76  * \brief Destructor
     77  *
     78  */
     79  ~VrpnObject(void);
    8380
    84             /*!
    85             * \brief Plot tab
    86             *
    87             * \return plot Tab
    88             */
    89             gui::Tab* GetPlotTab(void) const;
     81  /*!
     82  * \brief Plot tab
     83  *
     84  * \return plot Tab
     85  */
     86  gui::Tab *GetPlotTab(void) const;
    9087
    91             /*!
    92             * \brief Get Last Packet Time
    93             *
    94             * \return Time of last received packe
    95             */
    96             core::Time GetLastPacketTime(void) const;
     88  /*!
     89  * \brief Get Last Packet Time
     90  *
     91  * \return Time of last received packe
     92  */
     93  core::Time GetLastPacketTime(void) const;
    9794
    98             /*!
    99             * \brief Is object tracked?
    100             *
    101             * \param timeout_ms timeout
    102             * \return true if object is tracked during timeout_ms time
    103             */
    104             bool IsTracked(unsigned int timeout_ms) const;
     95  /*!
     96  * \brief Is object tracked?
     97  *
     98  * \param timeout_ms timeout
     99  * \return true if object is tracked during timeout_ms time
     100  */
     101  bool IsTracked(unsigned int timeout_ms) const;
    105102
    106             /*!
    107             * \brief Get Euler angles
    108             *
    109             * \param euler output datas
    110             */
    111             void GetEuler(core::Euler &euler) const;
     103  /*!
     104  * \brief Get Euler angles
     105  *
     106  * \param euler output datas
     107  */
     108  void GetEuler(core::Euler &euler) const;
    112109
    113             /*!
    114             * \brief Get Quaternion
    115             *
    116             * \param quaternion output datas
    117             */
    118             void GetQuaternion(core::Quaternion &quaternion) const;
    119             /*!
    120             * \brief Get Position
    121             *
    122             * \param point output datas
    123             */
    124             void GetPosition(core::Vector3D &point) const;
     110  /*!
     111  * \brief Get Quaternion
     112  *
     113  * \param quaternion output datas
     114  */
     115  void GetQuaternion(core::Quaternion &quaternion) const;
     116  /*!
     117  * \brief Get Position
     118  *
     119  * \param point output datas
     120  */
     121  void GetPosition(core::Vector3D &point) const;
    125122
    126             /*!
    127             * \brief Output matrix
    128             *
    129             * Matrix is of type float and as follows: \n
    130             * (0,0) roll (rad) \n
    131             * (1,0) pitch (rad) \n
    132             * (2,0) yaw (rad) \n
    133             * (3,0) x \n
    134             * (4,0) y \n
    135             * (5,0) z \n
    136             *
    137             * \return Output metrix
    138             */
    139             core::cvmatrix *Output(void) const;
     123  /*!
     124  * \brief Output matrix
     125  *
     126  * Matrix is of type float and as follows: \n
     127  * (0,0) roll (rad) \n
     128  * (1,0) pitch (rad) \n
     129  * (2,0) yaw (rad) \n
     130  * (3,0) x \n
     131  * (4,0) y \n
     132  * (5,0) z \n
     133  *
     134  * \return Output metrix
     135  */
     136  core::cvmatrix *Output(void) const;
    140137
    141             /*!
    142             * \brief State matrix
    143             *
    144             * Matrix is of type float and as follows: \n
    145             * (0,0) roll (deg) \n
    146             * (1,0) pitch (deg) \n
    147             * (2,0) yaw (deg) \n
    148             *
    149             * \return State metrix
    150             */
    151             core::cvmatrix *State(void) const;
     138  /*!
     139  * \brief State matrix
     140  *
     141  * Matrix is of type float and as follows: \n
     142  * (0,0) roll (deg) \n
     143  * (1,0) pitch (deg) \n
     144  * (2,0) yaw (deg) \n
     145  *
     146  * \return State metrix
     147  */
     148  core::cvmatrix *State(void) const;
    152149
    153             /*!
    154             * \brief x plot
    155             *
    156             * \return x plot
    157             */
    158             gui::DataPlot1D* xPlot(void) const;
     150  /*!
     151  * \brief x plot
     152  *
     153  * \return x plot
     154  */
     155  gui::DataPlot1D *xPlot(void) const;
    159156
    160             /*!
    161             * \brief y plot
    162             *
    163             * \return y plot
    164             */
    165             gui::DataPlot1D* yPlot(void) const;
     157  /*!
     158  * \brief y plot
     159  *
     160  * \return y plot
     161  */
     162  gui::DataPlot1D *yPlot(void) const;
    166163
    167             /*!
    168             * \brief z plot
    169             *
    170             * \return z plot
    171             */
    172             gui::DataPlot1D* zPlot(void) const;
     164  /*!
     165  * \brief z plot
     166  *
     167  * \return z plot
     168  */
     169  gui::DataPlot1D *zPlot(void) const;
    173170
    174         private:
    175             /*!
    176             * \brief Update using provided datas
    177             *
    178             * Reimplemented from IODevice.
    179             *
    180             * \param data data from the parent to process
    181             */
    182             void UpdateFrom(const core::io_data *data){};
     171private:
     172  /*!
     173  * \brief Update using provided datas
     174  *
     175  * Reimplemented from IODevice.
     176  *
     177  * \param data data from the parent to process
     178  */
     179  void UpdateFrom(const core::io_data *data){};
    183180
    184             void mainloop(void);
    185             class VrpnObject_impl* pimpl_;
    186     };
     181  void mainloop(void);
     182  class VrpnObject_impl *pimpl_;
     183};
    187184} // end namespace sensor
    188185} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

    r3 r15  
    3939using namespace flair::sensor;
    4040
    41 VrpnObject_impl::VrpnObject_impl(VrpnObject* self,const VrpnClient *parent,string name, int id,const TabWidget* tab)
    42 {
    43     this->parent=parent;
    44     this->self=self;
     41VrpnObject_impl::VrpnObject_impl(VrpnObject *self, const VrpnClient *parent,
     42                                 string name, int id, const TabWidget *tab) {
     43  this->parent = parent;
     44  this->self = self;
    4545
    46     if(id==-1 && parent->UseXbee())
    47     {
    48         self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
    49     }
    50     if(id!=-1 && !parent->UseXbee())
    51     {
    52         self->Warn("identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
    53     }
     46  if (id == -1 && parent->UseXbee()) {
     47    self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
     48  }
     49  if (id != -1 && !parent->UseXbee()) {
     50    self->Warn(
     51        "identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
     52  }
    5453
    55         if(parent->UseXbee())
    56         {
    57         parent->pimpl_->AddTrackable(this,id);
    58         tracker=NULL;
    59         }
    60         else
    61         {
    62             parent->pimpl_->AddTrackable(self);
    63         tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
    64         tracker->register_change_handler(this,handle_pos);
    65         tracker->shutup=true;
    66         }
     54  if (parent->UseXbee()) {
     55    parent->pimpl_->AddTrackable(this, id);
     56    tracker = NULL;
     57  } else {
     58    parent->pimpl_->AddTrackable(self);
     59    tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
     60    tracker->register_change_handler(this, handle_pos);
     61    tracker->shutup = true;
     62  }
    6763
    68         //state
    69         cvmatrix_descriptor* desc=new cvmatrix_descriptor(6,1);
    70         desc->SetElementName(0,0,"roll");
    71         desc->SetElementName(1,0,"pitch");
    72         desc->SetElementName(2,0,"yaw");
    73         desc->SetElementName(3,0,"x");
    74         desc->SetElementName(4,0,"y");
    75         desc->SetElementName(5,0,"z");
    76         output=new cvmatrix(self,desc,floatType);
     64  // state
     65  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
     66  desc->SetElementName(0, 0, "roll");
     67  desc->SetElementName(1, 0, "pitch");
     68  desc->SetElementName(2, 0, "yaw");
     69  desc->SetElementName(3, 0, "x");
     70  desc->SetElementName(4, 0, "y");
     71  desc->SetElementName(5, 0, "z");
     72  output = new cvmatrix(self, desc, floatType);
    7773
    78         desc=new cvmatrix_descriptor(3,1);
    79         desc->SetElementName(0,0,"roll");
    80         desc->SetElementName(1,0,"pitch");
    81         desc->SetElementName(2,0,"yaw");
    82         state=new cvmatrix(self,desc,floatType);
     74  desc = new cvmatrix_descriptor(3, 1);
     75  desc->SetElementName(0, 0, "roll");
     76  desc->SetElementName(1, 0, "pitch");
     77  desc->SetElementName(2, 0, "yaw");
     78  state = new cvmatrix(self, desc, floatType);
    8379
    84         //ui
    85         plot_tab=new Tab(tab,"Mesures "+ name);
    86         x_plot=new DataPlot1D(plot_tab->NewRow(),"x",-10,10);
    87             x_plot->AddCurve(output->Element(3));
    88         y_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"y",-10,10);
    89             y_plot->AddCurve(output->Element(4));
    90         z_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"z",-2,0);
    91             z_plot->AddCurve(output->Element(5));
     80  // ui
     81  plot_tab = new Tab(tab, "Mesures " + name);
     82  x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
     83  x_plot->AddCurve(output->Element(3));
     84  y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
     85  y_plot->AddCurve(output->Element(4));
     86  z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
     87  z_plot->AddCurve(output->Element(5));
    9288}
    9389
    94 VrpnObject_impl::~VrpnObject_impl(void)
    95 {
    96     if(tracker!=NULL)//normal
    97         {
    98             parent->pimpl_->RemoveTrackable(self);
    99         tracker->unregister_change_handler(this,handle_pos);
    100         delete tracker;
    101         }
    102         else//xbee
    103         {
    104         parent->pimpl_->RemoveTrackable(this);
    105         }
    106         delete plot_tab;
     90VrpnObject_impl::~VrpnObject_impl(void) {
     91  if (tracker != NULL) // normal
     92  {
     93    parent->pimpl_->RemoveTrackable(self);
     94    tracker->unregister_change_handler(this, handle_pos);
     95    delete tracker;
     96  } else // xbee
     97  {
     98    parent->pimpl_->RemoveTrackable(this);
     99  }
     100  delete plot_tab;
    107101}
    108102
    109 void VrpnObject_impl::mainloop(void)
    110 {
    111         tracker->mainloop();
     103void VrpnObject_impl::mainloop(void) { tracker->mainloop(); }
     104
     105bool VrpnObject_impl::IsTracked(unsigned int timeout_ms) {
     106  output->GetMutex();
     107  Time a = GetTime();
     108  Time dt = a - output->DataTime();
     109  output->ReleaseMutex();
     110
     111  if (dt > (Time)(timeout_ms * 1000000)) {
     112    // self->Printf("%lld %lld %lld
     113    // %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
     114    return false;
     115  } else {
     116    return true;
     117  }
    112118}
    113119
    114 bool VrpnObject_impl::IsTracked(unsigned int timeout_ms)
    115 {
    116     output->GetMutex();
    117     Time a=GetTime();
    118     Time dt=a-output->DataTime();
    119     output->ReleaseMutex();
    120 
    121     if(dt>(Time)(timeout_ms*1000000))
    122     {
    123         //self->Printf("%lld %lld %lld %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
    124         return false;
    125     }
    126     else
    127     {
    128         return true;
    129     }
    130 }
    131 
    132 void VrpnObject_impl::GetEuler(Euler &euler)
    133 {
    134     output->GetMutex();
    135     euler.roll=output->ValueNoMutex(0,0);
    136     euler.pitch=output->ValueNoMutex(1,0);
    137     euler.yaw=output->ValueNoMutex(2,0);
    138     output->ReleaseMutex();
     120void VrpnObject_impl::GetEuler(Euler &euler) {
     121  output->GetMutex();
     122  euler.roll = output->ValueNoMutex(0, 0);
     123  euler.pitch = output->ValueNoMutex(1, 0);
     124  euler.yaw = output->ValueNoMutex(2, 0);
     125  output->ReleaseMutex();
    139126}
    140127
    141128void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
    142     output->GetMutex();
    143     quaternion.q0=this->quaternion.q0;
    144     quaternion.q1=this->quaternion.q1;
    145     quaternion.q2=this->quaternion.q2;
    146     quaternion.q3=this->quaternion.q3;
    147     output->ReleaseMutex();
     129  output->GetMutex();
     130  quaternion.q0 = this->quaternion.q0;
     131  quaternion.q1 = this->quaternion.q1;
     132  quaternion.q2 = this->quaternion.q2;
     133  quaternion.q3 = this->quaternion.q3;
     134  output->ReleaseMutex();
    148135}
    149136
    150 void VrpnObject_impl::GetPosition(Vector3D &point)
    151 {
    152     output->GetMutex();
    153     point.x=output->ValueNoMutex(3,0);
    154     point.y=output->ValueNoMutex(4,0);
    155     point.z=output->ValueNoMutex(5,0);
    156     output->ReleaseMutex();
     137void VrpnObject_impl::GetPosition(Vector3D &point) {
     138  output->GetMutex();
     139  point.x = output->ValueNoMutex(3, 0);
     140  point.y = output->ValueNoMutex(4, 0);
     141  point.z = output->ValueNoMutex(5, 0);
     142  output->ReleaseMutex();
    157143}
    158144
    159 void VRPN_CALLBACK VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t)
    160 {
    161     bool is_nan=false;
    162         VrpnObject_impl* caller= reinterpret_cast<VrpnObject_impl*>(userdata);
    163         Time time=GetTime();
     145void VRPN_CALLBACK
     146VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t) {
     147  bool is_nan = false;
     148  VrpnObject_impl *caller = reinterpret_cast<VrpnObject_impl *>(userdata);
     149  Time time = GetTime();
    164150
    165     //check if something is nan
    166     for(int i=0;i<3;i++)
    167     {
    168         if(isnan(t.pos[i])==true) is_nan=true;
    169     }
    170     for(int i=0;i<4;i++)
    171     {
    172         if(isnan(t.quat[i])==true) is_nan=true;
    173     }
    174     if(is_nan==true)
    175     {
    176         caller->self->Warn("data is nan, skipping it (time %lld)\n",time);
    177         return;
    178     }
     151  // check if something is nan
     152  for (int i = 0; i < 3; i++) {
     153    if (isnan(t.pos[i]) == true)
     154      is_nan = true;
     155  }
     156  for (int i = 0; i < 4; i++) {
     157    if (isnan(t.quat[i]) == true)
     158      is_nan = true;
     159  }
     160  if (is_nan == true) {
     161    caller->self->Warn("data is nan, skipping it (time %lld)\n", time);
     162    return;
     163  }
    179164
    180     //on prend une fois pour toute le mutex et on fait des accès directs
    181     caller->output->GetMutex();
     165  // on prend une fois pour toute le mutex et on fait des accès directs
     166  caller->output->GetMutex();
    182167
    183     //warning: t.quat is defined as (qx,qy,qz,qw), which is different from flair::core::Quaternion
    184     caller->quaternion.q0=t.quat[3];
    185     caller->quaternion.q1=t.quat[0];
    186     caller->quaternion.q2=t.quat[1];
    187     caller->quaternion.q3=t.quat[2];
    188     Vector3D pos((float)t.pos[0],(float)t.pos[1],(float)t.pos[2]);
     168  // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
     169  // flair::core::Quaternion
     170  caller->quaternion.q0 = t.quat[3];
     171  caller->quaternion.q1 = t.quat[0];
     172  caller->quaternion.q2 = t.quat[1];
     173  caller->quaternion.q3 = t.quat[2];
     174  Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
    189175
    190     //on effectue les rotation
    191     caller->parent->pimpl_->ComputeRotations(pos);
    192     caller->parent->pimpl_->ComputeRotations(caller->quaternion);
     176  // on effectue les rotation
     177  caller->parent->pimpl_->ComputeRotations(pos);
     178  caller->parent->pimpl_->ComputeRotations(caller->quaternion);
    193179
    194     Euler euler;
    195     caller->quaternion.ToEuler(euler);
    196     caller->output->SetValueNoMutex( 0, 0,euler.roll);
    197     caller->output->SetValueNoMutex( 1, 0,euler.pitch);
    198     caller->output->SetValueNoMutex( 2, 0,euler.yaw);
    199     caller->output->SetValueNoMutex( 3, 0,pos.x);
    200     caller->output->SetValueNoMutex( 4, 0,pos.y);
    201     caller->output->SetValueNoMutex( 5, 0,pos.z);
     180  Euler euler;
     181  caller->quaternion.ToEuler(euler);
     182  caller->output->SetValueNoMutex(0, 0, euler.roll);
     183  caller->output->SetValueNoMutex(1, 0, euler.pitch);
     184  caller->output->SetValueNoMutex(2, 0, euler.yaw);
     185  caller->output->SetValueNoMutex(3, 0, pos.x);
     186  caller->output->SetValueNoMutex(4, 0, pos.y);
     187  caller->output->SetValueNoMutex(5, 0, pos.z);
    202188
    203     caller->output->SetDataTime(time);
    204     caller->output->ReleaseMutex();
     189  caller->output->SetDataTime(time);
     190  caller->output->ReleaseMutex();
    205191
    206     caller->state->GetMutex();
    207     caller->state->SetValueNoMutex( 0, 0,Euler::ToDegree(euler.roll));
    208     caller->state->SetValueNoMutex( 1, 0,Euler::ToDegree(euler.pitch));
    209     caller->state->SetValueNoMutex(2, 0,Euler::ToDegree(euler.yaw));
    210     caller->state->ReleaseMutex();
     192  caller->state->GetMutex();
     193  caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
     194  caller->state->SetValueNoMutex(1, 0, Euler::ToDegree(euler.pitch));
     195  caller->state->SetValueNoMutex(2, 0, Euler::ToDegree(euler.yaw));
     196  caller->state->ReleaseMutex();
    211197
    212     caller->self->ProcessUpdate(caller->output);
     198  caller->self->ProcessUpdate(caller->output);
    213199}
    214 

  • trunk/lib/FlairSensorActuator/src/XBldc.cpp

    r3 r15  
    2323using namespace flair::gui;
    2424
    25 namespace flair
    26 {
    27 namespace actuator
    28 {
     25namespace flair {
     26namespace actuator {
    2927
    30 XBldc::XBldc(const IODevice* parent,Layout* layout,string name,I2cPort* i2cport) : Bldc(parent,layout,name,4) {
    31     pimpl_=new XBldc_impl(this,i2cport);
     28XBldc::XBldc(const IODevice *parent, Layout *layout, string name,
     29             I2cPort *i2cport)
     30    : Bldc(parent, layout, name, 4) {
     31  pimpl_ = new XBldc_impl(this, i2cport);
    3232}
    3333
    34 XBldc::~XBldc() {
    35     delete pimpl_;
    36 }
     34XBldc::~XBldc() { delete pimpl_; }
    3735
    38 void XBldc::SetMotors(float* value) {
    39     pimpl_->SetMotors(value);
    40 }
     36void XBldc::SetMotors(float *value) { pimpl_->SetMotors(value); }
    4137
    4238} // end namespace actuator

  • trunk/lib/FlairSensorActuator/src/XBldc.h

    r3 r15  
    1616#include "Bldc.h"
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class IODevice;
    23         class I2cPort;
    24     }
    25     namespace gui
    26     {
    27         class Layout;
    28     }
     18namespace flair {
     19namespace core {
     20class IODevice;
     21class I2cPort;
     22}
     23namespace gui {
     24class Layout;
     25}
    2926}
    3027
    3128class XBldc_impl;
    3229
    33 namespace flair
    34 {
    35 namespace actuator
    36 {
    37     /*! \class XBldc
    38     *
    39     * \brief Class for Xufo Bldc
    40     */
    41     class XBldc : public Bldc
    42     {
    43         public:
    44             /*!
    45             * \brief Constructor
    46             *
    47             * Construct a XBldc.
    48             *
    49             * \param parent parent
    50             * \param layout layout
    51             * \param name name
    52             * \param i2cport i2cport
    53             */
    54             XBldc(const core::IODevice* parent,gui::Layout* layout,std::string name,core::I2cPort* i2cport);
     30namespace flair {
     31namespace actuator {
     32/*! \class XBldc
     33*
     34* \brief Class for Xufo Bldc
     35*/
     36class XBldc : public Bldc {
     37public:
     38  /*!
     39  * \brief Constructor
     40  *
     41  * Construct a XBldc.
     42  *
     43  * \param parent parent
     44  * \param layout layout
     45  * \param name name
     46  * \param i2cport i2cport
     47  */
     48  XBldc(const core::IODevice *parent, gui::Layout *layout, std::string name,
     49        core::I2cPort *i2cport);
    5550
    56             /*!
    57             * \brief Destructor
    58             *
    59             */
    60             ~XBldc();
     51  /*!
     52  * \brief Destructor
     53  *
     54  */
     55  ~XBldc();
    6156
    62             /*!
    63             * \brief Has speed measurement
    64             *
    65             * Reimplemented from Bldc. \n
    66             *
    67             * \return true if it has speed measurement
    68             */
    69             bool HasSpeedMeasurement(void) const{return false;};
     57  /*!
     58  * \brief Has speed measurement
     59  *
     60  * Reimplemented from Bldc. \n
     61  *
     62  * \return true if it has speed measurement
     63  */
     64  bool HasSpeedMeasurement(void) const { return false; };
    7065
    71             /*!
    72             * \brief Has current measurement
    73             *
    74             * Reimplemented from Bldc. \n
    75             *
    76             * \return true if it has current measurement
    77             */
    78             bool HasCurrentMeasurement(void) const{return false;};
     66  /*!
     67  * \brief Has current measurement
     68  *
     69  * Reimplemented from Bldc. \n
     70  *
     71  * \return true if it has current measurement
     72  */
     73  bool HasCurrentMeasurement(void) const { return false; };
    7974
    80         private:
    81             /*!
    82             * \brief Set motors values
    83             *
    84             * Reimplemented from Bldc. \n
    85             * Values size must be the same as MotorsCount()
    86             *
    87             * \param values motor values
    88             */
    89             void SetMotors(float* value);
     75private:
     76  /*!
     77  * \brief Set motors values
     78  *
     79  * Reimplemented from Bldc. \n
     80  * Values size must be the same as MotorsCount()
     81  *
     82  * \param values motor values
     83  */
     84  void SetMotors(float *value);
    9085
    91             class XBldc_impl* pimpl_;
    92     };
     86  class XBldc_impl *pimpl_;
     87};
    9388} // end namespace actuator
    9489} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp

    r3 r15  
    2929using namespace flair::actuator;
    3030
    31 XBldc_impl::XBldc_impl(XBldc* self,I2cPort* i2cport) {
    32     this->self=self;
    33         this->i2cport=i2cport;
     31XBldc_impl::XBldc_impl(XBldc *self, I2cPort *i2cport) {
     32  this->self = self;
     33  this->i2cport = i2cport;
    3434
    35     i2cport->SetTxTimeout(5000000);
     35  i2cport->SetTxTimeout(5000000);
    3636}
    3737
    38 XBldc_impl::~XBldc_impl() {
    39 }
     38XBldc_impl::~XBldc_impl() {}
    4039
    41 void XBldc_impl::SetMotors(float* value) {
    42     uint8_t tosend_value[5];
     40void XBldc_impl::SetMotors(float *value) {
     41  uint8_t tosend_value[5];
    4342
    44     for(int i=0;i<4;i++) tosend_value[i]=(uint8_t)(MAX_VALUE*value[i]);
     43  for (int i = 0; i < 4; i++)
     44    tosend_value[i] = (uint8_t)(MAX_VALUE * value[i]);
    4545
    46     ssize_t written;
    47     tosend_value[4]=0xaa;
    48     for(int i=0;i<4;i++) tosend_value[4]+=tosend_value[i];
    49     self->Err("please verify motors adresses\n");
    50     //todo faire la procedure changement addresse par la station sol
    51 /*
    52     i2cport->GetMutex();
    53     i2cport->SetSlave(BASE_ADDRESS);
     46  ssize_t written;
     47  tosend_value[4] = 0xaa;
     48  for (int i = 0; i < 4; i++)
     49    tosend_value[4] += tosend_value[i];
     50  self->Err("please verify motors adresses\n");
     51  // todo faire la procedure changement addresse par la station sol
     52  /*
     53      i2cport->GetMutex();
     54      i2cport->SetSlave(BASE_ADDRESS);
    5455
    55     written =i2cport->Write(tosend_value, sizeof(tosend_value));
    56     if(written<0)
    57     {
    58         self->Err("rt_dev_write error (%s)\n",strerror(-written));
    59     }
    60     else if (written != sizeof(tosend_value))
    61     {
    62         self->Err("rt_dev_write error %i/%i\n",written,sizeof(tosend_value));
    63     }
     56      written =i2cport->Write(tosend_value, sizeof(tosend_value));
     57      if(written<0)
     58      {
     59          self->Err("rt_dev_write error (%s)\n",strerror(-written));
     60      }
     61      else if (written != sizeof(tosend_value))
     62      {
     63          self->Err("rt_dev_write error %i/%i\n",written,sizeof(tosend_value));
     64      }
    6465
    65     i2cport->ReleaseMutex();*/
    66 
     66      i2cport->ReleaseMutex();*/
    6767}
    6868/*
    … …  
    9494*/
    9595void XBldc_impl::ChangeDirection(uint8_t moteur) {
    96     unsigned char tx[4];
    97     ssize_t written;
     96  unsigned char tx[4];
     97  ssize_t written;
    9898
    99     tx[0]=254;
    100     tx[1]=moteur;
    101     tx[2]=0;
    102     tx[3]=234+moteur;
     99  tx[0] = 254;
     100  tx[1] = moteur;
     101  tx[2] = 0;
     102  tx[3] = 234 + moteur;
    103103
    104     i2cport->GetMutex();
    105     i2cport->SetSlave(BASE_ADDRESS);
     104  i2cport->GetMutex();
     105  i2cport->SetSlave(BASE_ADDRESS);
    106106
    107     written =i2cport->Write(tx, sizeof(tx));
    108     if(written<0) {
    109         self->Err("rt_dev_write (%s) error\n",strerror(-written));
    110     } else if (written != sizeof(tx)) {
    111         self->Err("rt_dev_write %i/%i error\n",written,sizeof(tx));
    112     }
     107  written = i2cport->Write(tx, sizeof(tx));
     108  if (written < 0) {
     109    self->Err("rt_dev_write (%s) error\n", strerror(-written));
     110  } else if (written != sizeof(tx)) {
     111    self->Err("rt_dev_write %i/%i error\n", written, sizeof(tx));
     112  }
    113113
    114     i2cport->ReleaseMutex();
     114  i2cport->ReleaseMutex();
    115115}
    116116
    117 void XBldc_impl::ChangeAdress(uint8_t old_add,uint8_t new_add) {
    118     unsigned char tx[4];
    119     ssize_t written;
     117void XBldc_impl::ChangeAdress(uint8_t old_add, uint8_t new_add) {
     118  unsigned char tx[4];
     119  ssize_t written;
    120120
    121     tx[0]=250;
    122     tx[1]=old_add;
    123     tx[2]=new_add;
    124     tx[3]=230+old_add+new_add;
     121  tx[0] = 250;
     122  tx[1] = old_add;
     123  tx[2] = new_add;
     124  tx[3] = 230 + old_add + new_add;
    125125
    126     i2cport->GetMutex();
    127     i2cport->SetSlave(BASE_ADDRESS);
     126  i2cport->GetMutex();
     127  i2cport->SetSlave(BASE_ADDRESS);
    128128
    129     written =i2cport->Write(tx, sizeof(tx));
    130     if(written<0) {
    131         self->Err("rt_dev_write error (%s)\n",strerror(-written));
    132     } else if (written != sizeof(tx)) {
    133         self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
    134     }
     129  written = i2cport->Write(tx, sizeof(tx));
     130  if (written < 0) {
     131    self->Err("rt_dev_write error (%s)\n", strerror(-written));
     132  } else if (written != sizeof(tx)) {
     133    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
     134  }
    135135
    136     i2cport->ReleaseMutex();
     136  i2cport->ReleaseMutex();
    137137}
    138138
    139 uint8_t XBldc_impl::Sat(float value,uint8_t min,uint8_t max) {
    140   uint8_t sat_value=(uint8_t)value;
     139uint8_t XBldc_impl::Sat(float value, uint8_t min, uint8_t max) {
     140  uint8_t sat_value = (uint8_t)value;
    141141
    142   if(value>((float)sat_value+0.5)) sat_value++;
     142  if (value > ((float)sat_value + 0.5))
     143    sat_value++;
    143144
    144   if(value<(float)min) sat_value=min;
    145   if(value>(float)max) sat_value=max;
     145  if (value < (float)min)
     146    sat_value = min;
     147  if (value > (float)max)
     148    sat_value = max;
    146149
    147150  return sat_value;

  • trunk/lib/FlairSensorActuator/src/geodesie.cpp

    r3 r15  
    88
    99#ifdef _MSC_VER
    10 #   pragma warning(disable:4244)
     10#pragma warning(disable : 4244)
    1111#endif //_MSC_VER
    1212
    1313namespace Geodesie {
    1414/// ////////////////////////////////////////////////////////////////////
    15 Matrice::Matrice(const Matrice & A) {
    16     c0_l0=A.c0_l0;c1_l0=A.c1_l0;c2_l0=A.c2_l0;
    17     c0_l1=A.c0_l1;c1_l1=A.c1_l1;c2_l1=A.c2_l1;
    18     c0_l2=A.c0_l2;c1_l2=A.c1_l2;c2_l2=A.c2_l2;
     15Matrice::Matrice(const Matrice &A) {
     16  c0_l0 = A.c0_l0;
     17  c1_l0 = A.c1_l0;
     18  c2_l0 = A.c2_l0;
     19  c0_l1 = A.c0_l1;
     20  c1_l1 = A.c1_l1;
     21  c2_l1 = A.c2_l1;
     22  c0_l2 = A.c0_l2;
     23  c1_l2 = A.c1_l2;
     24  c2_l2 = A.c2_l2;
    1925}
    2026/// ////////////////////////////////////////////////////////////////////
    2127Matrice::Matrice() {
    22     c0_l0=0.0;c1_l0=0.0;c2_l0=0.0;
    23     c0_l1=0.0;c1_l1=0.0;c2_l1=0.0;
    24     c0_l2=0.0;c1_l2=0.0;c2_l2=0.0;
    25 }
    26 /// ////////////////////////////////////////////////////////////////////
    27 void Matrice::Apply(double v0,double v1,double v2, double& Mv0,double& Mv1,double& Mv2) {
    28     Mv0 = c0_l0*v0 + c1_l0*v1 + c2_l0*v2;
    29     Mv1 = c0_l1*v0 + c1_l1*v1 + c2_l1*v2;
    30     Mv2 = c0_l2*v0 + c1_l2*v1 + c2_l2*v2;
     28  c0_l0 = 0.0;
     29  c1_l0 = 0.0;
     30  c2_l0 = 0.0;
     31  c0_l1 = 0.0;
     32  c1_l1 = 0.0;
     33  c2_l1 = 0.0;
     34  c0_l2 = 0.0;
     35  c1_l2 = 0.0;
     36  c2_l2 = 0.0;
     37}
     38/// ////////////////////////////////////////////////////////////////////
     39void Matrice::Apply(double v0, double v1, double v2, double &Mv0, double &Mv1,
     40                    double &Mv2) {
     41  Mv0 = c0_l0 * v0 + c1_l0 * v1 + c2_l0 * v2;
     42  Mv1 = c0_l1 * v0 + c1_l1 * v1 + c2_l1 * v2;
     43  Mv2 = c0_l2 * v0 + c1_l2 * v1 + c2_l2 * v2;
    3144}
    3245/// ////////////////////////////////////////////////////////////////////
    3346Matrice ProdMat(const Matrice A, const Matrice B) {
    34     Matrice out;
    35 
    36     out.c0_l0=A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
    37     out.c1_l0=A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
    38     out.c2_l0=A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
    39 
    40     out.c0_l1=A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
    41     out.c1_l1=A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
    42     out.c2_l1=A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
    43 
    44     out.c0_l2=A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
    45     out.c1_l2=A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
    46     out.c2_l2=A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
    47     return out;
     47  Matrice out;
     48
     49  out.c0_l0 = A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
     50  out.c1_l0 = A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
     51  out.c2_l0 = A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;
     52
     53  out.c0_l1 = A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
     54  out.c1_l1 = A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
     55  out.c2_l1 = A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;
     56
     57  out.c0_l2 = A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
     58  out.c1_l2 = A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
     59  out.c2_l2 = A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
     60  return out;
    4861}
    4962
    5063/// ////////////////////////////////////////////////////////////////////
    5164Matrice TransMat(const Matrice A) {
    52     Matrice out;
    53     out.c0_l0=A.c0_l0 ; out.c1_l0 = A.c0_l1 ; out.c2_l0 = A.c0_l2 ;
    54     out.c0_l1=A.c1_l0 ; out.c1_l1 = A.c1_l1 ; out.c2_l1 = A.c1_l2 ;
    55     out.c0_l2=A.c2_l0 ; out.c1_l2 = A.c2_l1 ; out.c2_l2 = A.c2_l2 ;
    56     return out;
    57 }
    58 
    59 /// ////////////////////////////////////////////////////////////////////
    60 void Write(const Matrice A,std::ostream& out) {
    61     out<< A.c0_l0<<"\t"<< A.c1_l0<<"\t"<< A.c2_l0<<"\n";
    62     out<< A.c0_l1<<"\t"<< A.c1_l1<<"\t"<< A.c2_l1<<"\n";
    63     out<< A.c0_l2<<"\t"<< A.c1_l2<<"\t"<< A.c2_l2<<"\n";
    64 }
    65 
    66 /// ////////////////////////////////////////////////////////////////////
    67 Raf98::~Raf98() {
    68     m_dvalues.clear();
    69 }
     65  Matrice out;
     66  out.c0_l0 = A.c0_l0;
     67  out.c1_l0 = A.c0_l1;
     68  out.c2_l0 = A.c0_l2;
     69  out.c0_l1 = A.c1_l0;
     70  out.c1_l1 = A.c1_l1;
     71  out.c2_l1 = A.c1_l2;
     72  out.c0_l2 = A.c2_l0;
     73  out.c1_l2 = A.c2_l1;
     74  out.c2_l2 = A.c2_l2;
     75  return out;
     76}
     77
     78/// ////////////////////////////////////////////////////////////////////
     79void Write(const Matrice A, std::ostream &out) {
     80  out << A.c0_l0 << "\t" << A.c1_l0 << "\t" << A.c2_l0 << "\n";
     81  out << A.c0_l1 << "\t" << A.c1_l1 << "\t" << A.c2_l1 << "\n";
     82  out << A.c0_l2 << "\t" << A.c1_l2 << "\t" << A.c2_l2 << "\n";
     83}
     84
     85/// ////////////////////////////////////////////////////////////////////
     86Raf98::~Raf98() { m_dvalues.clear(); }
    7087
    7188//-----------------------------------------------------------------------------
    72 bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const {
    73     *Hwgs84 = 0.0;
    74     if (m_dvalues.size()==0)
    75         return false;
    76     const double longitude_min =  -5.5;
    77     const double longitude_max =  8.5;
    78     if (longitude < longitude_min)
    79         return false;
    80     if (longitude > longitude_max)
    81         return false;
    82 
    83     const double latitude_min =  42;
    84     const double latitude_max =  51.5;
    85     if (latitude < latitude_min)
    86         return false;
    87     if (latitude > latitude_max)
    88         return false;
    89 
    90     //conversion en position
    91     double longPix = (longitude - longitude_min) * 30.;
    92     double latPix  = (latitude_max - latitude) * 40.;
    93 
    94     double RestCol,RestLig;
    95     double  ColIni,LigIni;
    96     RestCol = modf(longPix,&ColIni);
    97     RestLig = modf(latPix,&LigIni);
    98 
    99 
    100     double Zbd = (1.0-RestCol) * (1.0-RestLig) * LitGrille(ColIni  , LigIni  );
    101     Zbd += RestCol * (1.0-RestLig) * LitGrille(ColIni+1, LigIni  );
    102     Zbd += (1.0-RestCol) *   RestLig  * LitGrille(ColIni  , LigIni+1);
    103     Zbd += RestCol * RestLig * LitGrille(ColIni+1, LigIni+1);
    104     *Hwgs84 = Zbd;
    105 
    106 
    107     return true;
    108 }
    109 /// ////////////////////////////////////////////////////////////////////
    110 double Raf98::LitGrille(unsigned int c,unsigned int l) const{
    111     const unsigned int w=421;
    112     //    const unsigned int h=381;
    113     return m_dvalues.at(c+l*w);
    114 }
    115 /// ////////////////////////////////////////////////////////////////////
    116 bool Raf98::Load(const std::string & sin) {
    117     std::ifstream in(sin.c_str());
    118     unsigned int w = 421;
    119     unsigned int h = 381;
    120 
    121     m_dvalues.reserve(w*h);
    122 
    123     char entete[1024];//sur 3 lignes
    124     in.getline(entete,1023);
    125     in.getline(entete,1023);
    126     in.getline(entete,1023);
    127 
    128     char bidon[1024];
    129     double val;
    130     for (unsigned int i=0; i< h; ++i) {
    131         for (unsigned int j=0; j< 52; ++j)  {
    132             for (unsigned int k=0; k< 8; ++k) {
    133                 in >> val;
    134                 m_dvalues.push_back( val );
    135             }
    136             in.getline(bidon,1023);
    137         }
    138         for (unsigned int k=0; k< 5; ++k) {
    139             in >> val;
    140             m_dvalues.push_back( val );
    141         }
    142         in.getline(bidon,1023);
    143         if (!in.good())         {
    144             m_dvalues.clear();
    145             return false;
    146         }
     89bool Raf98::Interpol(double longitude, double latitude, double *Hwgs84) const {
     90  *Hwgs84 = 0.0;
     91  if (m_dvalues.size() == 0)
     92    return false;
     93  const double longitude_min = -5.5;
     94  const double longitude_max = 8.5;
     95  if (longitude < longitude_min)
     96    return false;
     97  if (longitude > longitude_max)
     98    return false;
     99
     100  const double latitude_min = 42;
     101  const double latitude_max = 51.5;
     102  if (latitude < latitude_min)
     103    return false;
     104  if (latitude > latitude_max)
     105    return false;
     106
     107  // conversion en position
     108  double longPix = (longitude - longitude_min) * 30.;
     109  double latPix = (latitude_max - latitude) * 40.;
     110
     111  double RestCol, RestLig;
     112  double ColIni, LigIni;
     113  RestCol = modf(longPix, &ColIni);
     114  RestLig = modf(latPix, &LigIni);
     115
     116  double Zbd = (1.0 - RestCol) * (1.0 - RestLig) * LitGrille(ColIni, LigIni);
     117  Zbd += RestCol * (1.0 - RestLig) * LitGrille(ColIni + 1, LigIni);
     118  Zbd += (1.0 - RestCol) * RestLig * LitGrille(ColIni, LigIni + 1);
     119  Zbd += RestCol * RestLig * LitGrille(ColIni + 1, LigIni + 1);
     120  *Hwgs84 = Zbd;
     121
     122  return true;
     123}
     124/// ////////////////////////////////////////////////////////////////////
     125double Raf98::LitGrille(unsigned int c, unsigned int l) const {
     126  const unsigned int w = 421;
     127  //    const unsigned int h=381;
     128  return m_dvalues.at(c + l * w);
     129}
     130/// ////////////////////////////////////////////////////////////////////
     131bool Raf98::Load(const std::string &sin) {
     132  std::ifstream in(sin.c_str());
     133  unsigned int w = 421;
     134  unsigned int h = 381;
     135
     136  m_dvalues.reserve(w * h);
     137
     138  char entete[1024]; // sur 3 lignes
     139  in.getline(entete, 1023);
     140  in.getline(entete, 1023);
     141  in.getline(entete, 1023);
     142
     143  char bidon[1024];
     144  double val;
     145  for (unsigned int i = 0; i < h; ++i) {
     146    for (unsigned int j = 0; j < 52; ++j) {
     147      for (unsigned int k = 0; k < 8; ++k) {
     148        in >> val;
     149        m_dvalues.push_back(val);
     150      }
     151      in.getline(bidon, 1023);
    147152    }
    148     return in.good();
     153    for (unsigned int k = 0; k < 5; ++k) {
     154      in >> val;
     155      m_dvalues.push_back(val);
     156    }
     157    in.getline(bidon, 1023);
     158    if (!in.good()) {
     159      m_dvalues.clear();
     160      return false;
     161    }
     162  }
     163  return in.good();
    149164}
    150165
    … …  
    155170
    156171/// ////////////////////////////////////////////////////////////////////
    157 //ALGO0001
    158 double Geodesie::LatitueIsometrique(double latitude,double e) {
    159     double li;
    160     li = log(tan(M_PI_4 + latitude/2.)) + e*log( (1-e*sin(latitude))/(1+e*sin(latitude)) )/2;
    161     return li;
    162 }
    163 
    164 /// ////////////////////////////////////////////////////////////////////
    165 //ALGO0002
    166 double Geodesie::LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon) {
    167     double latitude_i=2*atan(exp(latitude_iso)) - M_PI_2;
    168     double latitude_ip1=latitude_i+epsilon*2;
    169     while (fabs(latitude_i-latitude_ip1)>epsilon) {
    170         latitude_i=latitude_ip1;
    171         latitude_ip1=2*atan(
    172                     exp(e*0.5*
    173                         log(
    174                             (1+e*sin(latitude_i))/(1-e*sin(latitude_i))
    175                             )
    176                         )
    177                     *exp(latitude_iso)
    178                     ) - M_PI_2;
    179     }
    180     return latitude_ip1;
    181 }
    182 /// ////////////////////////////////////////////////////////////////////
    183 void Geodesie::Geo2ProjLambert(
    184     double lambda,double phi,
    185     double n, double c,double e,
    186     double lambdac,double xs,double ys,
    187     double& X,double& Y)
    188 {
    189     double lat_iso=LatitueIsometrique(phi,e);
    190     X=xs+c*exp(-n*lat_iso)*sin(n*(lambda-lambdac));
    191     Y=ys-c*exp(-n*lat_iso)*cos(n*(lambda-lambdac));
    192 }
    193 /// ////////////////////////////////////////////////////////////////////
    194 //ALGO0004
    195 void Geodesie::Proj2GeoLambert(
    196     double X,double Y,
    197     double n, double c,double e,
    198     double lambdac,double xs,double ys,
    199     double epsilon,
    200     double& lambda,double& phi)
    201 {
    202     double X_xs=X-xs;
    203     double ys_Y=ys-Y;
    204     double R=sqrt(X_xs*X_xs+ys_Y*ys_Y);
    205     double gamma=atan(X_xs/ys_Y);
    206     lambda=lambdac+gamma/n;
    207     double lat_iso=-1/n*log(fabs(R/c));
    208     phi=LatitueIsometrique2Lat(lat_iso,e,epsilon);
     172// ALGO0001
     173double Geodesie::LatitueIsometrique(double latitude, double e) {
     174  double li;
     175  li = log(tan(M_PI_4 + latitude / 2.)) +
     176       e * log((1 - e * sin(latitude)) / (1 + e * sin(latitude))) / 2;
     177  return li;
     178}
     179
     180/// ////////////////////////////////////////////////////////////////////
     181// ALGO0002
     182double Geodesie::LatitueIsometrique2Lat(double latitude_iso, double e,
     183                                        double epsilon) {
     184  double latitude_i = 2 * atan(exp(latitude_iso)) - M_PI_2;
     185  double latitude_ip1 = latitude_i + epsilon * 2;
     186  while (fabs(latitude_i - latitude_ip1) > epsilon) {
     187    latitude_i = latitude_ip1;
     188    latitude_ip1 = 2 * atan(exp(e * 0.5 * log((1 + e * sin(latitude_i)) /
     189                                              (1 - e * sin(latitude_i)))) *
     190                            exp(latitude_iso)) -
     191                   M_PI_2;
     192  }
     193  return latitude_ip1;
     194}
     195/// ////////////////////////////////////////////////////////////////////
     196void Geodesie::Geo2ProjLambert(double lambda, double phi, double n, double c,
     197                               double e, double lambdac, double xs, double ys,
     198                               double &X, double &Y) {
     199  double lat_iso = LatitueIsometrique(phi, e);
     200  X = xs + c * exp(-n * lat_iso) * sin(n * (lambda - lambdac));
     201  Y = ys - c * exp(-n * lat_iso) * cos(n * (lambda - lambdac));
     202}
     203/// ////////////////////////////////////////////////////////////////////
     204// ALGO0004
     205void Geodesie::Proj2GeoLambert(double X, double Y, double n, double c, double e,
     206                               double lambdac, double xs, double ys,
     207                               double epsilon, double &lambda, double &phi) {
     208  double X_xs = X - xs;
     209  double ys_Y = ys - Y;
     210  double R = sqrt(X_xs * X_xs + ys_Y * ys_Y);
     211  double gamma = atan(X_xs / ys_Y);
     212  lambda = lambdac + gamma / n;
     213  double lat_iso = -1 / n * log(fabs(R / c));
     214  phi = LatitueIsometrique2Lat(lat_iso, e, epsilon);
    209215}
    210216/// ////////////////////////////////////////////////////////////////////
    211217double Geodesie::ConvMerApp(double longitude) {
    212     double phi0_Lambert93 = Deg2Rad(46.5);
    213     double lambda0_Lambert93 = Deg2Rad(3.0);
    214     double conv=-sin(phi0_Lambert93)*(longitude-lambda0_Lambert93);
    215     return conv;
     218  double phi0_Lambert93 = Deg2Rad(46.5);
     219  double lambda0_Lambert93 = Deg2Rad(3.0);
     220  double conv = -sin(phi0_Lambert93) * (longitude - lambda0_Lambert93);
     221  return conv;
    216222}
    217223
    218224////////////////////////////////////////////////////////////////////
    219 void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out) {
    220     Matrice passage;
    221     double conv=Geodesie::ConvMerApp(lambda);
    222     double c_=cos(conv);
    223     double s_=sin(conv);
    224 
    225     passage.c0_l0 = c_;
    226     passage.c0_l1 = s_;
    227     passage.c0_l2 =  0.0;
    228 
    229     passage.c1_l0 = -s_;
    230     passage.c1_l1 = c_;
    231     passage.c1_l2 =  0.0;
    232 
    233     passage.c2_l0 =  0.0;
    234     passage.c2_l1 =  0.0;
    235     passage.c2_l2 =  1.0;
    236 
    237     out=ProdMat(passage,in);
    238     double diff_h;
    239     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    240     h=he-diff_h;
    241 
    242     Geodesie::Geo2ProjLambert(
    243                 lambda,phi,
    244                 n_Lambert93,c_Lambert93,e_Lambert93,
    245                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    246                 E,N);
     225void Geodesie::Geographique_2_Lambert93(const Raf98 &raf98, double lambda,
     226                                        double phi, double he, Matrice in,
     227                                        double &E, double &N, double &h,
     228                                        Matrice &out) {
     229  Matrice passage;
     230  double conv = Geodesie::ConvMerApp(lambda);
     231  double c_ = cos(conv);
     232  double s_ = sin(conv);
     233
     234  passage.c0_l0 = c_;
     235  passage.c0_l1 = s_;
     236  passage.c0_l2 = 0.0;
     237
     238  passage.c1_l0 = -s_;
     239  passage.c1_l1 = c_;
     240  passage.c1_l2 = 0.0;
     241
     242  passage.c2_l0 = 0.0;
     243  passage.c2_l1 = 0.0;
     244  passage.c2_l2 = 1.0;
     245
     246  out = ProdMat(passage, in);
     247  double diff_h;
     248  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     249  h = he - diff_h;
     250
     251  Geodesie::Geo2ProjLambert(lambda, phi, n_Lambert93, c_Lambert93, e_Lambert93,
     252                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93, E,
     253                            N);
    247254}
    248255////////////////////////////////////////////////////////////////////////
    249 void Geodesie::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h) {
    250     Geodesie::Geo2ProjLambert(
    251                 lambda,phi,
    252                 n_Lambert93,c_Lambert93,e_Lambert93,
    253                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    254                 E,N);
    255 
    256     double diff_h;
    257     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    258     h=he-diff_h;
     256void Geodesie::Geographique_2_Lambert93(const Raf98 &raf98, double lambda,
     257                                        double phi, double he, double &E,
     258                                        double &N, double &h) {
     259  Geodesie::Geo2ProjLambert(lambda, phi, n_Lambert93, c_Lambert93, e_Lambert93,
     260                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93, E,
     261                            N);
     262
     263  double diff_h;
     264  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     265  h = he - diff_h;
    259266}
    260267/**
    261     Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height)
     268    Converts Lambert93 coordinates (East, North, Height) into geographical
     269   coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi),
     270   Height)
    262271*/
    263 void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he) {
    264     Geodesie::Proj2GeoLambert(
    265                 E,N,
    266                 n_Lambert93,c_Lambert93,e_Lambert93,
    267                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    268                 0.0000000000000001,
    269                 lambda,phi);
    270 
    271     double diff_h;
    272     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    273     he=h+diff_h;
     272void Geodesie::Lambert93_2_Geographique(const Raf98 &raf98, double E, double N,
     273                                        double h, double &lambda, double &phi,
     274                                        double &he) {
     275  Geodesie::Proj2GeoLambert(E, N, n_Lambert93, c_Lambert93, e_Lambert93,
     276                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     277                            0.0000000000000001, lambda, phi);
     278
     279  double diff_h;
     280  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     281  he = h + diff_h;
    274282}
    275283////////////////////////////////////////////////////////////////////////
    276 void Geodesie::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out) {
    277     Geodesie::Proj2GeoLambert(
    278                 E,N,
    279                 n_Lambert93,c_Lambert93,e_Lambert93,
    280                 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,
    281                 0.0000000000000001,
    282                 lambda,phi);
    283 
    284     Matrice passage;
    285     double conv=Geodesie::ConvMerApp(lambda);
    286     double c_=cos(conv);
    287     double s_=sin(conv);
    288 
    289     passage.c0_l0 = c_;
    290     passage.c0_l1 = -s_;
    291     passage.c0_l2 =  0.0;
    292 
    293     passage.c1_l0 = s_;
    294     passage.c1_l1 = c_;
    295     passage.c1_l2 =  0.0;
    296 
    297     passage.c2_l0 =  0.0;
    298     passage.c2_l1 =  0.0;
    299     passage.c2_l2 =  1.0;
    300 
    301     out=ProdMat(passage,in);
    302 
    303     double diff_h;
    304     raf98.Interpol(Rad2Deg(lambda),Rad2Deg(phi),&diff_h);
    305     he=h+diff_h;
     284void Geodesie::Lambert93_2_Geographique(const Raf98 &raf98, double E, double N,
     285                                        double h, Matrice in, double &lambda,
     286                                        double &phi, double &he, Matrice &out) {
     287  Geodesie::Proj2GeoLambert(E, N, n_Lambert93, c_Lambert93, e_Lambert93,
     288                            lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
     289                            0.0000000000000001, lambda, phi);
     290
     291  Matrice passage;
     292  double conv = Geodesie::ConvMerApp(lambda);
     293  double c_ = cos(conv);
     294  double s_ = sin(conv);
     295
     296  passage.c0_l0 = c_;
     297  passage.c0_l1 = -s_;
     298  passage.c0_l2 = 0.0;
     299
     300  passage.c1_l0 = s_;
     301  passage.c1_l1 = c_;
     302  passage.c1_l2 = 0.0;
     303
     304  passage.c2_l0 = 0.0;
     305  passage.c2_l1 = 0.0;
     306  passage.c2_l2 = 1.0;
     307
     308  out = ProdMat(passage, in);
     309
     310  double diff_h;
     311  raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
     312  he = h + diff_h;
    306313}
    307314
    308315////////////////////////////////////////////////////////////////////////
    309 void Geodesie::Geographique_2_ECEF(double longitude,double latitude,double he,double& x,double& y,double& z) {
    310     const double n = GRS_a / sqrt(1.0 - pow(GRS_e,2) * pow(sin(latitude),2));
    311     x = (n + he) * cos(latitude) * cos(longitude);
    312     y = (n + he) * cos(latitude) * sin(longitude);
    313     z = (n * (1.0 - pow(GRS_e,2)) + he) * sin(latitude);
     316void Geodesie::Geographique_2_ECEF(double longitude, double latitude, double he,
     317                                   double &x, double &y, double &z) {
     318  const double n = GRS_a / sqrt(1.0 - pow(GRS_e, 2) * pow(sin(latitude), 2));
     319  x = (n + he) * cos(latitude) * cos(longitude);
     320  y = (n + he) * cos(latitude) * sin(longitude);
     321  z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude);
    314322}
    315323
    316324////////////////////////////////////////////////////////////////////////
    317 void Geodesie::ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0) {
    318     double slat = std::sin(lat0);
    319     double clat = std::cos(lat0);
    320     double slon = std::sin(lon0);
    321     double clon = std::cos(lon0);
    322 
    323     Geodesie::Matrice C;
    324     C.c0_l0 = -slon;
    325     C.c1_l0 = clon;
    326 
    327     C.c0_l1 = -clon * slat;
    328     C.c1_l1 = -slon * slat;
    329     C.c2_l1 = clat;
    330 
    331     C.c0_l2 = clon * clat;
    332     C.c1_l2 = slon * clat;
    333     C.c2_l2 = slat;
    334 
    335     double x0, y0, z0;
    336     Geographique_2_ECEF(lon0,lat0,he0, x0,y0,z0);
    337 
    338     x -= x0;
    339     y -= y0;
    340     z -= z0;
    341 
    342     C.Apply(x,y,z, e,n,u);
    343 }
     325void Geodesie::ECEF_2_ENU(double x, double y, double z, double &e, double &n,
     326                          double &u, double lon0, double lat0, double he0) {
     327  double slat = std::sin(lat0);
     328  double clat = std::cos(lat0);
     329  double slon = std::sin(lon0);
     330  double clon = std::cos(lon0);
     331
     332  Geodesie::Matrice C;
     333  C.c0_l0 = -slon;
     334  C.c1_l0 = clon;
     335
     336  C.c0_l1 = -clon * slat;
     337  C.c1_l1 = -slon * slat;
     338  C.c2_l1 = clat;
     339
     340  C.c0_l2 = clon * clat;
     341  C.c1_l2 = slon * clat;
     342  C.c2_l2 = slat;
     343
     344  double x0, y0, z0;
     345  Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);
     346
     347  x -= x0;
     348  y -= y0;
     349  z -= z0;
     350
     351  C.Apply(x, y, z, e, n, u);
     352}

  • trunk/lib/FlairSensorActuator/src/unexported/AfroBldc_impl.h

    r3 r15  
    1616/*********************************************************************/
    1717
    18 
    1918#ifndef AFROBLDC_IMPL_H
    2019#define AFROBLDC_IMPL_H
    … …  
    2524#define MAX_MOTORS 8
    2625
    27 namespace flair
    28 {
    29     namespace core
    30     {
    31         class I2cPort;
    32     }
    33     namespace gui
    34     {
    35         class SpinBox;
    36         class GroupBox;
    37         class Layout;
    38     }
    39     namespace actuator
    40     {
    41         class AfroBldc;
    42     }
    43     namespace sensor
    44     {
    45         class BatteryMonitor;
    46     }
     26namespace flair {
     27namespace core {
     28class I2cPort;
     29}
     30namespace gui {
     31class SpinBox;
     32class GroupBox;
     33class Layout;
     34}
     35namespace actuator {
     36class AfroBldc;
     37}
     38namespace sensor {
     39class BatteryMonitor;
     40}
    4741}
    4842
    49 class AfroBldc_impl
    50 {
    51     public:
    52         AfroBldc_impl(flair::actuator::AfroBldc* self,flair::gui::Layout *layout,flair::core::I2cPort* i2cport);
    53         ~AfroBldc_impl();
    54         void SetMotors(float* value);
     43class AfroBldc_impl {
     44public:
     45  AfroBldc_impl(flair::actuator::AfroBldc *self, flair::gui::Layout *layout,
     46                flair::core::I2cPort *i2cport);
     47  ~AfroBldc_impl();
     48  void SetMotors(float *value);
    5549
    56     private:
    57         void WriteValue(uint16_t value);//I2cPort mutex must be taken before calling this function
    58         int nb_mot;
    59         flair::core::I2cPort* i2cport;
    60         flair::actuator::AfroBldc* self;
     50private:
     51  void WriteValue(uint16_t value); // I2cPort mutex must be taken before calling
     52                                   // this function
     53  int nb_mot;
     54  flair::core::I2cPort *i2cport;
     55  flair::actuator::AfroBldc *self;
    6156};
    6257

  • trunk/lib/FlairSensorActuator/src/unexported/BlCtrlV2_impl.h

    r3 r15  
    1616/*********************************************************************/
    1717
    18 
    1918#ifndef BLCTRLV2_IMPL_H
    2019#define BLCTRLV2_IMPL_H
    … …  
    2524#define MAX_MOTORS 8
    2625
    27 namespace flair
    28 {
    29     namespace core
    30     {
    31         class I2cPort;
    32     }
    33     namespace gui
    34     {
    35         class SpinBox;
    36         class GroupBox;
    37         class Layout;
    38     }
    39     namespace actuator
    40     {
    41         class BlCtrlV2;
    42     }
    43     namespace sensor
    44     {
    45         class BatteryMonitor;
    46     }
     26namespace flair {
     27namespace core {
     28class I2cPort;
     29}
     30namespace gui {
     31class SpinBox;
     32class GroupBox;
     33class Layout;
     34}
     35namespace actuator {
     36class BlCtrlV2;
     37}
     38namespace sensor {
     39class BatteryMonitor;
     40}
    4741}
    4842
    49 class BlCtrlV2_impl
    50 {
    51     public:
    52         BlCtrlV2_impl(flair::actuator::BlCtrlV2* self,flair::gui::Layout *layout,flair::core::I2cPort* i2cport);
    53         ~BlCtrlV2_impl();
    54         void SetMotors(float* value);
    55         flair::sensor::BatteryMonitor *battery;
    56         flair::gui::SpinBox *poles;
     43class BlCtrlV2_impl {
     44public:
     45  BlCtrlV2_impl(flair::actuator::BlCtrlV2 *self, flair::gui::Layout *layout,
     46                flair::core::I2cPort *i2cport);
     47  ~BlCtrlV2_impl();
     48  void SetMotors(float *value);
     49  flair::sensor::BatteryMonitor *battery;
     50  flair::gui::SpinBox *poles;
    5751
    58     private:
    59         void WriteValue(uint16_t value);//I2cPort mutex must be taken before calling this function
    60         void DetectMotors(void);
    61         void GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage);//I2cPort mutex must be taken before calling this function
    62         void GetCurrentAndSpeed(float &current,float &speed);//I2cPort mutex must be taken before calling this function
    63         flair::core::Time last_voltage_time;
    64         int nb_mot;
    65         flair::core::I2cPort* i2cport;
    66         flair::actuator::BlCtrlV2* self;
     52private:
     53  void WriteValue(uint16_t value); // I2cPort mutex must be taken before calling
     54                                   // this function
     55  void DetectMotors(void);
     56  void GetCurrentSpeedAndVoltage(float &current, float &speed,
     57                                 float &voltage); // I2cPort mutex must be taken
     58                                                  // before calling this
     59                                                  // function
     60  void GetCurrentAndSpeed(
     61      float &current,
     62      float &speed); // I2cPort mutex must be taken before calling this function
     63  flair::core::Time last_voltage_time;
     64  int nb_mot;
     65  flair::core::I2cPort *i2cport;
     66  flair::actuator::BlCtrlV2 *self;
    6767};
    6868

  • trunk/lib/FlairSensorActuator/src/unexported/Bldc_impl.h

    r3 r15  
    2222#include <stdint.h>
    2323
    24 namespace flair
    25 {
    26     namespace gui
    27     {
    28         class DoubleSpinBox;
    29         class Layout;
    30         class Label;
    31         class DataPlot1D;
    32         class TabWidget;
    33         class PushButton;
    34     }
    35     namespace actuator
    36     {
    37         class Bldc;
    38     }
     24namespace flair {
     25namespace gui {
     26class DoubleSpinBox;
     27class Layout;
     28class Label;
     29class DataPlot1D;
     30class TabWidget;
     31class PushButton;
     32}
     33namespace actuator {
     34class Bldc;
     35}
    3936}
    4037
    41 class Bldc_impl
    42 {
    43     public:
    44         Bldc_impl(flair::actuator::Bldc* self,flair::gui::Layout* layout,std::string name,uint8_t motors_count);
    45         Bldc_impl(flair::actuator::Bldc* self,uint8_t motors_count);//simulation
    46         ~Bldc_impl();
    47         void UpdateFrom(const flair::core::io_data *data);
    48         void LockUserInterface(void) const;
    49         void UnlockUserInterface(void) const;
    50         bool are_enabled;
    51         float* power;
    52         void UseDefaultPlot(flair::gui::TabWidget* tab);
    53         uint8_t motors_count;
    54         flair::gui::Layout* layout;
     38class Bldc_impl {
     39public:
     40  Bldc_impl(flair::actuator::Bldc *self, flair::gui::Layout *layout,
     41            std::string name, uint8_t motors_count);
     42  Bldc_impl(flair::actuator::Bldc *self, uint8_t motors_count); // simulation
     43  ~Bldc_impl();
     44  void UpdateFrom(const flair::core::io_data *data);
     45  void LockUserInterface(void) const;
     46  void UnlockUserInterface(void) const;
     47  bool are_enabled;
     48  float *power;
     49  void UseDefaultPlot(flair::gui::TabWidget *tab);
     50  uint8_t motors_count;
     51  flair::gui::Layout *layout;
    5552
    56     private:
    57         float *values;
    58         float Sat(float value);
    59         flair::actuator::Bldc* self;
    60         flair::gui::DoubleSpinBox *min_value,*max_value,*test_value;
    61         flair::gui::Label *flight_time;
    62         flair::gui::DataPlot1D *plots;
    63         flair::core::Time flight_start_time;
    64         flair::gui::PushButton **button_test;
    65         int time_sec;
    66         bool is_running;
    67         flair::core::Time test_start_time;
    68         int tested_motor;//=-1 if no motor is tested
     53private:
     54  float *values;
     55  float Sat(float value);
     56  flair::actuator::Bldc *self;
     57  flair::gui::DoubleSpinBox *min_value, *max_value, *test_value;
     58  flair::gui::Label *flight_time;
     59  flair::gui::DataPlot1D *plots;
     60  flair::core::Time flight_start_time;
     61  flair::gui::PushButton **button_test;
     62  int time_sec;
     63  bool is_running;
     64  flair::core::Time test_start_time;
     65  int tested_motor; //=-1 if no motor is tested
    6966};
    7067

  • trunk/lib/FlairSensorActuator/src/unexported/Gx3_25_imu_impl.h

    r3 r15  
    1818
    1919namespace flair {
    20     namespace core {
    21         class FrameworkManager;
    22         class SerialPort;
    23         class AhrsData;
    24     }
    25     namespace gui {
    26         class SpinBox;
    27         class CheckBox;
    28         class PushButton;
    29         class Label;
    30     }
     20namespace core {
     21class FrameworkManager;
     22class SerialPort;
     23class AhrsData;
     24}
     25namespace gui {
     26class SpinBox;
     27class CheckBox;
     28class PushButton;
     29class Label;
     30}
    3131}
    3232
    … …  
    3838class Gx3_25_imu_impl {
    3939
    40     public:
    41         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);
    42         ~Gx3_25_imu_impl();
    43         void Run(void);
     40public:
     41  Gx3_25_imu_impl(flair::sensor::Gx3_25_imu *self, std::string name,
     42                  flair::core::SerialPort *serialport,
     43                  flair::sensor::Gx3_25_imu::Command_t command,
     44                  flair::gui::GroupBox *setupgroupbox);
     45  ~Gx3_25_imu_impl();
     46  void Run(void);
    4447
    45     private:
    46         void DeviceReset(void);
    47         void GetData(uint8_t* buf,ssize_t buf_size,flair::core::Time *time);
    48         float Dequeue(uint8_t** buf);
    49         void GyrosBias(void);
    50         void SamplingSettings(void);
    51         void SetBaudrate(int value);
    52         bool CalcChecksum(uint8_t *buf,int size);
    53         int FirmwareNumber(void);
    54         void PrintModelInfo(void);
    55         void RealignUpNorth(bool realign_up,bool realign_north);
     48private:
     49  void DeviceReset(void);
     50  void GetData(uint8_t *buf, ssize_t buf_size, flair::core::Time *time);
     51  float Dequeue(uint8_t **buf);
     52  void GyrosBias(void);
     53  void SamplingSettings(void);
     54  void SetBaudrate(int value);
     55  bool CalcChecksum(uint8_t *buf, int size);
     56  int FirmwareNumber(void);
     57  void PrintModelInfo(void);
     58  void RealignUpNorth(bool realign_up, bool realign_north);
    5659
    57         void SetContinuousMode(uint8_t continuous_command);
     60  void SetContinuousMode(uint8_t continuous_command);
    5861
    59         flair::gui::GroupBox *setupgroupbox;
    60         flair::gui::SpinBox *data_rate,*gyro_acc_size,*mag_size,*up_comp,*north_comp;
    61         flair::gui::CheckBox *coning,*disable_magn,*disable_north_comp,*disable_grav_comp;
    62         flair::gui::PushButton *button_bias;
    63         flair::gui::Label *data_rate_label;
     62  flair::gui::GroupBox *setupgroupbox;
     63  flair::gui::SpinBox *data_rate, *gyro_acc_size, *mag_size, *up_comp,
     64      *north_comp;
     65  flair::gui::CheckBox *coning, *disable_magn, *disable_north_comp,
     66      *disable_grav_comp;
     67  flair::gui::PushButton *button_bias;
     68  flair::gui::Label *data_rate_label;
    6469
    65         flair::core::SerialPort *serialport;
    66         uint8_t command;
    67         flair::sensor::Gx3_25_imu *self;
    68         flair::core::AhrsData *ahrsData;
     70  flair::core::SerialPort *serialport;
     71  uint8_t command;
     72  flair::sensor::Gx3_25_imu *self;
     73  flair::core::AhrsData *ahrsData;
    6974};
    7075

  • trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h

    r3 r15  
    2222#include <vector>
    2323
    24 namespace flair
    25 {
    26     namespace core
    27     {
    28         class OneAxisRotation;
    29         class Vector3D;
    30         class Quaternion;
    31         class Mutex;
    32         class SerialPort;
    33     }
    34     namespace gui
    35     {
    36         class TabWidget;
    37         class Tab;
    38         class Layout;
    39     }
    40     namespace sensor
    41     {
    42         class VrpnClient;
    43         class VrpnObject;
    44     }
     24namespace flair {
     25namespace core {
     26class OneAxisRotation;
     27class Vector3D;
     28class Quaternion;
     29class Mutex;
     30class SerialPort;
     31}
     32namespace gui {
     33class TabWidget;
     34class Tab;
     35class Layout;
     36}
     37namespace sensor {
     38class VrpnClient;
     39class VrpnObject;
     40}
    4541}
    4642
    … …  
    4844class vrpn_Connection;
    4945
    50 class VrpnClient_impl
    51 {
    52     public:
    53         VrpnClient_impl(flair::sensor::VrpnClient* self,std::string name,std::string address,uint16_t us_period);
    54         VrpnClient_impl(flair::sensor::VrpnClient* self,std::string name,flair::core::SerialPort* serialport,uint16_t us_period);
    55         ~VrpnClient_impl();
    56         void AddTrackable(flair::sensor::VrpnObject* obj);//normal
    57         void RemoveTrackable(flair::sensor::VrpnObject* obj);//normal
    58         void AddTrackable(VrpnObject_impl* obj,uint8_t id);//xbee
    59         void RemoveTrackable(VrpnObject_impl* obj);//xbee
    60         void ComputeRotations(flair::core::Vector3D& point);
    61         void ComputeRotations(flair::core::Quaternion& quat);
    62         bool UseXbee(void);
    63         void Run(void);
    64         flair::gui::Tab* setup_tab;
    65         flair::gui::TabWidget* tab;
    66         vrpn_Connection *connection;
     46class VrpnClient_impl {
     47public:
     48  VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
     49                  std::string address, uint16_t us_period);
     50  VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
     51                  flair::core::SerialPort *serialport, uint16_t us_period);
     52  ~VrpnClient_impl();
     53  void AddTrackable(flair::sensor::VrpnObject *obj); // normal
     54  void RemoveTrackable(flair::sensor::VrpnObject *obj); // normal
     55  void AddTrackable(VrpnObject_impl *obj, uint8_t id); // xbee
     56  void RemoveTrackable(VrpnObject_impl *obj); // xbee
     57  void ComputeRotations(flair::core::Vector3D &point);
     58  void ComputeRotations(flair::core::Quaternion &quat);
     59  bool UseXbee(void);
     60  void Run(void);
     61  flair::gui::Tab *setup_tab;
     62  flair::gui::TabWidget *tab;
     63  vrpn_Connection *connection;
    6764
    68     private:
    69         flair::sensor::VrpnClient* self;
    70         flair::core::Mutex* mutex;
    71         uint16_t us_period;
    72         std::vector<flair::sensor::VrpnObject*> trackables;
    73         typedef struct xbee_object{
    74             VrpnObject_impl* vrpnobject;
    75             uint8_t id;
    76         } xbee_object;
     65private:
     66  flair::sensor::VrpnClient *self;
     67  flair::core::Mutex *mutex;
     68  uint16_t us_period;
     69  std::vector<flair::sensor::VrpnObject *> trackables;
     70  typedef struct xbee_object {
     71    VrpnObject_impl *vrpnobject;
     72    uint8_t id;
     73  } xbee_object;
    7774
    78         std::vector<xbee_object> xbee_objects;
    79         flair::gui::Tab* main_tab;
    80         flair::core::OneAxisRotation *rotation_1,*rotation_2;
    81         flair::core::SerialPort* serialport;
     75  std::vector<xbee_object> xbee_objects;
     76  flair::gui::Tab *main_tab;
     77  flair::core::OneAxisRotation *rotation_1, *rotation_2;
     78  flair::core::SerialPort *serialport;
    8279};
    8380

  • trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h

    r3 r15  
    2525#include "Quaternion.h"
    2626
    27 namespace flair
    28 {
    29     namespace core
    30     {
    31         class cvmatrix;
    32         class Vector3D;
    33         class Euler;
    34     }
    35     namespace gui
    36     {
    37         class TabWidget;
    38         class Tab;
    39         class DataPlot1D;
    40     }
    41     namespace sensor
    42     {
    43         class VrpnClient;
    44         class VrpnObject;
    45     }
     27namespace flair {
     28namespace core {
     29class cvmatrix;
     30class Vector3D;
     31class Euler;
     32}
     33namespace gui {
     34class TabWidget;
     35class Tab;
     36class DataPlot1D;
     37}
     38namespace sensor {
     39class VrpnClient;
     40class VrpnObject;
     41}
    4642}
    4743
    48 class VrpnObject_impl
    49 {
    50     friend class VrpnClient_impl;
     44class VrpnObject_impl {
     45  friend class VrpnClient_impl;
    5146
    52     public:
    53         VrpnObject_impl(flair::sensor::VrpnObject* self,const flair::sensor::VrpnClient *parent,std::string name,int id,const flair::gui::TabWidget* tab);
    54         ~VrpnObject_impl(void);
     47public:
     48  VrpnObject_impl(flair::sensor::VrpnObject *self,
     49                  const flair::sensor::VrpnClient *parent, std::string name,
     50                  int id, const flair::gui::TabWidget *tab);
     51  ~VrpnObject_impl(void);
    5552
    56         void mainloop(void);
    57         void GetEuler(flair::core::Euler &euler);
    58         void GetQuaternion(flair::core::Quaternion &quaternion);
    59         void GetPosition(flair::core::Vector3D &point);
    60         bool IsTracked(unsigned int timeout_ms);
     53  void mainloop(void);
     54  void GetEuler(flair::core::Euler &euler);
     55  void GetQuaternion(flair::core::Quaternion &quaternion);
     56  void GetPosition(flair::core::Vector3D &point);
     57  bool IsTracked(unsigned int timeout_ms);
    6158
    62         flair::gui::Tab* plot_tab;
    63         flair::gui::DataPlot1D* x_plot;
    64         flair::gui::DataPlot1D* y_plot;
    65         flair::gui::DataPlot1D* z_plot;
    66         flair::core::cvmatrix *output,*state;
     59  flair::gui::Tab *plot_tab;
     60  flair::gui::DataPlot1D *x_plot;
     61  flair::gui::DataPlot1D *y_plot;
     62  flair::gui::DataPlot1D *z_plot;
     63  flair::core::cvmatrix *output, *state;
    6764
    68         static void     VRPN_CALLBACK handle_pos(void *userdata, const vrpn_TRACKERCB t);
     65  static void VRPN_CALLBACK handle_pos(void *userdata, const vrpn_TRACKERCB t);
    6966
    70     private:
    71         flair::sensor::VrpnObject* self;
    72         const flair::sensor::VrpnClient *parent;
    73         vrpn_Tracker_Remote* tracker;
    74         flair::core::Quaternion quaternion;//todo: quaternion should be included in the output to replace euler angles
    75         void Update(void);
     67private:
     68  flair::sensor::VrpnObject *self;
     69  const flair::sensor::VrpnClient *parent;
     70  vrpn_Tracker_Remote *tracker;
     71  flair::core::Quaternion quaternion; // todo: quaternion should be included in
     72                                      // the output to replace euler angles
     73  void Update(void);
    7674};
    7775

  • trunk/lib/FlairSensorActuator/src/unexported/XBldc_impl.h

    r3 r15  
    1616/*********************************************************************/
    1717
    18 
    1918#ifndef XBLDC_IMPL_H
    2019#define XBLDC_IMPL_H
    … …  
    2322#include <stdint.h>
    2423
    25 namespace flair
    26 {
    27     namespace core
    28     {
    29         class I2cPort;
    30     }
    31     namespace actuator
    32     {
    33         class XBldc;
    34     }
     24namespace flair {
     25namespace core {
     26class I2cPort;
     27}
     28namespace actuator {
     29class XBldc;
     30}
    3531}
    3632
     33class XBldc_impl {
    3734
    38 class XBldc_impl
    39 {
     35public:
     36  XBldc_impl(flair::actuator::XBldc *self, flair::core::I2cPort *i2cport);
     37  ~XBldc_impl();
     38  void UpdateFrom(flair::core::io_data *data);
     39  void SetMotors(float *value);
    4040
    41     public:
    42         XBldc_impl(flair::actuator::XBldc* self,flair::core::I2cPort* i2cport);
    43         ~XBldc_impl();
    44         void UpdateFrom(flair::core::io_data *data);
    45         void SetMotors(float* value);
    46 
    47     private:
    48         uint8_t Sat(float value,uint8_t min,uint8_t max);
    49         //void StartTest(uint8_t moteur);
    50         void ChangeDirection(uint8_t moteur);
    51         void ChangeAdress(uint8_t old_add,uint8_t new_add);
    52         flair::actuator::XBldc* self;
    53         flair::core::I2cPort* i2cport;
     41private:
     42  uint8_t Sat(float value, uint8_t min, uint8_t max);
     43  // void StartTest(uint8_t moteur);
     44  void ChangeDirection(uint8_t moteur);
     45  void ChangeAdress(uint8_t old_add, uint8_t new_add);
     46  flair::actuator::XBldc *self;
     47  flair::core::I2cPort *i2cport;
    5448};
    5549

  • trunk/lib/FlairSensorActuator/src/unexported/geodesie.h

    r3 r15  
    1313
    1414#ifndef M_PI
    15 #   define M_PI 3.14159265358979323846
     15#define M_PI 3.14159265358979323846
    1616#endif
    1717#ifndef M_PI_2
    18 #   define M_PI_2 1.57079632679489661923
     18#define M_PI_2 1.57079632679489661923
    1919#endif
    2020#ifndef M_PI_4
    21 #   define M_PI_4 0.78539816339744830962
     21#define M_PI_4 0.78539816339744830962
    2222#endif
    2323
    2424////////////////////////////////////////////////////////////////////////
    2525struct Matrice {
    26     Matrice(const Matrice & A);
    27     Matrice();
    28     void Apply(double v0, double v1, double v2, double & Mv0, double & Mv1, double & Mv2);
    29     double c0_l0;double c1_l0;double c2_l0;
    30     double c0_l1;double c1_l1;double c2_l1;
    31     double c0_l2;double c1_l2;double c2_l2;
     26  Matrice(const Matrice &A);
     27  Matrice();
     28  void Apply(double v0, double v1, double v2, double &Mv0, double &Mv1,
     29             double &Mv2);
     30  double c0_l0;
     31  double c1_l0;
     32  double c2_l0;
     33  double c0_l1;
     34  double c1_l1;
     35  double c2_l1;
     36  double c0_l2;
     37  double c1_l2;
     38  double c2_l2;
    3239}; // class
    3340
    3441Matrice TransMat(const Matrice A);
    3542
    36 Matrice ProdMat(const Matrice A,const Matrice B);
    37 void Write(const Matrice A,std::ostream& out);
     43Matrice ProdMat(const Matrice A, const Matrice B);
     44void Write(const Matrice A, std::ostream &out);
    3845
    3946////////////////////////////////////////////////////////////////////////
    4047class Raf98 {
    41 private :
    42     std::vector<double> m_dvalues;
    43     double LitGrille(unsigned int c,unsigned int l) const;
    44 public :
    45     ~Raf98();
    46     Raf98() {}
    47     bool Load(const std::string & s);
    48     bool Interpol(double longitude/*deg*/, double latitude/*deg*/, double* Hwgs84) const;
     48private:
     49  std::vector<double> m_dvalues;
     50  double LitGrille(unsigned int c, unsigned int l) const;
     51
     52public:
     53  ~Raf98();
     54  Raf98() {}
     55  bool Load(const std::string &s);
     56  bool Interpol(double longitude /*deg*/, double latitude /*deg*/,
     57                double *Hwgs84) const;
    4958}; // class
    5059////////////////////////////////////////////////////////////////////////
    5160
    5261////////////////////////////////////////////////////////////////////////
    53 inline double Deg2Rad(double deg) {return deg*M_PI/180.0;}
    54 inline double Rad2Deg(double rad) {return rad*180.0/M_PI;}
     62inline double Deg2Rad(double deg) { return deg * M_PI / 180.0; }
     63inline double Rad2Deg(double rad) { return rad * 180.0 / M_PI; }
    5564////////////////////////////////////////////////////////////////////////
    5665
    57 const double a_Lambert93=6378137;
    58 const double f_Lambert93=1 / 298.257222101;
    59 const double e_Lambert93=sqrt(f_Lambert93*(2-f_Lambert93));
    60 const double lambda0_Lambert93=Deg2Rad(3.0);//degres
    61 const double phi0_Lambert93=Deg2Rad(46.5);
    62 const double phi1_Lambert93=Deg2Rad(44.0);
    63 const double phi2_Lambert93=Deg2Rad(49.0);//degres
    64 const double X0_Lambert93=700000;//
    65 const double Y0_Lambert93=6600000;//
     66const double a_Lambert93 = 6378137;
     67const double f_Lambert93 = 1 / 298.257222101;
     68const double e_Lambert93 = sqrt(f_Lambert93 * (2 - f_Lambert93));
     69const double lambda0_Lambert93 = Deg2Rad(3.0); // degres
     70const double phi0_Lambert93 = Deg2Rad(46.5);
     71const double phi1_Lambert93 = Deg2Rad(44.0);
     72const double phi2_Lambert93 = Deg2Rad(49.0); // degres
     73const double X0_Lambert93 = 700000;  //
     74const double Y0_Lambert93 = 6600000; //
    6675const double n_Lambert93 = 0.7256077650;
    6776const double c_Lambert93 = 11754255.426;
    … …  
    7079
    7180const double GRS_a = 6378137;
    72 const double GRS_f = 1/298.257222101;
    73 const double GRS_b = GRS_a*(1-GRS_f);
    74 const double GRS_e = sqrt((pow(GRS_a,2) - pow(GRS_b,2)) / pow(GRS_a,2));
     81const double GRS_f = 1 / 298.257222101;
     82const double GRS_b = GRS_a * (1 - GRS_f);
     83const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b, 2)) / pow(GRS_a, 2));
    7584
    7685////////////////////////////////////////////////////////////////////////
    77 void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out);
    78 void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h);
    79 void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he);
    80 void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out);
     86void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi,
     87                              double he, Matrice in, double &E, double &N,
     88                              double &h, Matrice &out);
     89void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi,
     90                              double he, double &E, double &N, double &h);
     91void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h,
     92                              double &lambda, double &phi, double &he);
     93void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h,
     94                              Matrice in, double &lambda, double &phi,
     95                              double &he, Matrice &out);
    8196/** Convert from geographique to ECEF.
    8297 * @param[in] longitude Longitude in radian.
    … …  
    8499 * @param[in] he Height in meter.
    85100 */
    86 void Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z);
     101void Geographique_2_ECEF(double longitude, double latitude, double he,
     102                         double &x, double &y, double &z);
    87103/** Convert from ECEF two ENU.
    88104 * @param[in] lon0 Longitude of the origin in radian.
    … …  
    90106 * @param[in] he0 Height of the origin in radian.
    91107 */
    92 void ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0);
     108void ECEF_2_ENU(double x, double y, double z, double &e, double &n, double &u,
     109                double lon0, double lat0, double he0);
    93110////////////////////////////////////////////////////////////////////////
    94111
    95 //ALGO0001
    96 double LatitueIsometrique(double latitude,double e);
    97 //ALGO0002
    98 double LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon);
     112// ALGO0001
     113double LatitueIsometrique(double latitude, double e);
     114// ALGO0002
     115double LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon);
    99116
    100 //ALGO0003
    101 void Geo2ProjLambert(
    102     double lambda,double phi,
    103     double n, double c,double e,
    104     double lambdac,double xs,double ys,
    105     double& X,double& Y);
    106 //ALGO0004
    107 void Proj2GeoLambert(
    108     double X,double Y,
    109     double n, double c,double e,
    110     double lambdac,double xs,double ys,
    111     double epsilon,
    112     double& lambda,double& phi);
     117// ALGO0003
     118void Geo2ProjLambert(double lambda, double phi, double n, double c, double e,
     119                     double lambdac, double xs, double ys, double &X,
     120                     double &Y);
     121// ALGO0004
     122void Proj2GeoLambert(double X, double Y, double n, double c, double e,
     123                     double lambdac, double xs, double ys, double epsilon,
     124                     double &lambda, double &phi);
    113125
    114126double ConvMerApp(double longitude);
    … …  
    118130*/
    119131template <typename _T1, typename _T2>
    120 void cartesianToPolar(const _T1 x, const _T1 y, _T2 & r, _T2 & theta) {
    121     r = std::sqrt(x*x + y*y);
    122     theta = std::atan2(x, y);
     132void cartesianToPolar(const _T1 x, const _T1 y, _T2 &r, _T2 &theta) {
     133  r = std::sqrt(x * x + y * y);
     134  theta = std::atan2(x, y);
    123135}
    124136
    … …  
    127139*/
    128140template <typename _T1, typename _T2>
    129 void polarToCartesian(const _T1 r, const _T1 theta, _T2 & x, _T2 & y) {
    130     x = r * std::cos(theta);
    131     y = r * std::sin(theta);
     141void polarToCartesian(const _T1 r, const _T1 theta, _T2 &x, _T2 &y) {
     142  x = r * std::cos(theta);
     143  y = r * std::sin(theta);
    132144}
    133145
    134146/**
    135 Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta, phi)
     147Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta,
     148phi)
    136149Angles expressed in radians.
    137150*/
    138151template <typename _T1, typename _T2>
    139 void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 & r, _T2 & theta, _T2 & phi) {
    140     r = std::sqrt(x*x + y*y + z*z);
    141     theta = std::acos(z / r);
    142     phi = std::atan2(y, x);
     152void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 &r,
     153                          _T2 &theta, _T2 &phi) {
     154  r = std::sqrt(x * x + y * y + z * z);
     155  theta = std::acos(z / r);
     156  phi = std::atan2(y, x);
    143157}
    144158
    145159/**
    146 Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z) coordinates.
     160Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z)
     161coordinates.
    147162Angles expressed in radians.
    148163*/
    149164template <typename _T1, typename _T2>
    150 void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 & x, _T2 & y, _T2 & z) {
    151     x = r * std::sin(theta) * std::cos(phi);
    152     y = r * std::sin(theta) * std::sin(phi);
    153     z = r * std::cos(theta);
     165void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 &x,
     166                          _T2 &y, _T2 &z) {
     167  x = r * std::sin(theta) * std::cos(phi);
     168  y = r * std::sin(theta) * std::sin(phi);
     169  z = r * std::cos(theta);
    154170}
    155171
Note: See TracChangeset for help on using the changeset viewer.