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


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (8 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