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


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/FlairFilter/src
Files:
54 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairFilter/src/Ahrs.cpp

    r10 r15  
    2727using namespace flair::sensor;
    2828
    29 namespace flair { namespace filter {
     29namespace flair {
     30namespace filter {
    3031
    31 Ahrs::Ahrs(const Imu* parent,string name) : IODevice(parent,name) {
    32     pimpl_=new Ahrs_impl(this);
    33     AddDataToLog(pimpl_->ahrsData);
     32Ahrs::Ahrs(const Imu *parent, string name) : IODevice(parent, name) {
     33  pimpl_ = new Ahrs_impl(this);
     34  AddDataToLog(pimpl_->ahrsData);
    3435}
    3536
    36 Ahrs::~Ahrs() {
    37     delete pimpl_;
    38 }
     37Ahrs::~Ahrs() { delete pimpl_; }
    3938
    40 const Imu *Ahrs::GetImu(void) const {
    41     return (Imu*)Parent();
    42 }
     39const Imu *Ahrs::GetImu(void) const { return (Imu *)Parent(); }
    4340
    44 const AhrsData *Ahrs::GetDatas(void) const {
    45     return pimpl_->ahrsData;
    46 }
     41const AhrsData *Ahrs::GetDatas(void) const { return pimpl_->ahrsData; }
    4742
    4843void Ahrs::GetDatas(core::AhrsData **ahrsData) const {
    49     *ahrsData=pimpl_->ahrsData;
     44  *ahrsData = pimpl_->ahrsData;
    5045}
    5146
    5247void Ahrs::LockUserInterface(void) const {
    53     //((Imu*)Parent())->LockUserInterface();
     48  //((Imu*)Parent())->LockUserInterface();
    5449}
    5550
    5651void Ahrs::UnlockUserInterface(void) const {
    57     //((Imu*)Parent())->UnlockUserInterface();
     52  //((Imu*)Parent())->UnlockUserInterface();
    5853}
    5954
    6055void Ahrs::UseDefaultPlot(void) {
    61     pimpl_->UseDefaultPlot();
    62     ((Imu*)Parent())->UseDefaultPlot();
     56  pimpl_->UseDefaultPlot();
     57  ((Imu *)Parent())->UseDefaultPlot();
    6358}
    6459
    65 void Ahrs::AddPlot(const AhrsData *ahrsData,DataPlot::Color_t color) {
    66     pimpl_->AddPlot(ahrsData,color);
     60void Ahrs::AddPlot(const AhrsData *ahrsData, DataPlot::Color_t color) {
     61  pimpl_->AddPlot(ahrsData, color);
    6762}
    6863
    69 DataPlot1D *Ahrs::RollPlot(void) const {
    70     return pimpl_->rollPlot;
    71 }
     64DataPlot1D *Ahrs::RollPlot(void) const { return pimpl_->rollPlot; }
    7265
    73 DataPlot1D *Ahrs::PitchPlot(void) const {
    74     return pimpl_->pitchPlot;
    75 }
     66DataPlot1D *Ahrs::PitchPlot(void) const { return pimpl_->pitchPlot; }
    7667
    77 DataPlot1D *Ahrs::YawPlot(void) const {
    78     return pimpl_->yawPlot;
    79 }
     68DataPlot1D *Ahrs::YawPlot(void) const { return pimpl_->yawPlot; }
    8069
    81 DataPlot1D *Ahrs::WXPlot(void) const {
    82     return pimpl_->wXPlot;
    83 }
     70DataPlot1D *Ahrs::WXPlot(void) const { return pimpl_->wXPlot; }
    8471
    85 DataPlot1D *Ahrs::WYPlot(void) const {
    86     return pimpl_->wYPlot;
    87 }
     72DataPlot1D *Ahrs::WYPlot(void) const { return pimpl_->wYPlot; }
    8873
    89 DataPlot1D *Ahrs::WZPlot(void) const {
    90     return pimpl_->wZPlot;
    91 }
     74DataPlot1D *Ahrs::WZPlot(void) const { return pimpl_->wZPlot; }
    9275
    9376} // end namespace filter

  • trunk/lib/FlairFilter/src/Ahrs.h

    r10 r15  
    1818
    1919namespace flair {
    20     namespace core {
    21         class Euler;
    22         class Vector3D;
    23         class ImuData;
    24         class Quaternion;
    25         class AhrsData;
    26     }
    27     namespace gui {
    28         class Tab;
    29         class DataPlot1D;
    30     }
    31     namespace sensor {
    32         class Imu;
    33     }
     20namespace core {
     21class Euler;
     22class Vector3D;
     23class ImuData;
     24class Quaternion;
     25class AhrsData;
     26}
     27namespace gui {
     28class Tab;
     29class DataPlot1D;
     30}
     31namespace sensor {
     32class Imu;
     33}
    3434}
    3535
    3636class Ahrs_impl;
    3737
    38 namespace flair { namespace filter {
    39     /*! \class Ahrs
    40     *
    41     * \brief Abstract class for AHRS
    42     *
    43     * Use this class to define a custom AHRS. This class is child
    44     * of an Imu class, which will provide measurements. \n
    45     *
    46     */
    47     class Ahrs : public core::IODevice {
    48         public:
    49             /*!
    50             * \brief Constructor
    51             *
    52             * Construct an Ahrs.
    53             *
    54             * \param parent parent
    55             * \param name name
    56             */
    57             Ahrs(const sensor::Imu* parent,std::string name);
     38namespace flair {
     39namespace filter {
     40/*! \class Ahrs
     41*
     42* \brief Abstract class for AHRS
     43*
     44* Use this class to define a custom AHRS. This class is child
     45* of an Imu class, which will provide measurements. \n
     46*
     47*/
     48class Ahrs : public core::IODevice {
     49public:
     50  /*!
     51  * \brief Constructor
     52  *
     53  * Construct an Ahrs.
     54  *
     55  * \param parent parent
     56  * \param name name
     57  */
     58  Ahrs(const sensor::Imu *parent, std::string name);
    5859
    59             /*!
    60             * \brief Destructor
    61             *
    62             */
    63             ~Ahrs();
     60  /*!
     61  * \brief Destructor
     62  *
     63  */
     64  ~Ahrs();
    6465
    65             /*!
    66             * \brief Get parent Imu
    67             *
    68             * This function is identical to (Imu*)Parent()
    69             */
    70             const sensor::Imu *GetImu(void) const;
     66  /*!
     67  * \brief Get parent Imu
     68  *
     69  * This function is identical to (Imu*)Parent()
     70  */
     71  const sensor::Imu *GetImu(void) const;
    7172
    72             /*!
    73             * \brief Get ahrs datas
    74             *
    75             * \return AhrsData
    76             */
    77             const core::AhrsData *GetDatas(void) const;
     73  /*!
     74  * \brief Get ahrs datas
     75  *
     76  * \return AhrsData
     77  */
     78  const core::AhrsData *GetDatas(void) const;
    7879
    79             /*!
    80             * \brief Lock the graphical user interface
    81             *
    82             * When locked, parameters cannot be modified.
    83             *
    84             */
    85             void LockUserInterface(void) const;
     80  /*!
     81  * \brief Lock the graphical user interface
     82  *
     83  * When locked, parameters cannot be modified.
     84  *
     85  */
     86  void LockUserInterface(void) const;
    8687
    87             /*!
    88             * \brief Unlock the graphical user interface
    89             *
    90             */
    91             void UnlockUserInterface(void) const;
     88  /*!
     89  * \brief Unlock the graphical user interface
     90  *
     91  */
     92  void UnlockUserInterface(void) const;
    9293
    93             /*!
    94             * \brief Use default plot
    95             *
    96             * Plot the datas defined in imudata,
    97             * and datas defined in Imu::imudata.
    98             *
    99             */
    100             void UseDefaultPlot(void);
     94  /*!
     95  * \brief Use default plot
     96  *
     97  * Plot the datas defined in imudata,
     98  * and datas defined in Imu::imudata.
     99  *
     100  */
     101  void UseDefaultPlot(void);
    101102
    102             /*!
    103             * \brief Add plot
    104             *
    105             * Add plot of an AhrsData to the default plot
    106             *
    107             * \param ahrsData ahrs datas to plot
    108             * \param color color to use
    109             */
    110             void AddPlot(const core::AhrsData *ahrsData,gui::DataPlot::Color_t color);
     103  /*!
     104  * \brief Add plot
     105  *
     106  * Add plot of an AhrsData to the default plot
     107  *
     108  * \param ahrsData ahrs datas to plot
     109  * \param color color to use
     110  */
     111  void AddPlot(const core::AhrsData *ahrsData, gui::DataPlot::Color_t color);
    111112
    112             /*!
    113             * \brief Roll plot
    114             *
    115             * Use this plot to add own curves.
    116             *
    117             * \return plot
    118             *
    119             */
    120             gui::DataPlot1D *RollPlot(void)const ;
     113  /*!
     114  * \brief Roll plot
     115  *
     116  * Use this plot to add own curves.
     117  *
     118  * \return plot
     119  *
     120  */
     121  gui::DataPlot1D *RollPlot(void) const;
    121122
    122             /*!
    123             * \brief Pitch plot
    124             *
    125             * Use this plot to add own curves.
    126             *
    127             * \return plot
    128             *
    129             */
    130             gui::DataPlot1D *PitchPlot(void) const;
     123  /*!
     124  * \brief Pitch plot
     125  *
     126  * Use this plot to add own curves.
     127  *
     128  * \return plot
     129  *
     130  */
     131  gui::DataPlot1D *PitchPlot(void) const;
    131132
    132             /*!
    133             * \brief Yaw plot
    134             *
    135             * Use this plot to add own curves.
    136             *
    137             * \return plot
    138             *
    139             */
    140             gui::DataPlot1D *YawPlot(void) const;
     133  /*!
     134  * \brief Yaw plot
     135  *
     136  * Use this plot to add own curves.
     137  *
     138  * \return plot
     139  *
     140  */
     141  gui::DataPlot1D *YawPlot(void) const;
    141142
    142             /*!
    143             * \brief Rotation speed around x axis plot
    144             *
    145             * Use this plot to add own curves.
    146             *
    147             * \return plot
    148             *
    149             */
    150             gui::DataPlot1D *WXPlot(void) const;
     143  /*!
     144  * \brief Rotation speed around x axis plot
     145  *
     146  * Use this plot to add own curves.
     147  *
     148  * \return plot
     149  *
     150  */
     151  gui::DataPlot1D *WXPlot(void) const;
    151152
    152             /*!
    153             * \brief Rotation speed around y axis plot
    154             *
    155             * Use this plot to add own curves.
    156             *
    157             * \return plot
    158             *
    159             */
    160             gui::DataPlot1D *WYPlot(void) const;
     153  /*!
     154  * \brief Rotation speed around y axis plot
     155  *
     156  * Use this plot to add own curves.
     157  *
     158  * \return plot
     159  *
     160  */
     161  gui::DataPlot1D *WYPlot(void) const;
    161162
    162             /*!
    163             * \brief Rotation speed around z axis plot
    164             *
    165             * Use this plot to add own curves.
    166             *
    167             * \return plot
    168             *
    169             */
    170             gui::DataPlot1D *WZPlot(void) const;
     163  /*!
     164  * \brief Rotation speed around z axis plot
     165  *
     166  * Use this plot to add own curves.
     167  *
     168  * \return plot
     169  *
     170  */
     171  gui::DataPlot1D *WZPlot(void) const;
    171172
    172         protected:
    173             /*!
    174             * \brief Get ahrs datas
    175             *
    176             * \param ahrsData ahrs datas
    177             */
    178             void GetDatas(core::AhrsData **ahrsData) const;
     173protected:
     174  /*!
     175  * \brief Get ahrs datas
     176  *
     177  * \param ahrsData ahrs datas
     178  */
     179  void GetDatas(core::AhrsData **ahrsData) const;
    179180
    180         private:
    181             class Ahrs_impl *pimpl_;
    182     };
     181private:
     182  class Ahrs_impl *pimpl_;
     183};
    183184} // end namespace filter
    184185} // end namespace flair

  • trunk/lib/FlairFilter/src/Ahrs_impl.cpp

    r10 r15  
    3030using namespace flair::sensor;
    3131
    32 Ahrs_impl::Ahrs_impl(Ahrs* inSelf): self(inSelf) {
    33     rollPlot=NULL;
    34     pitchPlot=NULL;
    35     yawPlot=NULL;
    36     wXPlot=NULL;
    37     wYPlot=NULL;
    38     wZPlot=NULL;
    39     q0Plot=NULL;
    40     q1Plot=NULL;
    41     q2Plot=NULL;
    42     q3Plot=NULL;
     32Ahrs_impl::Ahrs_impl(Ahrs *inSelf) : self(inSelf) {
     33  rollPlot = NULL;
     34  pitchPlot = NULL;
     35  yawPlot = NULL;
     36  wXPlot = NULL;
     37  wYPlot = NULL;
     38  wZPlot = NULL;
     39  q0Plot = NULL;
     40  q1Plot = NULL;
     41  q2Plot = NULL;
     42  q3Plot = NULL;
    4343
    44     eulerTab=NULL;
    45     quaternionTab=NULL;
     44  eulerTab = NULL;
     45  quaternionTab = NULL;
    4646
    47     ahrsData=new AhrsData(self);
     47  ahrsData = new AhrsData(self);
    4848}
    4949
    50 Ahrs_impl::~Ahrs_impl() {
    51 }
     50Ahrs_impl::~Ahrs_impl() {}
    5251
    5352void Ahrs_impl::UseDefaultPlot(void) {
    5453
    55     eulerTab=new Tab(((Imu*)(self->Parent()))->tab,"AHRS");
    56     rollPlot=new DataPlot1D(eulerTab->NewRow(),"roll",-30,30);
    57     rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg));
    58     pitchPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"pitch",-30,30);
    59     pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg));
    60     yawPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"yaw",-180,180);
    61     yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg));
     54  eulerTab = new Tab(((Imu *)(self->Parent()))->tab, "AHRS");
     55  rollPlot = new DataPlot1D(eulerTab->NewRow(), "roll", -30, 30);
     56  rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg));
     57  pitchPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "pitch", -30, 30);
     58  pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg));
     59  yawPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "yaw", -180, 180);
     60  yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg));
    6261
    63     wXPlot=new DataPlot1D(eulerTab->NewRow(),"w_x",-200,200);
    64     wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg));
    65     wYPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_y",-200,200);
    66     wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg));
    67     wZPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_z",-200,200);
    68     wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg));
     62  wXPlot = new DataPlot1D(eulerTab->NewRow(), "w_x", -200, 200);
     63  wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg));
     64  wYPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_y", -200, 200);
     65  wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg));
     66  wZPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_z", -200, 200);
     67  wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg));
    6968
    70     quaternionTab=new Tab(((Imu*)(self->Parent()))->tab,"Quaternion");
    71     q0Plot=new DataPlot1D(quaternionTab->NewRow(),"q0",-1,1);
    72     q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0));
    73     q1Plot=new DataPlot1D(quaternionTab->NewRow(),"q1",-1,1);
    74     q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1));
    75     q2Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q2",-1,1);
    76     q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2));
    77     q3Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q3",-1,1);
    78     q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3));
    79 
     69  quaternionTab = new Tab(((Imu *)(self->Parent()))->tab, "Quaternion");
     70  q0Plot = new DataPlot1D(quaternionTab->NewRow(), "q0", -1, 1);
     71  q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0));
     72  q1Plot = new DataPlot1D(quaternionTab->NewRow(), "q1", -1, 1);
     73  q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1));
     74  q2Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q2", -1, 1);
     75  q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2));
     76  q3Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q3", -1, 1);
     77  q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3));
    8078}
    8179
    82 void Ahrs_impl::AddPlot(const AhrsData *ahrsData,DataPlot::Color_t color) {
    83     if(rollPlot!=NULL) {
    84         rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg),color);
    85         pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg),color);
    86         yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg),color);
    87     }
    88     if(wXPlot!=NULL) {
    89         wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg),color);
    90         wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg),color);
    91         wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg),color);
    92     }
    93     if(quaternionTab!=NULL) {
    94         q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0),color);
    95         q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1),color);
    96         q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2),color);
    97         q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3),color);
    98     }
     80void Ahrs_impl::AddPlot(const AhrsData *ahrsData, DataPlot::Color_t color) {
     81  if (rollPlot != NULL) {
     82    rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg), color);
     83    pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg), color);
     84    yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg), color);
     85  }
     86  if (wXPlot != NULL) {
     87    wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg), color);
     88    wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg), color);
     89    wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg), color);
     90  }
     91  if (quaternionTab != NULL) {
     92    q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0), color);
     93    q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1), color);
     94    q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2), color);
     95    q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3), color);
     96  }
    9997}

  • trunk/lib/FlairFilter/src/ButterworthLowPass.cpp

    r10 r15  
    2626using namespace flair::gui;
    2727
    28 namespace flair
    29 {
    30 namespace filter
    31 {
     28namespace flair {
     29namespace filter {
    3230
    33 ButterworthLowPass::ButterworthLowPass(const IODevice* parent,const LayoutPosition* position,string name,int order): IODevice(parent,name)
    34 {
    35     pimpl_=new ButterworthLowPass_impl(this,position,name,order);
    36     AddDataToLog(pimpl_->output);
     31ButterworthLowPass::ButterworthLowPass(const IODevice *parent,
     32                                       const LayoutPosition *position,
     33                                       string name, int order)
     34    : IODevice(parent, name) {
     35  pimpl_ = new ButterworthLowPass_impl(this, position, name, order);
     36  AddDataToLog(pimpl_->output);
    3737}
    3838
    39 ButterworthLowPass::ButterworthLowPass(const gui::LayoutPosition* position,string name,int order): IODevice(position->getLayout(),name)
    40 {
    41     pimpl_=new ButterworthLowPass_impl(this,position,name,order);
    42     AddDataToLog(pimpl_->output);
     39ButterworthLowPass::ButterworthLowPass(const gui::LayoutPosition *position,
     40                                       string name, int order)
     41    : IODevice(position->getLayout(), name) {
     42  pimpl_ = new ButterworthLowPass_impl(this, position, name, order);
     43  AddDataToLog(pimpl_->output);
    4344}
    4445
     46ButterworthLowPass::~ButterworthLowPass() { delete pimpl_; }
    4547
    46 ButterworthLowPass::~ButterworthLowPass()
    47 {
    48     delete pimpl_;
     48cvmatrix *ButterworthLowPass::Matrix(void) const { return pimpl_->output; }
     49
     50float ButterworthLowPass::Output(void) const {
     51  return pimpl_->output->Value(0, 0);
    4952}
    5053
    51 cvmatrix *ButterworthLowPass::Matrix(void) const
    52 {
    53     return pimpl_->output;
    54 }
    55 
    56 float ButterworthLowPass::Output(void) const
    57 {
    58    return pimpl_->output->Value(0,0);
    59 }
    60 
    61 void ButterworthLowPass::UpdateFrom(const io_data *data)
    62 {
    63     pimpl_->UpdateFrom(data);
    64     ProcessUpdate(pimpl_->output);
     54void ButterworthLowPass::UpdateFrom(const io_data *data) {
     55  pimpl_->UpdateFrom(data);
     56  ProcessUpdate(pimpl_->output);
    6557}
    6658

  • trunk/lib/FlairFilter/src/ButterworthLowPass.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23     }
    24     namespace gui
    25     {
    26         class LayoutPosition;
    27     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class LayoutPosition;
     24}
    2825}
    2926
    3027class ButterworthLowPass_impl;
    3128
    32 namespace flair
    33 {
    34 namespace filter
    35 {
    36     /*! \class ButterworthLowPass
    37     *
    38     * \brief Class defining a Butterworth low pass filter
    39     */
    40     class ButterworthLowPass : public core::IODevice
    41     {
    42         public:
    43             /*!
    44             * \brief Constructor
    45             *
    46             * Construct a ButterworthLowPass at position. \n
    47             * After calling this function, position will be deleted as it is no longer usefull. \n
    48             * The filter is automatically updated when parent's
    49             * IODevice::ProcessUpdate is called.
    50             *
    51             * \param parent parent
    52             * \param position position to display settings
    53             * \param name name
    54             * \param order order of the filter
    55             */
    56             ButterworthLowPass(const IODevice* parent,const gui::LayoutPosition* position,std::string name,int order);
     29namespace flair {
     30namespace filter {
     31/*! \class ButterworthLowPass
     32*
     33* \brief Class defining a Butterworth low pass filter
     34*/
     35class ButterworthLowPass : public core::IODevice {
     36public:
     37  /*!
     38  * \brief Constructor
     39  *
     40  * Construct a ButterworthLowPass at position. \n
     41  * After calling this function, position will be deleted as it is no longer
     42  *usefull. \n
     43  * The filter is automatically updated when parent's
     44  * IODevice::ProcessUpdate is called.
     45  *
     46  * \param parent parent
     47  * \param position position to display settings
     48  * \param name name
     49  * \param order order of the filter
     50  */
     51  ButterworthLowPass(const IODevice *parent,
     52                     const gui::LayoutPosition *position, std::string name,
     53                     int order);
    5754
    58             /*!
    59             * \brief Constructor
    60             *
    61             * Construct a ButterworthLowPass at position. \n
    62             * The ButterworthLowPass will automatically be child of position->getLayout() Layout. After calling this function,
    63             * position will be deleted as it is no longer usefull. \n
    64             * The filter is updated manually with UpdateFrom method. \n
    65             *
    66             * \param position position to display settings
    67             * \param name name
    68             * \param order order of the filter
    69             */
    70             ButterworthLowPass(const gui::LayoutPosition* position,std::string name,int order);
     55  /*!
     56  * \brief Constructor
     57  *
     58  * Construct a ButterworthLowPass at position. \n
     59  * The ButterworthLowPass will automatically be child of position->getLayout()
     60  *Layout. After calling this function,
     61  * position will be deleted as it is no longer usefull. \n
     62  * The filter is updated manually with UpdateFrom method. \n
     63  *
     64  * \param position position to display settings
     65  * \param name name
     66  * \param order order of the filter
     67  */
     68  ButterworthLowPass(const gui::LayoutPosition *position, std::string name,
     69                     int order);
    7170
    72             /*!
    73             * \brief Destructor
    74             *
    75             */
    76             ~ButterworthLowPass();
     71  /*!
     72  * \brief Destructor
     73  *
     74  */
     75  ~ButterworthLowPass();
    7776
    78             /*!
    79             * \brief Output value
    80             *
    81             * \return filtered output
    82             */
    83             float Output(void) const;
     77  /*!
     78  * \brief Output value
     79  *
     80  * \return filtered output
     81  */
     82  float Output(void) const;
    8483
    85             /*!
    86             * \brief Output matrix
    87             *
    88             * \return filtered output
    89             */
    90             core::cvmatrix* Matrix(void)const ;
     84  /*!
     85  * \brief Output matrix
     86  *
     87  * \return filtered output
     88  */
     89  core::cvmatrix *Matrix(void) const;
    9190
    92             /*!
    93             * \brief Update using provided datas
    94             *
    95             * Reimplemented from IODevice.
    96             *
    97             * \param data data from the parent to process
    98             */
    99             void UpdateFrom(const core::io_data *data);
     91  /*!
     92  * \brief Update using provided datas
     93  *
     94  * Reimplemented from IODevice.
     95  *
     96  * \param data data from the parent to process
     97  */
     98  void UpdateFrom(const core::io_data *data);
    10099
    101         private:
    102 
    103             class ButterworthLowPass_impl* pimpl_;
    104     };
     100private:
     101  class ButterworthLowPass_impl *pimpl_;
     102};
    105103} // end namespace filter
    106104} // end namespace flair

  • trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp

    r10 r15  
    2929using namespace flair::filter;
    3030
    31 ButterworthLowPass_impl::ButterworthLowPass_impl(ButterworthLowPass* self,const LayoutPosition* position,string name,int order)
    32 {
    33     //init UI
    34     GroupBox* reglages_groupbox=new GroupBox(position,name);
    35         T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,10,0.01);
    36         cutoff=new DoubleSpinBox(reglages_groupbox->NewRow(),"cutoff frequency"," Hz",0,10000,0.1,2,1);
     31ButterworthLowPass_impl::ButterworthLowPass_impl(ButterworthLowPass *self,
     32                                                 const LayoutPosition *position,
     33                                                 string name, int order) {
     34  // init UI
     35  GroupBox *reglages_groupbox = new GroupBox(position, name);
     36  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     37                        0, 10, 0.01);
     38  cutoff = new DoubleSpinBox(reglages_groupbox->NewRow(), "cutoff frequency",
     39                             " Hz", 0, 10000, 0.1, 2, 1);
    3740
    38     cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
    39     desc->SetElementName(0,0,"output");
    40     output=new cvmatrix(self,desc,floatType,name);
     41  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     42  desc->SetElementName(0, 0, "output");
     43  output = new cvmatrix(self, desc, floatType, name);
    4144
    42     output->SetValue(0,0,0);
     45  output->SetValue(0, 0, 0);
    4346
    44         f= new PoleFilter(order);
     47  f = new PoleFilter(order);
    4548
    46     if(T->Value()!=0) f->setup(1./T->Value(), cutoff->Value());
    47         f->reset();
     49  if (T->Value() != 0)
     50    f->setup(1. / T->Value(), cutoff->Value());
     51  f->reset();
    4852
    49         first_update=true;
     53  first_update = true;
    5054}
    5155
    52 ButterworthLowPass_impl::~ButterworthLowPass_impl()
    53 {
    54     delete f;
     56ButterworthLowPass_impl::~ButterworthLowPass_impl() { delete f; }
     57
     58void ButterworthLowPass_impl::UpdateFrom(const io_data *data) {
     59  float result;
     60  cvmatrix *input = (cvmatrix *)data;
     61
     62  if (T->ValueChanged() || cutoff->ValueChanged()) {
     63    if (T->Value() != 0) {
     64      f->setup(1. / T->Value(), cutoff->Value());
     65    } else {
     66      first_update = true;
     67    }
     68  }
     69
     70  // on prend une fois pour toute les mutex et on fait des accès directs
     71  output->GetMutex();
     72  input->GetMutex();
     73
     74  if (T->Value() == 0) {
     75    float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     76    f->setup(1. / delta_t, cutoff->Value());
     77  }
     78
     79  if (first_update == true) {
     80    first_update = false;
     81  } else {
     82    result = f->filter(input->ValueNoMutex(0, 0));
     83    output->SetValueNoMutex(0, 0, result);
     84  }
     85
     86  input->ReleaseMutex();
     87  output->ReleaseMutex();
     88
     89  output->SetDataTime(data->DataTime());
     90  previous_time = data->DataTime();
    5591}
    56 
    57 void ButterworthLowPass_impl::UpdateFrom(const io_data *data)
    58 {
    59     float result;
    60     cvmatrix *input=(cvmatrix*)data;
    61 
    62     if(T->ValueChanged() || cutoff->ValueChanged())
    63     {
    64         if(T->Value()!=0)
    65         {
    66             f->setup (1./T->Value(), cutoff->Value());
    67         }
    68         else
    69         {
    70             first_update=true;
    71         }
    72     }
    73 
    74     //on prend une fois pour toute les mutex et on fait des accès directs
    75     output->GetMutex();
    76     input->GetMutex();
    77 
    78     if(T->Value()==0)
    79     {
    80         float delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    81         f->setup (1./delta_t, cutoff->Value());
    82     }
    83 
    84     if(first_update==true)
    85     {
    86         first_update=false;
    87     }
    88     else
    89     {
    90         result = f->filter(input->ValueNoMutex(0,0));
    91         output->SetValueNoMutex(0,0,result);
    92     }
    93 
    94     input->ReleaseMutex();
    95     output->ReleaseMutex();
    96 
    97     output->SetDataTime(data->DataTime());
    98     previous_time=data->DataTime();
    99 }
    100 

  • trunk/lib/FlairFilter/src/ControlLaw.cpp

    r10 r15  
    2929namespace filter {
    3030
    31 ControlLaw::ControlLaw(const Object* parent,string name,uint32_t nb_out) : IODevice(parent,name) {
    32     if(nb_out==1) {
    33         output=new cvmatrix(this,nb_out,1,floatType,name);
    34     } else {
    35         cvmatrix_descriptor* desc=new cvmatrix_descriptor(nb_out,1);
    36         for(int i=0;i<nb_out;i++) {
    37             std::stringstream ss;
    38             ss << i;
    39             desc->SetElementName(i,0,ss.str());
    40         }
    41         output=new cvmatrix(this,desc,floatType,name);
     31ControlLaw::ControlLaw(const Object *parent, string name, uint32_t nb_out)
     32    : IODevice(parent, name) {
     33  if (nb_out == 1) {
     34    output = new cvmatrix(this, nb_out, 1, floatType, name);
     35  } else {
     36    cvmatrix_descriptor *desc = new cvmatrix_descriptor(nb_out, 1);
     37    for (int i = 0; i < nb_out; i++) {
     38      std::stringstream ss;
     39      ss << i;
     40      desc->SetElementName(i, 0, ss.str());
    4241    }
     42    output = new cvmatrix(this, desc, floatType, name);
     43  }
    4344
    44     input=NULL;
    45     AddDataToLog(output);
     45  input = NULL;
     46  AddDataToLog(output);
    4647}
    4748
    48 ControlLaw::~ControlLaw(void) {
    49 
    50 }
     49ControlLaw::~ControlLaw(void) {}
    5150
    5251void ControlLaw::Update(Time time) {
    53     input->SetDataTime(time);
    54     UpdateFrom(input);
     52  input->SetDataTime(time);
     53  UpdateFrom(input);
    5554}
    5655
    5756float ControlLaw::Output(uint32_t index) const {
    58     return output->Value(index,0);
     57  return output->Value(index, 0);
    5958}
    6059
    61 void ControlLaw::UseDefaultPlot(const LayoutPosition* position) {
    62     if(output->Rows()!=1) Warn("Output size is different from 1. Plotting only Output(1,1).\n");
     60void ControlLaw::UseDefaultPlot(const LayoutPosition *position) {
     61  if (output->Rows() != 1)
     62    Warn("Output size is different from 1. Plotting only Output(1,1).\n");
    6363
    64     DataPlot1D *plot=new DataPlot1D(position,ObjectName(),-1,1);
    65     plot->AddCurve(output->Element(0));
     64  DataPlot1D *plot = new DataPlot1D(position, ObjectName(), -1, 1);
     65  plot->AddCurve(output->Element(0));
    6666}
    6767} // end namespace filter

  • trunk/lib/FlairFilter/src/ControlLaw.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace gui {
    20         class LayoutPosition;
    21     }
    22     namespace core {
    23         class cvmatrix;
    24     }
     19namespace gui {
     20class LayoutPosition;
     21}
     22namespace core {
     23class cvmatrix;
     24}
    2525}
    2626
    27 namespace flair { namespace filter {
    28     /*! \class ControlLaw
    29     *
    30     * \brief Base class for control law
    31     * input must be created by reimplemented class.\n
    32     * output is created by this class, it is of size (nb_out,1) and type float.\n
    33     * see constructor for nb_out
    34     */
    35     class ControlLaw : public core::IODevice {
    36         public:
    37             /*!
    38             * \brief Constructor
    39             *
    40             * Construct a ControlLaw
    41             *
    42             * \param parent parent
    43             * \param name name
    44             * \param nb_out number of output
    45             */
    46             ControlLaw(const core::Object* parent,std::string name,uint32_t nb_out=1);
     27namespace flair {
     28namespace filter {
     29/*! \class ControlLaw
     30*
     31* \brief Base class for control law
     32* input must be created by reimplemented class.\n
     33* output is created by this class, it is of size (nb_out,1) and type float.\n
     34* see constructor for nb_out
     35*/
     36class ControlLaw : public core::IODevice {
     37public:
     38  /*!
     39  * \brief Constructor
     40  *
     41  * Construct a ControlLaw
     42  *
     43  * \param parent parent
     44  * \param name name
     45  * \param nb_out number of output
     46  */
     47  ControlLaw(const core::Object *parent, std::string name, uint32_t nb_out = 1);
    4748
    48             /*!
    49             * \brief Destructor
    50             *
    51             */
    52             ~ControlLaw();
     49  /*!
     50  * \brief Destructor
     51  *
     52  */
     53  ~ControlLaw();
    5354
    54             /*!
    55             * \brief Output value
    56             *
    57             * \param index output index, between 0 and nb_out-1
    58             *
    59             * \return output value
    60             */
    61             float Output(uint32_t index=0) const;
     55  /*!
     56  * \brief Output value
     57  *
     58  * \param index output index, between 0 and nb_out-1
     59  *
     60  * \return output value
     61  */
     62  float Output(uint32_t index = 0) const;
    6263
    63             /*!
    64             * \brief Use default plot
    65             *
    66             * Plot the output value at given position. \n
    67             * Only Output(1,1) is plotted. \n
    68             * In case of a mutliple output ControlLaw, this function should be reimplemented. \n
    69             * After calling this function, position will be deleted as it is no longer usefull. \n
    70             *
    71             * \param position position to display plot
    72             */
    73             virtual void UseDefaultPlot(const gui::LayoutPosition* position);
     64  /*!
     65  * \brief Use default plot
     66  *
     67  * Plot the output value at given position. \n
     68  * Only Output(1,1) is plotted. \n
     69  * In case of a mutliple output ControlLaw, this function should be
     70  *reimplemented. \n
     71  * After calling this function, position will be deleted as it is no longer
     72  *usefull. \n
     73  *
     74  * \param position position to display plot
     75  */
     76  virtual void UseDefaultPlot(const gui::LayoutPosition *position);
    7477
    75             /*!
    76             * \brief Update using provided datas
    77             *
    78             * Reimplemented class must fill input matrix before calling this.
    79             *
    80             * \param time time of the update
    81             */
    82             void Update(core::Time time);
     78  /*!
     79  * \brief Update using provided datas
     80  *
     81  * Reimplemented class must fill input matrix before calling this.
     82  *
     83  * \param time time of the update
     84  */
     85  void Update(core::Time time);
    8386
    84             /*!
    85             * \brief Reset the internal state of the control law
    86             *
    87             * Doesn't do anything by default
    88             *
    89             */
    90             virtual void Reset() {};
     87  /*!
     88  * \brief Reset the internal state of the control law
     89  *
     90  * Doesn't do anything by default
     91  *
     92  */
     93  virtual void Reset(){};
    9194
    92         protected:
    93             /*!
    94             * \brief input matrix
    95             *
    96             * This matrix must be created by the reimplemented class.
    97             *
    98             */
    99             core::cvmatrix *input;
     95protected:
     96  /*!
     97  * \brief input matrix
     98  *
     99  * This matrix must be created by the reimplemented class.
     100  *
     101  */
     102  core::cvmatrix *input;
    100103
    101             /*!
    102             * \brief output matrix
    103             *
    104             * This matrix is created by this class. Its size is (nb_out,1) and its type
    105             * is io_data::Float.
    106             *
    107             */
    108             core::cvmatrix *output;
     104  /*!
     105  * \brief output matrix
     106  *
     107  * This matrix is created by this class. Its size is (nb_out,1) and its type
     108  * is io_data::Float.
     109  *
     110  */
     111  core::cvmatrix *output;
    109112
    110         private:
    111             /*!
    112             * \brief Update using provided datas
    113             *
    114             * Reimplemented from IODevice.
    115             *
    116             * \param data data from the parent to process
    117             */
    118             virtual void UpdateFrom(const core::io_data *data)=0;
    119     };
     113private:
     114  /*!
     115  * \brief Update using provided datas
     116  *
     117  * Reimplemented from IODevice.
     118  *
     119  * \param data data from the parent to process
     120  */
     121  virtual void UpdateFrom(const core::io_data *data) = 0;
     122};
    120123} // end namespace filter
    121124} // end namespace flair

  • trunk/lib/FlairFilter/src/EulerDerivative.cpp

    r10 r15  
    2525using namespace flair::gui;
    2626
    27 namespace flair
    28 {
    29 namespace filter
    30 {
     27namespace flair {
     28namespace filter {
    3129
    32 EulerDerivative::EulerDerivative(const IODevice* parent,const LayoutPosition* position,string name,const cvmatrix* init_value) : IODevice(parent,name)
    33 {
    34     pimpl_=new EulerDerivative_impl(this,position,name,init_value);
    35     AddDataToLog(pimpl_->output);
     30EulerDerivative::EulerDerivative(const IODevice *parent,
     31                                 const LayoutPosition *position, string name,
     32                                 const cvmatrix *init_value)
     33    : IODevice(parent, name) {
     34  pimpl_ = new EulerDerivative_impl(this, position, name, init_value);
     35  AddDataToLog(pimpl_->output);
    3636}
    3737
    38 EulerDerivative::~EulerDerivative()
    39 {
    40     delete pimpl_;
     38EulerDerivative::~EulerDerivative() { delete pimpl_; }
     39
     40cvmatrix *EulerDerivative::Matrix(void) const { return pimpl_->output; }
     41
     42float EulerDerivative::Output(int row, int col) const {
     43  return pimpl_->output->Value(row, col);
    4144}
    4245
    43 cvmatrix* EulerDerivative::Matrix(void) const
    44 {
    45     return pimpl_->output;
    46 }
    47 
    48 float EulerDerivative::Output(int row,int col) const
    49 {
    50     return pimpl_->output->Value(row,col);
    51 }
    52 
    53 void EulerDerivative::UpdateFrom(const io_data *data)
    54 {
    55     pimpl_->UpdateFrom(data);
    56     ProcessUpdate(pimpl_->output);
     46void EulerDerivative::UpdateFrom(const io_data *data) {
     47  pimpl_->UpdateFrom(data);
     48  ProcessUpdate(pimpl_->output);
    5749}
    5850

  • trunk/lib/FlairFilter/src/EulerDerivative.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23     }
    24     namespace gui
    25     {
    26         class LayoutPosition;
    27     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class LayoutPosition;
     24}
    2825}
    2926
    3027class EulerDerivative_impl;
    3128
    32 namespace flair
    33 {
    34 namespace filter
    35 {
    36     /*! \class EulerDerivative
    37     *
    38     * \brief Class defining an euler derivative
    39     */
     29namespace flair {
     30namespace filter {
     31/*! \class EulerDerivative
     32*
     33* \brief Class defining an euler derivative
     34*/
    4035
    41     class EulerDerivative : public core::IODevice
    42     {
    43         public:
    44             /*!
    45             * \brief Constructor
    46             *
    47             * Construct an EulerDerivative at given position. \n
    48             * After calling this function, position will be deleted as it is no longer usefull. \n
    49             * The filter is automatically updated when parent's
    50             * IODevice::ProcessUpdate is called. \n
    51             * The optional init_value parameters allow to specify
    52             * the size of the input datas and its inital values.
    53             * If unspecified, a 1*1 size is used, and values are
    54             * initialized with 0.
    55             *
    56             * \param parent parent
    57             * \param position position to display settings
    58             * \param name name
    59             * \param init_value initial value
    60             */
    61             EulerDerivative(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name,const core::cvmatrix* init_value=NULL);
     36class EulerDerivative : public core::IODevice {
     37public:
     38  /*!
     39  * \brief Constructor
     40  *
     41  * Construct an EulerDerivative at given position. \n
     42  * After calling this function, position will be deleted as it is no longer
     43  *usefull. \n
     44  * The filter is automatically updated when parent's
     45  * IODevice::ProcessUpdate is called. \n
     46  * The optional init_value parameters allow to specify
     47  * the size of the input datas and its inital values.
     48  * If unspecified, a 1*1 size is used, and values are
     49  * initialized with 0.
     50  *
     51  * \param parent parent
     52  * \param position position to display settings
     53  * \param name name
     54  * \param init_value initial value
     55  */
     56  EulerDerivative(const core::IODevice *parent,
     57                  const gui::LayoutPosition *position, std::string name,
     58                  const core::cvmatrix *init_value = NULL);
    6259
    63             /*!
    64             * \brief Destructor
    65             *
    66             */
    67             ~EulerDerivative();
     60  /*!
     61  * \brief Destructor
     62  *
     63  */
     64  ~EulerDerivative();
    6865
    69             /*!
    70             * \brief Output value
    71             *
    72             * \param row row element
    73             * \param col column element
    74             *
    75             * \return element value
    76             */
    77             float Output(int row, int col) const;
     66  /*!
     67  * \brief Output value
     68  *
     69  * \param row row element
     70  * \param col column element
     71  *
     72  * \return element value
     73  */
     74  float Output(int row, int col) const;
    7875
    79             /*!
    80             * \brief Output matrix
    81             *
    82             * \return filtered output
    83             */
    84             core::cvmatrix *Matrix(void) const;
     76  /*!
     77  * \brief Output matrix
     78  *
     79  * \return filtered output
     80  */
     81  core::cvmatrix *Matrix(void) const;
    8582
    86         private:
    87             /*!
    88             * \brief Update using provided datas
    89             *
    90             * Reimplemented from IODevice.
    91             *
    92             * \param data data from the parent to process
    93             */
    94             void UpdateFrom(const core::io_data *data);
     83private:
     84  /*!
     85  * \brief Update using provided datas
     86  *
     87  * Reimplemented from IODevice.
     88  *
     89  * \param data data from the parent to process
     90  */
     91  void UpdateFrom(const core::io_data *data);
    9592
    96             class EulerDerivative_impl* pimpl_;
    97     };
     93  class EulerDerivative_impl *pimpl_;
     94};
    9895} // end namespace filter
    9996} // end namespace flair

  • trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp

    r10 r15  
    2828using namespace flair::filter;
    2929
    30 EulerDerivative_impl::EulerDerivative_impl(EulerDerivative* self,const LayoutPosition* position,string name,const cvmatrix* init_value)
    31 {
    32     first_update=true;
     30EulerDerivative_impl::EulerDerivative_impl(EulerDerivative *self,
     31                                           const LayoutPosition *position,
     32                                           string name,
     33                                           const cvmatrix *init_value) {
     34  first_update = true;
    3335
    34     if(init_value!=NULL)
    35     {
    36         prev_value=(cvmatrix*)init_value;
     36  if (init_value != NULL) {
     37    prev_value = (cvmatrix *)init_value;
     38  } else {
     39    // if NULL, assume dimension 1, and init=0
     40    cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     41    desc->SetElementName(0, 0, "output");
     42    prev_value = new cvmatrix(self, desc, floatType, name);
     43    prev_value->SetValue(0, 0, 0);
     44  }
     45
     46  // init UI
     47  GroupBox *reglages_groupbox = new GroupBox(position, name);
     48  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
     49                        " s", 0, 1, 0.01);
     50
     51  // init output matrix of same size as init
     52  cvmatrix_descriptor *desc =
     53      new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());
     54
     55  for (int i = 0; i < prev_value->Rows(); i++) {
     56    for (int j = 0; j < prev_value->Cols(); j++) {
     57      desc->SetElementName(i, j, prev_value->Name(i, j));
    3758    }
    38     else
    39     {
    40         //if NULL, assume dimension 1, and init=0
    41         cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
    42         desc->SetElementName(0,0,"output");
    43         prev_value=new cvmatrix(self,desc,floatType,name);
    44         prev_value->SetValue(0,0,0);
     59  }
     60
     61  output = new cvmatrix(self, desc,
     62                        prev_value->GetDataType().GetElementDataType(), name);
     63}
     64
     65EulerDerivative_impl::~EulerDerivative_impl() {}
     66
     67void EulerDerivative_impl::UpdateFrom(const io_data *data) {
     68  float delta_t;
     69  cvmatrix *input = (cvmatrix *)data;
     70
     71  // on prend une fois pour toute les mutex et on fait des accès directs
     72  output->GetMutex();
     73  input->GetMutex();
     74
     75  if (first_update == true) {
     76    for (int i = 0; i < input->Rows(); i++) {
     77      for (int j = 0; j < input->Cols(); j++) {
     78        output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
     79        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     80      }
     81    }
     82    first_update = false;
     83  } else {
     84    if (T->Value() == 0) {
     85      delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     86    } else {
     87      delta_t = T->Value();
    4588    }
    4689
    47     //init UI
    48     GroupBox* reglages_groupbox=new GroupBox(position,name);
    49         T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);
     90    for (int i = 0; i < input->Rows(); i++) {
     91      for (int j = 0; j < input->Cols(); j++) {
     92        output->SetValueNoMutex(
     93            i, j, (input->ValueNoMutex(i, j) - prev_value->ValueNoMutex(i, j)) /
     94                      delta_t);
     95        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     96      }
     97    }
     98  }
    5099
    51     //init output matrix of same size as init
    52     cvmatrix_descriptor* desc=new cvmatrix_descriptor(prev_value->Rows(),prev_value->Cols());
     100  input->ReleaseMutex();
     101  output->ReleaseMutex();
    53102
    54     for(int i=0;i<prev_value->Rows();i++)
    55     {
    56         for(int j=0;j<prev_value->Cols();j++)
    57         {
    58             desc->SetElementName(i,j,prev_value->Name(i,j));
    59         }
    60     }
    61 
    62     output=new cvmatrix(self,desc,prev_value->GetDataType().GetElementDataType(),name);
     103  output->SetDataTime(data->DataTime());
     104  previous_time = data->DataTime();
    63105}
    64 
    65 EulerDerivative_impl::~EulerDerivative_impl()
    66 {
    67 
    68 }
    69 
    70 void EulerDerivative_impl::UpdateFrom(const io_data *data)
    71 {
    72     float delta_t;
    73     cvmatrix *input=(cvmatrix*)data;
    74 
    75     //on prend une fois pour toute les mutex et on fait des accès directs
    76     output->GetMutex();
    77     input->GetMutex();
    78 
    79     if(first_update==true)
    80     {
    81         for(int i=0;i<input->Rows();i++)
    82         {
    83             for(int j=0;j<input->Cols();j++)
    84             {
    85                 output->SetValueNoMutex(i,j,prev_value->ValueNoMutex(i,j));
    86                 prev_value->SetValueNoMutex(i,j,input->ValueNoMutex(i,j));
    87             }
    88         }
    89         first_update=false;
    90     }
    91     else
    92     {
    93         if(T->Value()==0)
    94         {
    95             delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    96         }
    97         else
    98         {
    99             delta_t=T->Value();
    100         }
    101 
    102         for(int i=0;i<input->Rows();i++)
    103         {
    104             for(int j=0;j<input->Cols();j++)
    105             {
    106                 output->SetValueNoMutex(i,j,(input->ValueNoMutex(i,j)-prev_value->ValueNoMutex(i,j))/delta_t);
    107                 prev_value->SetValueNoMutex(i,j,input->ValueNoMutex(i,j));
    108             }
    109         }
    110     }
    111 
    112     input->ReleaseMutex();
    113     output->ReleaseMutex();
    114 
    115     output->SetDataTime(data->DataTime());
    116     previous_time=data->DataTime();
    117 }
    118 

  • trunk/lib/FlairFilter/src/Gx3_25_ahrs.cpp

    r10 r15  
    2424using namespace flair::sensor;
    2525
    26 namespace flair { namespace filter {
     26namespace flair {
     27namespace filter {
    2728
    28 Gx3_25_ahrs::Gx3_25_ahrs(const FrameworkManager* parent,string name,SerialPort *serialport,Gx3_25_imu::Command_t command,uint8_t priority) : Ahrs(new Gx3_25_imu(parent,name,serialport,command,priority),name) {
    29 }
     29Gx3_25_ahrs::Gx3_25_ahrs(const FrameworkManager *parent, string name,
     30                         SerialPort *serialport, Gx3_25_imu::Command_t command,
     31                         uint8_t priority)
     32    : Ahrs(new Gx3_25_imu(parent, name, serialport, command, priority), name) {}
    3033
    31 Gx3_25_ahrs::~Gx3_25_ahrs() {
    32 }
     34Gx3_25_ahrs::~Gx3_25_ahrs() {}
    3335
    34 void Gx3_25_ahrs::Start(void) {
    35     ((Gx3_25_imu*)GetImu())->Start();
    36 }
     36void Gx3_25_ahrs::Start(void) { ((Gx3_25_imu *)GetImu())->Start(); }
    3737
    38 //datas from Gx3_25_imu are AhrsData!
     38// datas from Gx3_25_imu are AhrsData!
    3939void Gx3_25_ahrs::UpdateFrom(const io_data *data) {
    40     AhrsData *input=(AhrsData*)data;
    41     AhrsData *output;
    42     GetDatas(&output);
     40  AhrsData *input = (AhrsData *)data;
     41  AhrsData *output;
     42  GetDatas(&output);
    4343
    44     Quaternion quaternion;
    45     Vector3D filteredAngRates;
    46     input->GetQuaternionAndAngularRates(quaternion,filteredAngRates);
    47     output->SetQuaternionAndAngularRates(quaternion,filteredAngRates);
    48     output->SetDataTime(input->DataTime());
     44  Quaternion quaternion;
     45  Vector3D filteredAngRates;
     46  input->GetQuaternionAndAngularRates(quaternion, filteredAngRates);
     47  output->SetQuaternionAndAngularRates(quaternion, filteredAngRates);
     48  output->SetDataTime(input->DataTime());
    4949
    50     ProcessUpdate(output);
     50  ProcessUpdate(output);
    5151}
    5252
    5353} // end namespace filter
    5454} // end namespace flair
    55 

  • trunk/lib/FlairFilter/src/Gx3_25_ahrs.h

    r10 r15  
    1717#include <Gx3_25_imu.h>
    1818
    19 namespace flair
    20 {
    21 namespace filter
    22 {
    23     /*! \class Gx3_25_ahrs
    24     *
    25     * \brief Class for 3dmgx3-25 ahrs
    26     *
    27     * This class constructs a Gx3_25_imu as Imu of this Ahrs.
    28     */
    29     class Gx3_25_ahrs : public Ahrs
    30     {
    31         public:
    32             /*!
    33             * \brief Constructor
    34             *
    35             * Construct an Ahrs for 3dmgx3-25
    36             *
    37             * \param parent parent
    38             * \param name name
    39             * \param serialport Imu SerialPort
    40             * \param command command for the Gx3_25_imu continuous mode
    41             * \param priority priority of the Gx3_25_imu Thread
    42             */
    43             Gx3_25_ahrs(const core::FrameworkManager* parent,std::string name,core::SerialPort *serialport,sensor::Gx3_25_imu::Command_t command,uint8_t priority);
     19namespace flair {
     20namespace filter {
     21/*! \class Gx3_25_ahrs
     22*
     23* \brief Class for 3dmgx3-25 ahrs
     24*
     25* This class constructs a Gx3_25_imu as Imu of this Ahrs.
     26*/
     27class Gx3_25_ahrs : public Ahrs {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct an Ahrs for 3dmgx3-25
     33  *
     34  * \param parent parent
     35  * \param name name
     36  * \param serialport Imu SerialPort
     37  * \param command command for the Gx3_25_imu continuous mode
     38  * \param priority priority of the Gx3_25_imu Thread
     39  */
     40  Gx3_25_ahrs(const core::FrameworkManager *parent, std::string name,
     41              core::SerialPort *serialport,
     42              sensor::Gx3_25_imu::Command_t command, uint8_t priority);
    4443
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~Gx3_25_ahrs();
     44  /*!
     45  * \brief Destructor
     46  *
     47  */
     48  ~Gx3_25_ahrs();
    5049
    51             /*!
    52             * \brief Start Gx3_25_imu Thread
    53             *
    54             */
    55             void Start(void);
     50  /*!
     51  * \brief Start Gx3_25_imu Thread
     52  *
     53  */
     54  void Start(void);
    5655
    57         private:
    58             /*!
    59             * \brief Update using provided datas
    60             *
    61             * Reimplemented from IODevice.
    62             *
    63             * \param data data from the parent to process
    64             */
    65             void UpdateFrom(const core::io_data *data);
    66     };
     56private:
     57  /*!
     58  * \brief Update using provided datas
     59  *
     60  * Reimplemented from IODevice.
     61  *
     62  * \param data data from the parent to process
     63  */
     64  void UpdateFrom(const core::io_data *data);
     65};
    6766} // end namespace filter
    6867} // end namespace flair

  • trunk/lib/FlairFilter/src/JoyReference.cpp

    r10 r15  
    2929using namespace flair::gui;
    3030
    31 namespace flair { namespace filter {
     31namespace flair {
     32namespace filter {
    3233
    33 JoyReference::JoyReference(const LayoutPosition* position,string name) : IODevice(position->getLayout(),name) {
    34     pimpl_=new JoyReference_impl(this,position,name);
    35     AddDataToLog(pimpl_->output);
    36     AddDataToLog(pimpl_->ahrsData);
     34JoyReference::JoyReference(const LayoutPosition *position, string name)
     35    : IODevice(position->getLayout(), name) {
     36  pimpl_ = new JoyReference_impl(this, position, name);
     37  AddDataToLog(pimpl_->output);
     38  AddDataToLog(pimpl_->ahrsData);
    3739}
    3840
    39 JoyReference::~JoyReference(void) {
    40     delete pimpl_;
     41JoyReference::~JoyReference(void) { delete pimpl_; }
     42
     43AhrsData *JoyReference::GetReferenceOrientation(void) const {
     44  return pimpl_->ahrsData;
    4145}
    4246
    43 AhrsData* JoyReference::GetReferenceOrientation(void) const{
    44     return pimpl_->ahrsData;
     47void JoyReference::SetRollAxis(float value) { pimpl_->SetRollAxis(value); }
     48
     49void JoyReference::SetPitchAxis(float value) { pimpl_->SetPitchAxis(value); }
     50
     51void JoyReference::SetYawAxis(float value) { pimpl_->SetYawAxis(value); }
     52
     53void JoyReference::SetAltitudeAxis(float value) {
     54  pimpl_->SetAltitudeAxis(value);
    4555}
    4656
    47 void JoyReference::SetRollAxis(float value) {
    48     pimpl_->SetRollAxis(value);
    49 }
     57void JoyReference::RollTrimUp(void) { pimpl_->RollTrimUp(); }
    5058
    51 void JoyReference::SetPitchAxis(float value) {
    52     pimpl_->SetPitchAxis(value);
    53 }
     59void JoyReference::RollTrimDown(void) { pimpl_->RollTrimDown(); }
    5460
    55 void JoyReference::SetYawAxis(float value) {
    56     pimpl_->SetYawAxis(value);
    57 }
     61void JoyReference::PitchTrimUp(void) { pimpl_->PitchTrimUp(); }
    5862
    59 void JoyReference::SetAltitudeAxis(float value) {
    60     pimpl_->SetAltitudeAxis(value);
    61 }
     63void JoyReference::PitchTrimDown(void) { pimpl_->PitchTrimDown(); }
    6264
    63 void JoyReference::RollTrimUp(void) {
    64     pimpl_->RollTrimUp();
    65 }
    66 
    67 void JoyReference::RollTrimDown(void) {
    68     pimpl_->RollTrimDown();
    69 }
    70 
    71 void JoyReference::PitchTrimUp(void) {
    72     pimpl_->PitchTrimUp();
    73 }
    74 
    75 void JoyReference::PitchTrimDown(void) {
    76     pimpl_->PitchTrimDown();
    77 }
    78 
    79 void JoyReference::SetYawRef(float value) {
    80     pimpl_->SetYawRef(value);
    81 }
     65void JoyReference::SetYawRef(float value) { pimpl_->SetYawRef(value); }
    8266
    8367void JoyReference::SetYawRef(core::Quaternion const &value) {
    84     Euler euler;
    85     value.ToEuler(euler);
    86     pimpl_->SetYawRef(euler.yaw);
     68  Euler euler;
     69  value.ToEuler(euler);
     70  pimpl_->SetYawRef(euler.yaw);
    8771}
    88 void JoyReference::SetZRef(float value) {
    89     pimpl_->SetZRef(value);
    90 }
     72void JoyReference::SetZRef(float value) { pimpl_->SetZRef(value); }
    9173
    92 float JoyReference::ZRef(void) const {
    93     return pimpl_->ZRef();
    94 }
     74float JoyReference::ZRef(void) const { return pimpl_->ZRef(); }
    9575
    96 float JoyReference::DzRef(void) const {
    97     return pimpl_->dZRef();
    98 }
     76float JoyReference::DzRef(void) const { return pimpl_->dZRef(); }
    9977
    100 float JoyReference::RollTrim(void) const {
    101     return pimpl_->RollTrim();
    102 }
     78float JoyReference::RollTrim(void) const { return pimpl_->RollTrim(); }
    10379
    104 float JoyReference::PitchTrim(void) const {
    105     return pimpl_->PitchTrim();
    106 }
     80float JoyReference::PitchTrim(void) const { return pimpl_->PitchTrim(); }
    10781
    10882void JoyReference::Update(Time time) {
    109     pimpl_->Update(time);
    110     ProcessUpdate(pimpl_->output);
     83  pimpl_->Update(time);
     84  ProcessUpdate(pimpl_->output);
    11185}
    11286
    11387void JoyReference::UpdateFrom(const io_data *data) {
    114     pimpl_->UpdateFrom(data);
    115     ProcessUpdate(pimpl_->output);
     88  pimpl_->UpdateFrom(data);
     89  ProcessUpdate(pimpl_->output);
    11690}
    11791
    11892} // end namespace sensor
    11993} // end namespace flair
    120 

  • trunk/lib/FlairFilter/src/JoyReference.h

    r10 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21     namespace core {
    22         class Quaternion;
    23         class AhrsData;
    24     }
    25     namespace gui {
    26         class LayoutPosition;
    27     }
     19namespace flair {
     20namespace core {
     21class Quaternion;
     22class AhrsData;
    2823}
     24namespace gui {
     25class LayoutPosition;
     26}
     27}
    2928
    3029class JoyReference_impl;
    3130
    32 namespace flair { namespace filter {
    33     /*! \class JoyReference
    34     *
    35     * \brief Class creating references from a joystick
    36     */
    37     class JoyReference : public core::IODevice
    38     {
    39         public:
    40             /*!
    41             * \brief Constructor
    42             *
    43             * Construct a JoyReference at given position. \n
    44             * The JoyReference will automatically be child of position->getLayout() Layout. After calling this function,
    45             * position will be deleted as it is no longer usefull. \n
    46             * JoyReference compute reference in quaternion, wz, altitude and altitude speed.
    47             *
    48             * \param position position
    49             * \param name name
    50             */
    51             JoyReference(const gui::LayoutPosition* position,std::string name);
    52 
    53             /*!
    54             * \brief Destructor
    55             *
    56             */
    57             ~JoyReference();
    58 
    59             /*!
    60             * \brief Set roll axis value
    61             *
    62             * \param value value
    63             */
    64             void SetRollAxis(float value);
    65 
    66             /*!
    67             * \brief Set pitch axis value
    68             *
    69             * \param value value
    70             */
    71             void SetPitchAxis(float value);
    72 
    73             /*!
    74             * \brief Set yaw axis value
    75             *
    76             * \param value value
    77             */
    78             void SetYawAxis(float value);
    79 
    80             /*!
    81             * \brief Set thrust axis value
    82             *
    83             * \param value value
    84             */
    85             void SetAltitudeAxis(float value);
    86 
    87             /*!
    88             * \brief Get orientation reference
    89             *
    90             * \return reference
    91             */
    92             core::AhrsData* GetReferenceOrientation(void) const;
    93 
    94             /*!
    95             * \brief Get z reference
    96             *
    97             * \return reference
    98             */
    99             float ZRef(void) const;
    100 
    101             /*!
    102             * \brief Get z derivative reference
    103             *
    104             * \return reference
    105             */
    106             float DzRef(void) const;
    107 
    108             /*!
    109             * \brief Get roll trim
    110             *
    111             * \return trim value
    112             */
    113             float RollTrim(void) const;
    114 
    115             /*!
    116             * \brief Get pitch trim
    117             *
    118             * \return trim value
    119             */
    120             float PitchTrim(void) const;
    121 
    122             /*!
    123             * \brief Set yaw reference
    124             *
    125             * Yaw part of the output quaternion is obtained by integrating the wz desired angular speed.\n
    126             * This method reset the yaw.
    127             *
    128             * \param value value
    129             */
    130             void SetYawRef(float value);
    131 
    132             /*!
    133             * \brief Set yaw reference
    134             *
    135             * Yaw part of the output quaternion is obtained by integrating the wz desired angular speed.\n
    136             * This method reset the yaw.
    137             *
    138             * \param value value, only the yaw part of the quaternion is used
    139             */
    140             void SetYawRef(core::Quaternion const &value);
    141 
    142             /*!
    143             * \brief Set z reference
    144             *
    145             * Altitude of the output is obtained by integrating the vz desired altitude speed.\n
    146             * This method reset z.
    147             *
    148             * \param value value
    149             */
    150             void SetZRef(float value);
    151 
    152             /*!
    153             * \brief Trim up roll
    154             *
    155             * Roll trim value is increased by one
    156             */
    157             void RollTrimUp(void);
    158 
    159             /*!
    160             * \brief Trim down roll
    161             *
    162             * Roll trim value is decreased by one
    163             */
    164             void RollTrimDown(void);
    165 
    166             /*!
    167             * \brief Trim up pitch
    168             *
    169             * Pitch trim value is increased by one
    170             */
    171             void PitchTrimUp(void);
    172 
    173             /*!
    174             * \brief Trim down pitch
    175             *
    176             * Pitch trim value is decreased by one
    177             */
    178             void PitchTrimDown(void);
    179 
    180             /*!
    181             * \brief Update references
    182             *
    183             * Calls UpdateFrom with values entered manually.
    184             *
    185             * \param time time
    186             */
    187             void Update(core::Time time);
    188 
    189         private:
    190             /*!
    191             * \brief Update using provided datas
    192             *
    193             * Reimplemented from IODevice.
    194             *
    195             * \param data data from the parent to process
    196             */
    197             void UpdateFrom(const core::io_data *data);
    198 
    199             class JoyReference_impl* pimpl_;
    200     };
     31namespace flair {
     32namespace filter {
     33/*! \class JoyReference
     34*
     35* \brief Class creating references from a joystick
     36*/
     37class JoyReference : public core::IODevice {
     38public:
     39  /*!
     40  * \brief Constructor
     41  *
     42  * Construct a JoyReference at given position. \n
     43  * The JoyReference will automatically be child of position->getLayout()
     44  *Layout. After calling this function,
     45  * position will be deleted as it is no longer usefull. \n
     46  * JoyReference compute reference in quaternion, wz, altitude and altitude
     47  *speed.
     48  *
     49  * \param position position
     50  * \param name name
     51  */
     52  JoyReference(const gui::LayoutPosition *position, std::string name);
     53
     54  /*!
     55  * \brief Destructor
     56  *
     57  */
     58  ~JoyReference();
     59
     60  /*!
     61  * \brief Set roll axis value
     62  *
     63  * \param value value
     64  */
     65  void SetRollAxis(float value);
     66
     67  /*!
     68  * \brief Set pitch axis value
     69  *
     70  * \param value value
     71  */
     72  void SetPitchAxis(float value);
     73
     74  /*!
     75  * \brief Set yaw axis value
     76  *
     77  * \param value value
     78  */
     79  void SetYawAxis(float value);
     80
     81  /*!
     82  * \brief Set thrust axis value
     83  *
     84  * \param value value
     85  */
     86  void SetAltitudeAxis(float value);
     87
     88  /*!
     89  * \brief Get orientation reference
     90  *
     91  * \return reference
     92  */
     93  core::AhrsData *GetReferenceOrientation(void) const;
     94
     95  /*!
     96  * \brief Get z reference
     97  *
     98  * \return reference
     99  */
     100  float ZRef(void) const;
     101
     102  /*!
     103  * \brief Get z derivative reference
     104  *
     105  * \return reference
     106  */
     107  float DzRef(void) const;
     108
     109  /*!
     110  * \brief Get roll trim
     111  *
     112  * \return trim value
     113  */
     114  float RollTrim(void) const;
     115
     116  /*!
     117  * \brief Get pitch trim
     118  *
     119  * \return trim value
     120  */
     121  float PitchTrim(void) const;
     122
     123  /*!
     124  * \brief Set yaw reference
     125  *
     126  * Yaw part of the output quaternion is obtained by integrating the wz desired
     127  *angular speed.\n
     128  * This method reset the yaw.
     129  *
     130  * \param value value
     131  */
     132  void SetYawRef(float value);
     133
     134  /*!
     135  * \brief Set yaw reference
     136  *
     137  * Yaw part of the output quaternion is obtained by integrating the wz desired
     138  *angular speed.\n
     139  * This method reset the yaw.
     140  *
     141  * \param value value, only the yaw part of the quaternion is used
     142  */
     143  void SetYawRef(core::Quaternion const &value);
     144
     145  /*!
     146  * \brief Set z reference
     147  *
     148  * Altitude of the output is obtained by integrating the vz desired altitude
     149  *speed.\n
     150  * This method reset z.
     151  *
     152  * \param value value
     153  */
     154  void SetZRef(float value);
     155
     156  /*!
     157  * \brief Trim up roll
     158  *
     159  * Roll trim value is increased by one
     160  */
     161  void RollTrimUp(void);
     162
     163  /*!
     164  * \brief Trim down roll
     165  *
     166  * Roll trim value is decreased by one
     167  */
     168  void RollTrimDown(void);
     169
     170  /*!
     171  * \brief Trim up pitch
     172  *
     173  * Pitch trim value is increased by one
     174  */
     175  void PitchTrimUp(void);
     176
     177  /*!
     178  * \brief Trim down pitch
     179  *
     180  * Pitch trim value is decreased by one
     181  */
     182  void PitchTrimDown(void);
     183
     184  /*!
     185  * \brief Update references
     186  *
     187  * Calls UpdateFrom with values entered manually.
     188  *
     189  * \param time time
     190  */
     191  void Update(core::Time time);
     192
     193private:
     194  /*!
     195  * \brief Update using provided datas
     196  *
     197  * Reimplemented from IODevice.
     198  *
     199  * \param data data from the parent to process
     200  */
     201  void UpdateFrom(const core::io_data *data);
     202
     203  class JoyReference_impl *pimpl_;
     204};
    201205} // end namespace sensor
    202206} // end namespace flair

  • trunk/lib/FlairFilter/src/JoyReference_impl.cpp

    r10 r15  
    3434using namespace flair::filter;
    3535
    36 JoyReference_impl::JoyReference_impl(JoyReference *inSelf,const LayoutPosition* position,string name): self(inSelf) {
    37 
    38     ahrsData= new AhrsData(self);
    39         input=new cvmatrix(self,4,1,floatType,name);
    40 
    41         cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,1);
    42         desc->SetElementName(0,0,"z");;
    43         desc->SetElementName(1,0,"dz");
    44     desc->SetElementName(2,0,"trim_roll");
    45     desc->SetElementName(3,0,"trim_pitch");
    46     output=new cvmatrix(self,desc,floatType,name);
    47 
    48         reglages_groupbox=new GroupBox(position,name);
    49     deb_roll=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement roll"," deg",-45,45,1,0);
    50     deb_pitch=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement pitch"," deg",-45,45,1,0);
    51     deb_wz=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement wz"," deg/s",-180,180,1,0);
    52     deb_dz=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement dz"," m/s",-2,2,0.1,1);
    53     trim=new DoubleSpinBox(reglages_groupbox->NewRow(),"trim",-1,1,0.01);
    54     label_roll=new Label(reglages_groupbox->NewRow(),"trim roll");
    55     button_roll=new PushButton(reglages_groupbox->LastRowLastCol(),"reset roll trim");
    56     label_pitch=new Label(reglages_groupbox->NewRow(),"trim pitch");
    57     button_pitch=new PushButton(reglages_groupbox->LastRowLastCol(),"reset pitch trim");
    58 
    59     z_ref=0;
    60     previous_time=0;
    61     trim_roll=0;
    62     trim_pitch=0;
    63 
    64     label_roll->SetText("trim roll: %.2f",trim_roll);
    65     label_pitch->SetText("trim pitch: %.2f",trim_pitch);
    66 }
    67 
    68 JoyReference_impl::~JoyReference_impl(void) {
    69 }
     36JoyReference_impl::JoyReference_impl(JoyReference *inSelf,
     37                                     const LayoutPosition *position,
     38                                     string name)
     39    : self(inSelf) {
     40
     41  ahrsData = new AhrsData(self);
     42  input = new cvmatrix(self, 4, 1, floatType, name);
     43
     44  cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 1);
     45  desc->SetElementName(0, 0, "z");
     46  ;
     47  desc->SetElementName(1, 0, "dz");
     48  desc->SetElementName(2, 0, "trim_roll");
     49  desc->SetElementName(3, 0, "trim_pitch");
     50  output = new cvmatrix(self, desc, floatType, name);
     51
     52  reglages_groupbox = new GroupBox(position, name);
     53  deb_roll = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement roll",
     54                               " deg", -45, 45, 1, 0);
     55  deb_pitch = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     56                                "debattement pitch", " deg", -45, 45, 1, 0);
     57  deb_wz = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement wz",
     58                             " deg/s", -180, 180, 1, 0);
     59  deb_dz = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     60                             "debattement dz", " m/s", -2, 2, 0.1, 1);
     61  trim = new DoubleSpinBox(reglages_groupbox->NewRow(), "trim", -1, 1, 0.01);
     62  label_roll = new Label(reglages_groupbox->NewRow(), "trim roll");
     63  button_roll =
     64      new PushButton(reglages_groupbox->LastRowLastCol(), "reset roll trim");
     65  label_pitch = new Label(reglages_groupbox->NewRow(), "trim pitch");
     66  button_pitch =
     67      new PushButton(reglages_groupbox->LastRowLastCol(), "reset pitch trim");
     68
     69  z_ref = 0;
     70  previous_time = 0;
     71  trim_roll = 0;
     72  trim_pitch = 0;
     73
     74  label_roll->SetText("trim roll: %.2f", trim_roll);
     75  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
     76}
     77
     78JoyReference_impl::~JoyReference_impl(void) {}
    7079
    7180void JoyReference_impl::SetRollAxis(float value) {
    72     input->SetValue(0,0,value);
     81  input->SetValue(0, 0, value);
    7382}
    7483
    7584void JoyReference_impl::SetPitchAxis(float value) {
    76     input->SetValue(1,0,value);
     85  input->SetValue(1, 0, value);
    7786}
    7887
    7988void JoyReference_impl::SetYawAxis(float value) {
    80     input->SetValue(2,0,value);
     89  input->SetValue(2, 0, value);
    8190}
    8291
    8392void JoyReference_impl::SetAltitudeAxis(float value) {
    84     input->SetValue(3,0,value);
     93  input->SetValue(3, 0, value);
    8594}
    8695
    8796void JoyReference_impl::RollTrimUp(void) {
    88     trim_roll+=trim->Value();
    89     output->SetValue(2,0,trim_roll);
    90     label_roll->SetText("trim roll: %.2f",trim_roll);
     97  trim_roll += trim->Value();
     98  output->SetValue(2, 0, trim_roll);
     99  label_roll->SetText("trim roll: %.2f", trim_roll);
    91100}
    92101
    93102void JoyReference_impl::RollTrimDown(void) {
    94     trim_roll-=trim->Value();
    95     output->SetValue(2,0,trim_roll);
    96     label_roll->SetText("trim roll: %.2f",trim_roll);
     103  trim_roll -= trim->Value();
     104  output->SetValue(2, 0, trim_roll);
     105  label_roll->SetText("trim roll: %.2f", trim_roll);
    97106}
    98107
    99108void JoyReference_impl::PitchTrimUp(void) {
    100     trim_pitch+=trim->Value();
    101     output->SetValue(3,0,trim_pitch);
    102     label_pitch->SetText("trim pitch: %.2f",trim_pitch);
     109  trim_pitch += trim->Value();
     110  output->SetValue(3, 0, trim_pitch);
     111  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
    103112}
    104113
    105114void JoyReference_impl::PitchTrimDown(void) {
    106     trim_pitch-=trim->Value();
    107     output->SetValue(3,0,trim_pitch);
    108     label_pitch->SetText("trim pitch: %.2f",trim_pitch);
     115  trim_pitch -= trim->Value();
     116  output->SetValue(3, 0, trim_pitch);
     117  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
    109118}
    110119
    111120void JoyReference_impl::SetYawRef(float value) {
    112     Euler ref(0,0,value);
    113     input->GetMutex();
    114     ref.ToQuaternion(q_z);
    115     input->ReleaseMutex();
    116 
    117     Update(GetTime());
     121  Euler ref(0, 0, value);
     122  input->GetMutex();
     123  ref.ToQuaternion(q_z);
     124  input->ReleaseMutex();
     125
     126  Update(GetTime());
    118127}
    119128
    120129void JoyReference_impl::SetZRef(float value) {
    121     z_ref=value;
    122     output->SetValue(0,0,z_ref);
    123 }
    124 
    125 float JoyReference_impl::ZRef(void) const {
    126     return output->Value(0,0);
    127 }
    128 
    129 float JoyReference_impl::dZRef(void) const {
    130    return output->Value(1,0);
    131 }
    132 
    133 float JoyReference_impl::RollTrim(void) const {
    134     return trim_roll;
    135 }
    136 
    137 float JoyReference_impl::PitchTrim(void) const {
    138     return trim_pitch;
    139 }
     130  z_ref = value;
     131  output->SetValue(0, 0, z_ref);
     132}
     133
     134float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); }
     135
     136float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); }
     137
     138float JoyReference_impl::RollTrim(void) const { return trim_roll; }
     139
     140float JoyReference_impl::PitchTrim(void) const { return trim_pitch; }
    140141
    141142void JoyReference_impl::Update(Time time) {
    142     input->SetDataTime(time);
    143     UpdateFrom(input);
     143  input->SetDataTime(time);
     144  UpdateFrom(input);
    144145}
    145146
    146147void JoyReference_impl::UpdateFrom(const io_data *data) {
    147     cvmatrix *input=(cvmatrix*)data;
    148 
    149     if(previous_time==0) previous_time=data->DataTime();//pour la premiere iteration
    150     float delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    151     previous_time=data->DataTime();
    152 
    153     if(button_roll->Clicked()==true) {
    154         trim_roll=0;
    155         output->SetValue(2,0,0);
    156         label_roll->SetText("trim roll: %.2f",trim_roll);
    157     }
    158     if(button_pitch->Clicked()==true) {
    159         trim_pitch=0;
    160         output->SetValue(3,0,0);
    161         label_pitch->SetText("trim pitch: %.2f",trim_pitch);
    162     }
    163 
    164     //les box sont en degrés
    165     input->GetMutex();
    166 
    167     Vector3D theta_xy(-Euler::ToRadian(input->ValueNoMutex(0,0)*deb_roll->Value()),
    168                       -Euler::ToRadian(input->ValueNoMutex(1,0)*deb_pitch->Value()),
    169                       0);
    170     Vector3D e_bar=theta_xy;
    171     e_bar.Normalize();
    172     Quaternion q_xy(cos(theta_xy.GetNorm()/2.0f),
    173                     e_bar.x*sin(theta_xy.GetNorm()/2.0f),
    174                     e_bar.y*sin(theta_xy.GetNorm()/2.0f),
    175                     0);
    176     q_xy.Normalize();
    177 
    178     float wz_ref=Euler::ToRadian(input->ValueNoMutex(2,0)*deb_wz->Value());
    179         Quaternion w_zd(1,0,0,wz_ref*delta_t/2);
    180         w_zd.Normalize();
    181         q_z = q_z*w_zd;
    182         q_z.Normalize();
    183 
    184         Quaternion q_ref = q_z*q_xy;
    185         q_ref.Normalize();
    186 
    187     z_ref+=input->ValueNoMutex(3,0)*deb_dz->Value()*delta_t;
    188     float dz_ref=input->ValueNoMutex(3,0)*deb_dz->Value();
    189 
    190     input->ReleaseMutex();
    191 
    192     ahrsData->SetQuaternionAndAngularRates(q_ref,Vector3D(0,0,wz_ref));
    193 
    194     //ouput quaternion for control law
    195     output->GetMutex();
    196     output->SetValueNoMutex(0,0,z_ref);
    197     output->SetValueNoMutex(1,0,dz_ref);
    198     output->ReleaseMutex();
    199 
    200     output->SetDataTime(data->DataTime());
    201 }
    202 
     148  cvmatrix *input = (cvmatrix *)data;
     149
     150  if (previous_time == 0)
     151    previous_time = data->DataTime(); // pour la premiere iteration
     152  float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     153  previous_time = data->DataTime();
     154
     155  if (button_roll->Clicked() == true) {
     156    trim_roll = 0;
     157    output->SetValue(2, 0, 0);
     158    label_roll->SetText("trim roll: %.2f", trim_roll);
     159  }
     160  if (button_pitch->Clicked() == true) {
     161    trim_pitch = 0;
     162    output->SetValue(3, 0, 0);
     163    label_pitch->SetText("trim pitch: %.2f", trim_pitch);
     164  }
     165
     166  // les box sont en degrés
     167  input->GetMutex();
     168
     169  Vector3D theta_xy(
     170      -Euler::ToRadian(input->ValueNoMutex(0, 0) * deb_roll->Value()),
     171      -Euler::ToRadian(input->ValueNoMutex(1, 0) * deb_pitch->Value()), 0);
     172  Vector3D e_bar = theta_xy;
     173  e_bar.Normalize();
     174  Quaternion q_xy(cos(theta_xy.GetNorm() / 2.0f),
     175                  e_bar.x * sin(theta_xy.GetNorm() / 2.0f),
     176                  e_bar.y * sin(theta_xy.GetNorm() / 2.0f), 0);
     177  q_xy.Normalize();
     178
     179  float wz_ref = Euler::ToRadian(input->ValueNoMutex(2, 0) * deb_wz->Value());
     180  Quaternion w_zd(1, 0, 0, wz_ref * delta_t / 2);
     181  w_zd.Normalize();
     182  q_z = q_z * w_zd;
     183  q_z.Normalize();
     184
     185  Quaternion q_ref = q_z * q_xy;
     186  q_ref.Normalize();
     187
     188  z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t;
     189  float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value();
     190
     191  input->ReleaseMutex();
     192
     193  ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3D(0, 0, wz_ref));
     194
     195  // ouput quaternion for control law
     196  output->GetMutex();
     197  output->SetValueNoMutex(0, 0, z_ref);
     198  output->SetValueNoMutex(1, 0, dz_ref);
     199  output->ReleaseMutex();
     200
     201  output->SetDataTime(data->DataTime());
     202}

  • trunk/lib/FlairFilter/src/LowPassFilter.cpp

    r10 r15  
    2525using namespace flair::gui;
    2626
    27 namespace flair
    28 {
    29 namespace filter
    30 {
     27namespace flair {
     28namespace filter {
    3129
    32 LowPassFilter::LowPassFilter(const IODevice* parent,const LayoutPosition* position,string name,const cvmatrix* init_value) : IODevice(parent,name)
    33 {
    34     pimpl_=new LowPassFilter_impl(this,position,name,init_value);
    35     AddDataToLog(pimpl_->output);
     30LowPassFilter::LowPassFilter(const IODevice *parent,
     31                             const LayoutPosition *position, string name,
     32                             const cvmatrix *init_value)
     33    : IODevice(parent, name) {
     34  pimpl_ = new LowPassFilter_impl(this, position, name, init_value);
     35  AddDataToLog(pimpl_->output);
    3636}
    3737
    38 LowPassFilter::~LowPassFilter()
    39 {
    40     delete pimpl_;
     38LowPassFilter::~LowPassFilter() { delete pimpl_; }
     39
     40cvmatrix *LowPassFilter::Matrix(void) const { return pimpl_->output; }
     41
     42float LowPassFilter::Output(int row, int col) const {
     43  return pimpl_->output->Value(row, col);
    4144}
    4245
    43 cvmatrix *LowPassFilter::Matrix(void) const
    44 {
    45     return pimpl_->output;
    46 }
    47 
    48 float LowPassFilter::Output(int row,int col) const
    49 {
    50    return pimpl_->output->Value(row,col);
    51 }
    52 
    53 void LowPassFilter::UpdateFrom(const io_data *data)
    54 {
    55     pimpl_->UpdateFrom(data);
    56     ProcessUpdate(pimpl_->output);
     46void LowPassFilter::UpdateFrom(const io_data *data) {
     47  pimpl_->UpdateFrom(data);
     48  ProcessUpdate(pimpl_->output);
    5749}
    5850

  • trunk/lib/FlairFilter/src/LowPassFilter.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23     }
    24     namespace gui
    25     {
    26         class Layout;
    27         class LayoutPosition;
    28     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class Layout;
     24class LayoutPosition;
     25}
    2926}
    3027
    3128class LowPassFilter_impl;
    3229
    33 namespace flair
    34 {
    35 namespace filter
    36 {
    37     /*! \class LowPassFilter
    38     *
    39     * \brief Class defining a first order low pass filter
    40     */
    41     class LowPassFilter : public core::IODevice
    42     {
    43         public:
    44             /*!
    45             * \brief Constructor
    46             *
    47             * Construct a LowPassFilter at position. \n
    48             * After calling this function, position will be deleted as it is no longer usefull. \n
    49             * The filter is automatically updated when parent's
    50             * IODevice::ProcessUpdate is called. \n
    51             * The optional init_value parameters allow to specify
    52             * the size of the input datas and its inital values.
    53             * If unspecified, a 1*1 size is used, and values are
    54             * initialized with 0.
    55             *
    56             * \param parent parent
    57             * \param position position to display settings
    58             * \param name name
    59             * \param init_value initial value
    60             */
    61             LowPassFilter(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name,const core::cvmatrix* init_value=NULL);
     30namespace flair {
     31namespace filter {
     32/*! \class LowPassFilter
     33*
     34* \brief Class defining a first order low pass filter
     35*/
     36class LowPassFilter : public core::IODevice {
     37public:
     38  /*!
     39  * \brief Constructor
     40  *
     41  * Construct a LowPassFilter at position. \n
     42  * After calling this function, position will be deleted as it is no longer
     43  *usefull. \n
     44  * The filter is automatically updated when parent's
     45  * IODevice::ProcessUpdate is called. \n
     46  * The optional init_value parameters allow to specify
     47  * the size of the input datas and its inital values.
     48  * If unspecified, a 1*1 size is used, and values are
     49  * initialized with 0.
     50  *
     51  * \param parent parent
     52  * \param position position to display settings
     53  * \param name name
     54  * \param init_value initial value
     55  */
     56  LowPassFilter(const core::IODevice *parent,
     57                const gui::LayoutPosition *position, std::string name,
     58                const core::cvmatrix *init_value = NULL);
    6259
    63             /*!
    64             * \brief Destructor
    65             *
    66             */
    67             ~LowPassFilter();
     60  /*!
     61  * \brief Destructor
     62  *
     63  */
     64  ~LowPassFilter();
    6865
    69             /*!
    70             * \brief Output value
    71             *
    72             * \param row row element
    73             * \param col column element
    74             *
    75             * \return element value
    76             */
    77             float Output(int row, int col) const;
     66  /*!
     67  * \brief Output value
     68  *
     69  * \param row row element
     70  * \param col column element
     71  *
     72  * \return element value
     73  */
     74  float Output(int row, int col) const;
    7875
    79             /*!
    80             * \brief Output matrix
    81             *
    82             * \return filtered output
    83             */
    84             core::cvmatrix* Matrix(void) const;
     76  /*!
     77  * \brief Output matrix
     78  *
     79  * \return filtered output
     80  */
     81  core::cvmatrix *Matrix(void) const;
    8582
    86         private:
    87             /*!
    88             * \brief Update using provided datas
    89             *
    90             * Reimplemented from IODevice.
    91             *
    92             * \param data data from the parent to process
    93             */
    94             void UpdateFrom(const core::io_data *data);
     83private:
     84  /*!
     85  * \brief Update using provided datas
     86  *
     87  * Reimplemented from IODevice.
     88  *
     89  * \param data data from the parent to process
     90  */
     91  void UpdateFrom(const core::io_data *data);
    9592
    96             class LowPassFilter_impl* pimpl_;
    97     };
     93  class LowPassFilter_impl *pimpl_;
     94};
    9895} // end namespace filter
    9996} // end namespace flair

  • trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp

    r10 r15  
    3131using namespace flair::filter;
    3232
    33 LowPassFilter_impl::LowPassFilter_impl(const LowPassFilter* self,const LayoutPosition* position,string name,const cvmatrix* init_value)
    34 {
    35     first_update=true;
     33LowPassFilter_impl::LowPassFilter_impl(const LowPassFilter *self,
     34                                       const LayoutPosition *position,
     35                                       string name,
     36                                       const cvmatrix *init_value) {
     37  first_update = true;
    3638
    37     if(init_value!=NULL)
    38     {
    39         prev_value=(cvmatrix*)init_value;
     39  if (init_value != NULL) {
     40    prev_value = (cvmatrix *)init_value;
     41  } else {
     42    // if NULL, assume dimension 1, and init=0
     43    cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     44    desc->SetElementName(0, 0, "output");
     45    prev_value = new cvmatrix(self, desc, floatType, name);
     46    prev_value->SetValue(0, 0, 0);
     47  }
     48
     49  // init UI
     50  GroupBox *reglages_groupbox = new GroupBox(position, name);
     51  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     52                        0, 10, 0.01);
     53  freq = new DoubleSpinBox(reglages_groupbox->NewRow(), "cutoff frequency",
     54                           " Hz", 0, 10000, 0.1, 2, 1);
     55
     56  // init output matrix of same size as init
     57  cvmatrix_descriptor *desc =
     58      new cvmatrix_descriptor(prev_value->Rows(), prev_value->Cols());
     59
     60  for (int i = 0; i < prev_value->Rows(); i++) {
     61    for (int j = 0; j < prev_value->Cols(); j++) {
     62      desc->SetElementName(i, j, prev_value->Name(i, j));
    4063    }
    41     else
    42     {
    43         //if NULL, assume dimension 1, and init=0
    44         cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
    45         desc->SetElementName(0,0,"output");
    46         prev_value=new cvmatrix(self,desc,floatType,name);
    47         prev_value->SetValue(0,0,0);
    48     }
     64  }
    4965
    50     //init UI
    51     GroupBox* reglages_groupbox=new GroupBox(position,name);
    52         T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,10,0.01);
    53         freq=new DoubleSpinBox(reglages_groupbox->NewRow(),"cutoff frequency"," Hz",0,10000,0.1,2,1);
     66  output = new cvmatrix(self, desc,
     67                        prev_value->GetDataType().GetElementDataType(), name);
    5468
    55 
    56     //init output matrix of same size as init
    57     cvmatrix_descriptor* desc=new cvmatrix_descriptor(prev_value->Rows(),prev_value->Cols());
    58 
    59     for(int i=0;i<prev_value->Rows();i++)
    60     {
    61         for(int j=0;j<prev_value->Cols();j++)
    62         {
    63             desc->SetElementName(i,j,prev_value->Name(i,j));
    64         }
    65     }
    66 
    67     output=new cvmatrix(self,desc,prev_value->GetDataType().GetElementDataType(),name);
    68 
    69     output->SetValue(0,0,0);
     69  output->SetValue(0, 0, 0);
    7070}
    7171
    72 LowPassFilter_impl::~LowPassFilter_impl()
    73 {
     72LowPassFilter_impl::~LowPassFilter_impl() {}
     73
     74void LowPassFilter_impl::UpdateFrom(const io_data *data) {
     75  float delta_t;
     76  float result;
     77  cvmatrix *input = (cvmatrix *)data;
     78
     79  // on prend une fois pour toute les mutex et on fait des accès directs
     80  output->GetMutex();
     81  input->GetMutex();
     82
     83  if (first_update == true) {
     84    for (int i = 0; i < input->Rows(); i++) {
     85      for (int j = 0; j < input->Cols(); j++) {
     86        output->SetValueNoMutex(i, j, prev_value->ValueNoMutex(i, j));
     87        prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     88      }
     89    }
     90    first_update = false;
     91  } else {
     92    if (T->Value() == 0) {
     93      delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     94    } else {
     95      delta_t = T->Value();
     96    }
     97    for (int i = 0; i < input->Rows(); i++) {
     98      for (int j = 0; j < input->Cols(); j++) {
     99
     100        if (freq->Value() != 0) {
     101          output->SetValueNoMutex(i, j, (1 - 2 * PI * freq->Value() * delta_t) *
     102                                                prev_value->ValueNoMutex(i, j) +
     103                                            2 * PI * freq->Value() * delta_t *
     104                                                input->ValueNoMutex(i, j));
     105        } else {
     106          output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     107        }
     108        prev_value->SetValueNoMutex(i, j, output->ValueNoMutex(i, j));
     109      }
     110    }
     111  }
     112  input->ReleaseMutex();
     113  output->ReleaseMutex();
     114
     115  output->SetDataTime(data->DataTime());
     116  previous_time = data->DataTime();
    74117}
    75 
    76 void LowPassFilter_impl::UpdateFrom(const io_data *data)
    77 {
    78     float delta_t;
    79     float result;
    80     cvmatrix *input=(cvmatrix*)data;
    81 
    82     //on prend une fois pour toute les mutex et on fait des accès directs
    83     output->GetMutex();
    84     input->GetMutex();
    85 
    86     if(first_update==true)
    87     {
    88         for(int i=0;i<input->Rows();i++)
    89         {
    90             for(int j=0;j<input->Cols();j++)
    91             {
    92                 output->SetValueNoMutex(i,j,prev_value->ValueNoMutex(i,j));
    93                 prev_value->SetValueNoMutex(i,j,input->ValueNoMutex(i,j));
    94             }
    95         }
    96         first_update=false;
    97     }
    98     else
    99     {
    100         if(T->Value()==0)
    101         {
    102             delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    103         }
    104         else
    105         {
    106             delta_t=T->Value();
    107         }
    108         for(int i=0;i<input->Rows();i++)
    109         {
    110             for(int j=0;j<input->Cols();j++)
    111             {
    112 
    113                 if(freq->Value()!=0)
    114                 {
    115                     output->SetValueNoMutex(i,j,(1-2*PI*freq->Value()*delta_t)*prev_value->ValueNoMutex(i,j)+2*PI*freq->Value()*delta_t*input->ValueNoMutex(i,j));
    116                 }
    117                 else
    118                 {
    119                     output->SetValueNoMutex(i,j,input->ValueNoMutex(i,j));
    120                 }
    121                 prev_value->SetValueNoMutex(i,j,output->ValueNoMutex(i,j));
    122             }
    123         }
    124     }
    125     input->ReleaseMutex();
    126     output->ReleaseMutex();
    127 
    128     output->SetDataTime(data->DataTime());
    129     previous_time=data->DataTime();
    130 }
    131 

  • trunk/lib/FlairFilter/src/NestedSat.cpp

    r10 r15  
    2727using namespace flair::gui;
    2828
    29 namespace flair
    30 {
    31 namespace filter
    32 {
     29namespace flair {
     30namespace filter {
    3331
    34 NestedSat::NestedSat(const LayoutPosition* position,string name) : ControlLaw(position->getLayout(),name)
    35 {
    36     pimpl_=new NestedSat_impl(this,position,name);
     32NestedSat::NestedSat(const LayoutPosition *position, string name)
     33    : ControlLaw(position->getLayout(), name) {
     34  pimpl_ = new NestedSat_impl(this, position, name);
    3735}
    3836
    39 NestedSat::~NestedSat(void)
    40 {
    41     delete pimpl_;
     37NestedSat::~NestedSat(void) { delete pimpl_; }
     38
     39void NestedSat::UpdateFrom(const io_data *data) {
     40  pimpl_->UpdateFrom(data);
     41  ProcessUpdate(output);
    4242}
    4343
    44 void NestedSat::UpdateFrom(const io_data *data)
    45 {
    46     pimpl_->UpdateFrom(data);
    47     ProcessUpdate(output);
     44void NestedSat::SetValues(float p_ref, float p, float d) {
     45  input->SetValue(0, 0, p_ref);
     46  input->SetValue(1, 0, p);
     47  input->SetValue(2, 0, d);
    4848}
    49 
    50 void NestedSat::SetValues(float p_ref,float p,float d)
    51 {
    52     input->SetValue(0,0,p_ref);
    53     input->SetValue(1,0,p);
    54     input->SetValue(2,0,d);
    55 }
    56 //TODO: add a combobox to choose between rad and deg
    57 void NestedSat::ConvertSatFromDegToRad(void)
    58 {
    59     pimpl_->k=Euler::ToRadian(1);
    60 }
     49// TODO: add a combobox to choose between rad and deg
     50void NestedSat::ConvertSatFromDegToRad(void) { pimpl_->k = Euler::ToRadian(1); }
    6151
    6252} // end namespace filter

  • trunk/lib/FlairFilter/src/NestedSat.h

    r10 r15  
    1616#include <ControlLaw.h>
    1717
    18 namespace flair
    19 {
    20     namespace gui
    21     {
    22         class Layout;
    23         class LayoutPosition;
    24     }
     18namespace flair {
     19namespace gui {
     20class Layout;
     21class LayoutPosition;
     22}
    2523}
    2624
    2725class NestedSat_impl;
    2826
    29 namespace flair
    30 {
    31 namespace filter
    32 {
    33     /*! \class NestedSat
    34     *
    35     * \brief  Class defining a PID with saturations
    36     *
    37     * The output of this control law is calculated
    38     * as follows: \n
    39     * p_ref=Sat(p_ref,k*sat_ref) \n
    40     * d_ref=Sat[(p_ref-p)*kp,k*sat_dref] \n
    41     * law=Sat[(d-d_ref)*kd,sat_u] \n
    42     * where p, p_ref and d are input values (see SetValues()), \n
    43     * where sat_ref, sat_dref and sat_u are settings availables on the ground station, \n
    44     * where k is a conversion factor (see ConvertSatFromDegToRad()).
    45     */
    46     class NestedSat : public ControlLaw
    47     {
    48         friend class ::NestedSat_impl;
     27namespace flair {
     28namespace filter {
     29/*! \class NestedSat
     30*
     31* \brief  Class defining a PID with saturations
     32*
     33* The output of this control law is calculated
     34* as follows: \n
     35* p_ref=Sat(p_ref,k*sat_ref) \n
     36* d_ref=Sat[(p_ref-p)*kp,k*sat_dref] \n
     37* law=Sat[(d-d_ref)*kd,sat_u] \n
     38* where p, p_ref and d are input values (see SetValues()), \n
     39* where sat_ref, sat_dref and sat_u are settings availables on the ground
     40*station, \n
     41* where k is a conversion factor (see ConvertSatFromDegToRad()).
     42*/
     43class NestedSat : public ControlLaw {
     44  friend class ::NestedSat_impl;
    4945
    50         public:
    51             /*!
    52             * \brief Constructor
    53             *
    54             * Construct a NestedSat at given place position. \n
    55             * The NestedSat will automatically be child of position->getLayout() Layout. After calling this function,
    56             * position will be deleted as it is no longer usefull. \n
    57             *
    58             * \param position position to display settings
    59             * \param name name
    60             */
    61             NestedSat(const gui::LayoutPosition* position,std::string name);
     46public:
     47  /*!
     48  * \brief Constructor
     49  *
     50  * Construct a NestedSat at given place position. \n
     51  * The NestedSat will automatically be child of position->getLayout() Layout.
     52  *After calling this function,
     53  * position will be deleted as it is no longer usefull. \n
     54  *
     55  * \param position position to display settings
     56  * \param name name
     57  */
     58  NestedSat(const gui::LayoutPosition *position, std::string name);
    6259
    63             /*!
    64             * \brief Destructor
    65             *
    66             */
    67             ~NestedSat();
     60  /*!
     61  * \brief Destructor
     62  *
     63  */
     64  ~NestedSat();
    6865
    69             /*!
    70             * \brief Set input values
    71             *
    72             * \param p_ref proportional reference
    73             * \param p proportional value
    74             * \param d derivative value
    75             */
    76             void SetValues(float p_ref,float p,float d);
     66  /*!
     67  * \brief Set input values
     68  *
     69  * \param p_ref proportional reference
     70  * \param p proportional value
     71  * \param d derivative value
     72  */
     73  void SetValues(float p_ref, float p, float d);
    7774
    78             /*!
    79             * \brief Convert saturation parameters in radians
    80             *
    81             * If this function is called, saturation parameters
    82             * on the ground station will be interpreted as degrees. \n
    83             * Thus, an internal conversion from degrees to radians will
    84             * be done (k=PI/180).
    85             */
    86             void ConvertSatFromDegToRad(void);
     75  /*!
     76  * \brief Convert saturation parameters in radians
     77  *
     78  * If this function is called, saturation parameters
     79  * on the ground station will be interpreted as degrees. \n
     80  * Thus, an internal conversion from degrees to radians will
     81  * be done (k=PI/180).
     82  */
     83  void ConvertSatFromDegToRad(void);
    8784
    88         private:
    89             /*!
    90             * \brief Update using provided datas
    91             *
    92             * Reimplemented from IODevice.
    93             *
    94             * \param data data from the parent to process
    95             */
    96             void UpdateFrom(const core::io_data *data);
     85private:
     86  /*!
     87  * \brief Update using provided datas
     88  *
     89  * Reimplemented from IODevice.
     90  *
     91  * \param data data from the parent to process
     92  */
     93  void UpdateFrom(const core::io_data *data);
    9794
    98             NestedSat_impl* pimpl_;
    99     };
     95  NestedSat_impl *pimpl_;
     96};
    10097} // end namespace filter
    10198} // end namespace flair

  • trunk/lib/FlairFilter/src/NestedSat_impl.cpp

    r10 r15  
    2828using namespace flair::filter;
    2929
    30 NestedSat_impl::NestedSat_impl(NestedSat* self,const LayoutPosition* position,string name)
    31 {
    32     this->self=self;
    33         k=1;
     30NestedSat_impl::NestedSat_impl(NestedSat *self, const LayoutPosition *position,
     31                               string name) {
     32  this->self = self;
     33  k = 1;
    3434
    35     //init matrix
    36     self->input=new cvmatrix(self,3,1,floatType,name);
     35  // init matrix
     36  self->input = new cvmatrix(self, 3, 1, floatType, name);
    3737
    38     GroupBox* reglages_groupbox=new GroupBox(position,name);
    39             sat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat ref:",0,9000000,1);
    40             kp=new DoubleSpinBox(reglages_groupbox->NewRow(),"kp:",0,9000000,1);
    41             dsat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat dref:",0,9000000,1);
    42             kd=new DoubleSpinBox(reglages_groupbox->NewRow(),"kd:",0,9000000,0.1);
    43             usat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat u:",0,1,.1);
     38  GroupBox *reglages_groupbox = new GroupBox(position, name);
     39  sat =
     40      new DoubleSpinBox(reglages_groupbox->NewRow(), "sat ref:", 0, 9000000, 1);
     41  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 9000000, 1);
     42  dsat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat dref:", 0, 9000000,
     43                           1);
     44  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 9000000, 0.1);
     45  usat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat u:", 0, 1, .1);
    4446}
    4547
    46 NestedSat_impl::~NestedSat_impl(void)
    47 {
     48NestedSat_impl::~NestedSat_impl(void) {}
     49
     50void NestedSat_impl::UpdateFrom(const io_data *data) {
     51  float cons, dcons, law;
     52  cvmatrix *input = (cvmatrix *)data;
     53
     54  input->GetMutex();
     55
     56  cons = Sat(input->ValueNoMutex(0, 0), k * sat->Value());
     57  dcons = (cons - input->ValueNoMutex(1, 0)) * kp->Value();
     58  dcons = Sat(dcons, k * dsat->Value());
     59  law = (input->ValueNoMutex(2, 0) - dcons) * kd->Value();
     60  law = Sat(law, usat->Value());
     61
     62  input->ReleaseMutex();
     63
     64  self->output->SetValue(0, 0, law);
     65  self->output->SetDataTime(data->DataTime());
    4866}
    4967
    50 void NestedSat_impl::UpdateFrom(const io_data *data)
    51 {
    52     float cons,dcons,law;
    53     cvmatrix *input=(cvmatrix*)data;
    54 
    55     input->GetMutex();
    56 
    57     cons=Sat(input->ValueNoMutex(0,0),k*sat->Value());
    58     dcons=(cons-input->ValueNoMutex(1,0))*kp->Value();
    59     dcons=Sat(dcons,k*dsat->Value());
    60     law=(input->ValueNoMutex(2,0)-dcons)*kd->Value();
    61     law=Sat(law,usat->Value());
    62 
    63     input->ReleaseMutex();
    64 
    65     self->output->SetValue(0,0,law);
    66     self->output->SetDataTime(data->DataTime());
     68float NestedSat_impl::Sat(float value, float borne) {
     69  if (value < -borne)
     70    return -borne;
     71  if (value > borne)
     72    return borne;
     73  return value;
    6774}
    68 
    69 float NestedSat_impl::Sat(float value,float borne)
    70 {
    71     if(value<-borne) return -borne;
    72     if(value>borne) return borne;
    73     return value;
    74 }

  • trunk/lib/FlairFilter/src/Pid.cpp

    r10 r15  
    2626using namespace flair::gui;
    2727
    28 namespace flair { namespace filter {
     28namespace flair {
     29namespace filter {
    2930
    30 Pid::Pid(const LayoutPosition* position,string name) : ControlLaw(position->getLayout(),name) {
    31     pimpl_=new Pid_impl(this,position,name);
     31Pid::Pid(const LayoutPosition *position, string name)
     32    : ControlLaw(position->getLayout(), name) {
     33  pimpl_ = new Pid_impl(this, position, name);
    3234}
    3335
    34 Pid::~Pid(void) {
    35     delete pimpl_;
    36 }
     36Pid::~Pid(void) { delete pimpl_; }
    3737
    38 void Pid::UseDefaultPlot(const gui::LayoutPosition* position) {
    39     pimpl_->UseDefaultPlot(position);
     38void Pid::UseDefaultPlot(const gui::LayoutPosition *position) {
     39  pimpl_->UseDefaultPlot(position);
    4040}
    4141
    4242void Pid::Reset(void) {
    43     pimpl_->i=0;
    44     pimpl_->first_update=true;
     43  pimpl_->i = 0;
     44  pimpl_->first_update = true;
    4545}
    4646
    4747void Pid::UpdateFrom(const io_data *data) {
    48     pimpl_->UpdateFrom(data);
    49     ProcessUpdate(output);
     48  pimpl_->UpdateFrom(data);
     49  ProcessUpdate(output);
    5050}
    5151
    52 void Pid::SetValues(float p,float d) {
    53     input->SetValue(0,0,p);
    54     input->SetValue(1,0,d);
     52void Pid::SetValues(float p, float d) {
     53  input->SetValue(0, 0, p);
     54  input->SetValue(1, 0, d);
    5555}
    56 
    5756
    5857} // end namespace filter

  • trunk/lib/FlairFilter/src/Pid.h

    r10 r15  
    1616#include <ControlLaw.h>
    1717
    18 namespace flair
    19 {
    20     namespace gui
    21     {
    22         class LayoutPosition;
    23     }
     18namespace flair {
     19namespace gui {
     20class LayoutPosition;
     21}
    2422}
    2523
    2624class Pid_impl;
    2725
    28 namespace flair
    29 {
    30 namespace filter
    31 {
    32     /*! \class Pid
    33     *
    34     * \brief Class defining a PID
    35     */
    36     class Pid : public ControlLaw
    37     {
    38         friend class ::Pid_impl;
     26namespace flair {
     27namespace filter {
     28/*! \class Pid
     29*
     30* \brief Class defining a PID
     31*/
     32class Pid : public ControlLaw {
     33  friend class ::Pid_impl;
    3934
    40         public:
    41             /*!
    42             * \brief Constructor
    43             *
    44             * Construct a PID at given position. \n
    45             * The PID will automatically be child of position->getLayout() Layout. After calling this function,
    46             * position will be deleted as it is no longer usefull. \n
    47             *
    48             * \param position position to display settings
    49             * \param name name
    50             */
    51             Pid(const gui::LayoutPosition* position,std::string name);
     35public:
     36  /*!
     37  * \brief Constructor
     38  *
     39  * Construct a PID at given position. \n
     40  * The PID will automatically be child of position->getLayout() Layout. After
     41  *calling this function,
     42  * position will be deleted as it is no longer usefull. \n
     43  *
     44  * \param position position to display settings
     45  * \param name name
     46  */
     47  Pid(const gui::LayoutPosition *position, std::string name);
    5248
    53             /*!
    54             * \brief Destructor
    55             *
    56             */
    57             ~Pid();
     49  /*!
     50  * \brief Destructor
     51  *
     52  */
     53  ~Pid();
    5854
    59             /*!
    60             * \brief Reset integral
    61             *
    62             */
    63             void Reset(void);
     55  /*!
     56  * \brief Reset integral
     57  *
     58  */
     59  void Reset(void);
    6460
    65             /*!
    66             * \brief Set input values
    67             *
    68             * \param p proportional value
    69             * \param d derivative value
    70             */
    71             void SetValues(float p,float d);
     61  /*!
     62  * \brief Set input values
     63  *
     64  * \param p proportional value
     65  * \param d derivative value
     66  */
     67  void SetValues(float p, float d);
    7268
    73             /*!
    74             * \brief Use default plot
    75             *
    76             * Plot the output values at position. \n
    77             * Plot consists of 4 curves: proportional part,
    78             * derivative part, integral part and
    79             * the sum of the three. \n
    80             * After calling this function, position will be deleted as it is no longer usefull. \n
    81             *
    82             * \param position position to display plot
    83             */
    84             void UseDefaultPlot(const gui::LayoutPosition* position);
     69  /*!
     70  * \brief Use default plot
     71  *
     72  * Plot the output values at position. \n
     73  * Plot consists of 4 curves: proportional part,
     74  * derivative part, integral part and
     75  * the sum of the three. \n
     76  * After calling this function, position will be deleted as it is no longer
     77  *usefull. \n
     78  *
     79  * \param position position to display plot
     80  */
     81  void UseDefaultPlot(const gui::LayoutPosition *position);
    8582
    86         private:
    87             /*!
    88             * \brief Update using provided datas
    89             *
    90             * Reimplemented from IODevice.
    91             *
    92             * \param data data from the parent to process
    93             */
    94             void UpdateFrom(const core::io_data *data);
     83private:
     84  /*!
     85  * \brief Update using provided datas
     86  *
     87  * Reimplemented from IODevice.
     88  *
     89  * \param data data from the parent to process
     90  */
     91  void UpdateFrom(const core::io_data *data);
    9592
    96             Pid_impl* pimpl_;
    97     };
     93  Pid_impl *pimpl_;
     94};
    9895} // end namespace filter
    9996} // end namespace flair

  • trunk/lib/FlairFilter/src/PidThrust.cpp

    r10 r15  
    2727using namespace flair::gui;
    2828
    29 namespace flair
    30 {
    31 namespace filter
    32 {
     29namespace flair {
     30namespace filter {
    3331
    34 PidThrust::PidThrust(const LayoutPosition* position,string name) : ControlLaw(position->getLayout(),name)
    35 {
    36     pimpl_=new PidThrust_impl(this,position,name);
     32PidThrust::PidThrust(const LayoutPosition *position, string name)
     33    : ControlLaw(position->getLayout(), name) {
     34  pimpl_ = new PidThrust_impl(this, position, name);
    3735}
    3836
    39 PidThrust::~PidThrust(void)
    40 {
    41     delete pimpl_;
    42 }
     37PidThrust::~PidThrust(void) { delete pimpl_; }
    4338
    44 void PidThrust::UseDefaultPlot(const flair::gui::LayoutPosition* position)
    45 {
    46     pimpl_->UseDefaultPlot(position);
     39void PidThrust::UseDefaultPlot(const flair::gui::LayoutPosition *position) {
     40  pimpl_->UseDefaultPlot(position);
    4741}
    4842
    4943void PidThrust::Reset(void) {
    50     pimpl_->i=0;
    51     pimpl_->offset_g=0;
     44  pimpl_->i = 0;
     45  pimpl_->offset_g = 0;
    5246}
    5347
    54 void PidThrust::ResetI(void)
    55 {
    56     pimpl_->i=0;
     48void PidThrust::ResetI(void) { pimpl_->i = 0; }
     49
     50float PidThrust::GetOffset(void) { return pimpl_->offset_g; }
     51
     52void PidThrust::UpdateFrom(const io_data *data) {
     53  pimpl_->UpdateFrom(data);
     54  ProcessUpdate(output);
    5755}
    5856
    59 float PidThrust::GetOffset(void) {
    60     return pimpl_->offset_g;
     57void PidThrust::SetValues(float p, float d) {
     58  input->SetValue(0, 0, p);
     59  input->SetValue(1, 0, d);
    6160}
    6261
    63 void PidThrust::UpdateFrom(const io_data *data)
    64 {
    65     pimpl_->UpdateFrom(data);
    66     ProcessUpdate(output);
     62void PidThrust::ResetOffset(void) { pimpl_->offset_g = 0; }
     63
     64void PidThrust::SetOffset(void) { pimpl_->offset_g = pimpl_->offset->Value(); }
     65
     66bool PidThrust::OffsetStepUp(void) {
     67  pimpl_->offset_g += pimpl_->pas_offset->Value();
     68  if (pimpl_->offset_g > 1) {
     69    pimpl_->offset_g = 1;
     70    return false;
     71  } else {
     72    return true;
     73  }
    6774}
    6875
    69 void PidThrust::SetValues(float p,float d)
    70 {
    71     input->SetValue(0,0,p);
    72     input->SetValue(1,0,d);
    73 }
    74 
    75 void PidThrust::ResetOffset(void)
    76 {
    77     pimpl_->offset_g=0;
    78 }
    79 
    80 void PidThrust::SetOffset(void)
    81 {
    82     pimpl_->offset_g=pimpl_->offset->Value();
    83 }
    84 
    85 bool PidThrust::OffsetStepUp(void)
    86 {
    87     pimpl_->offset_g+=pimpl_->pas_offset->Value();
    88     if(pimpl_->offset_g>1)
    89     {
    90         pimpl_->offset_g=1;
    91         return false;
    92     }
    93     else
    94     {
    95         return true;
    96     }
    97 }
    98 
    99 bool PidThrust::OffsetStepDown(void)
    100 {
    101     pimpl_->offset_g-=pimpl_->pas_offset->Value();
    102     if(pimpl_->offset_g<pimpl_->offset->Value())
    103     {
    104         pimpl_->offset_g=pimpl_->offset->Value();
    105         return false;
    106     }
    107     else
    108     {
    109         return true;
    110     }
     76bool PidThrust::OffsetStepDown(void) {
     77  pimpl_->offset_g -= pimpl_->pas_offset->Value();
     78  if (pimpl_->offset_g < pimpl_->offset->Value()) {
     79    pimpl_->offset_g = pimpl_->offset->Value();
     80    return false;
     81  } else {
     82    return true;
     83  }
    11184}
    11285

  • trunk/lib/FlairFilter/src/PidThrust.h

    r10 r15  
    1616#include <ControlLaw.h>
    1717
    18 namespace flair
    19 {
    20     namespace gui
    21     {
    22         class LayoutPosition;
    23     }
     18namespace flair {
     19namespace gui {
     20class LayoutPosition;
     21}
    2422}
    2523
    2624class PidThrust_impl;
    2725
    28 namespace flair
    29 {
    30 namespace filter
    31 {
    32     /*! \class PidThrust
    33     *
    34     * \brief Class defining a Pid for Thrust.\n
    35     * This Pid as an extra offset for compensating gravity.
    36     */
    37     class PidThrust : public ControlLaw
    38     {
    39         friend class ::PidThrust_impl;
     26namespace flair {
     27namespace filter {
     28/*! \class PidThrust
     29*
     30* \brief Class defining a Pid for Thrust.\n
     31* This Pid as an extra offset for compensating gravity.
     32*/
     33class PidThrust : public ControlLaw {
     34  friend class ::PidThrust_impl;
    4035
    41         public:
    42             /*!
    43             * \brief Constructor
    44             *
    45             * Construct a PidThrust at given position
    46             * The PidThrust will automatically be child of position->getLayout() Layout. After calling this function,
    47             * position will be deleted as it is no longer usefull. \n
    48             *
    49             * \param position position to display settings
    50             * \param name name
    51             */
    52             PidThrust(const gui::LayoutPosition* position,std::string name);
     36public:
     37  /*!
     38  * \brief Constructor
     39  *
     40  * Construct a PidThrust at given position
     41  * The PidThrust will automatically be child of position->getLayout() Layout.
     42  *After calling this function,
     43  * position will be deleted as it is no longer usefull. \n
     44  *
     45  * \param position position to display settings
     46  * \param name name
     47  */
     48  PidThrust(const gui::LayoutPosition *position, std::string name);
    5349
    54             /*!
    55             * \brief Destructor
    56             *
    57             */
    58             ~PidThrust();
     50  /*!
     51  * \brief Destructor
     52  *
     53  */
     54  ~PidThrust();
    5955
    60             /*!
    61             * \brief Reset integral and offset to 0
    62             *
    63             */
    64             void Reset(void);
     56  /*!
     57  * \brief Reset integral and offset to 0
     58  *
     59  */
     60  void Reset(void);
    6561
    66             /*!
    67             * \brief Reset integral to 0
    68             *
    69             */
    70             void ResetI(void);
     62  /*!
     63  * \brief Reset integral to 0
     64  *
     65  */
     66  void ResetI(void);
    7167
    72             /*!
    73             * \brief Reset offset to 0
    74             *
    75             */
    76             void ResetOffset(void);
     68  /*!
     69  * \brief Reset offset to 0
     70  *
     71  */
     72  void ResetOffset(void);
    7773
    78             /*!
    79             * \brief Set offset to specified value in ground station
    80             *
    81             */
    82             void SetOffset(void);
     74  /*!
     75  * \brief Set offset to specified value in ground station
     76  *
     77  */
     78  void SetOffset(void);
    8379
    84             /*!
    85             * \brief Get offset
    86             *
    87             * \return current offset
    88             */
    89             float GetOffset(void);
     80  /*!
     81  * \brief Get offset
     82  *
     83  * \return current offset
     84  */
     85  float GetOffset(void);
    9086
    91             /*!
    92             * \brief Step up the offset according to specified value in ground station
    93             *
    94             * \return false if offset is at its maximum (1) value, true otherwise
    95             */
    96             bool OffsetStepUp(void);
     87  /*!
     88  * \brief Step up the offset according to specified value in ground station
     89  *
     90  * \return false if offset is at its maximum (1) value, true otherwise
     91  */
     92  bool OffsetStepUp(void);
    9793
    98             /*!
    99             * \brief Step down the offset according to specified value in ground station
    100             *
    101             * \return false if offset is at its minimum (specified in ground station) value, true otherwise
    102             */
    103             bool OffsetStepDown(void);
     94  /*!
     95  * \brief Step down the offset according to specified value in ground station
     96  *
     97  * \return false if offset is at its minimum (specified in ground station)
     98  *value, true otherwise
     99  */
     100  bool OffsetStepDown(void);
    104101
    105             /*!
    106             * \brief Set input values
    107             *
    108             * \param p proportional value
    109             * \param d derivative value
    110             */
    111             void SetValues(float p,float d);
     102  /*!
     103  * \brief Set input values
     104  *
     105  * \param p proportional value
     106  * \param d derivative value
     107  */
     108  void SetValues(float p, float d);
    112109
    113             /*!
    114             * \brief Use default plot
    115             *
    116             * Plot the output values at position. \n
    117             * Plot consists of 4 curves: proportional part,
    118             * derivative part, integral part and
    119             * the sum of the three. \n
    120             * After calling this function, position will be deleted as it is no longer usefull. \n
    121             *
    122             * \param position position to display plot
    123             */
    124             void UseDefaultPlot(const gui::LayoutPosition* position);
     110  /*!
     111  * \brief Use default plot
     112  *
     113  * Plot the output values at position. \n
     114  * Plot consists of 4 curves: proportional part,
     115  * derivative part, integral part and
     116  * the sum of the three. \n
     117  * After calling this function, position will be deleted as it is no longer
     118  *usefull. \n
     119  *
     120  * \param position position to display plot
     121  */
     122  void UseDefaultPlot(const gui::LayoutPosition *position);
    125123
    126         private:
    127             /*!
    128             * \brief Update using provided datas
    129             *
    130             * Reimplemented from IODevice.
    131             *
    132             * \param data data from the parent to process
    133             */
    134             void UpdateFrom(const core::io_data *data);
     124private:
     125  /*!
     126  * \brief Update using provided datas
     127  *
     128  * Reimplemented from IODevice.
     129  *
     130  * \param data data from the parent to process
     131  */
     132  void UpdateFrom(const core::io_data *data);
    135133
    136             PidThrust_impl* pimpl_;
    137     };
     134  PidThrust_impl *pimpl_;
     135};
    138136} // end namespace filter
    139137} // end namespace flair

  • trunk/lib/FlairFilter/src/PidThrust_impl.cpp

    r10 r15  
    2828using namespace flair::filter;
    2929
    30 PidThrust_impl::PidThrust_impl(PidThrust* self,const LayoutPosition* position,string name) {
    31     i=0;
    32     offset_g=0;
    33         first_update=true;
    34         this->self=self;
     30PidThrust_impl::PidThrust_impl(PidThrust *self, const LayoutPosition *position,
     31                               string name) {
     32  i = 0;
     33  offset_g = 0;
     34  first_update = true;
     35  this->self = self;
    3536
    36     //init matrix
    37     self->input=new cvmatrix(self,2,1,floatType,name);
     37  // init matrix
     38  self->input = new cvmatrix(self, 2, 1, floatType, name);
    3839
    39     cvmatrix_descriptor* desc=new cvmatrix_descriptor(5,1);
    40     desc->SetElementName(0,0,"p");
    41         desc->SetElementName(1,0,"i");
    42         desc->SetElementName(2,0,"d");
    43         desc->SetElementName(3,0,"p+i+d");
    44         desc->SetElementName(4,0,"p+i+d+offset");
    45     state=new cvmatrix(self,desc,floatType,name);
     40  cvmatrix_descriptor *desc = new cvmatrix_descriptor(5, 1);
     41  desc->SetElementName(0, 0, "p");
     42  desc->SetElementName(1, 0, "i");
     43  desc->SetElementName(2, 0, "d");
     44  desc->SetElementName(3, 0, "p+i+d");
     45  desc->SetElementName(4, 0, "p+i+d+offset");
     46  state = new cvmatrix(self, desc, floatType, name);
    4647
    47     GroupBox* reglages_groupbox=new GroupBox(position,name);
    48             T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,1,0.01);
    49             kp=new DoubleSpinBox(reglages_groupbox->NewRow(),"kp:",0,90000000,0.01);
    50             ki=new DoubleSpinBox(reglages_groupbox->NewRow(),"ki:",0,90000000,0.01);
    51             sati=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"sat i:",0,1,0.01);
    52             kd=new DoubleSpinBox(reglages_groupbox->NewRow(),"kd:",0,90000000,0.01);
    53             offset=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"offset g:",0,1,0.01);
    54             sat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat:",0,1,0.1);
    55             pas_offset=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"offset step:",0,1,.0001,4);
     48  GroupBox *reglages_groupbox = new GroupBox(position, name);
     49  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     50                        0, 1, 0.01);
     51  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01);
     52  ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01);
     53  sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
     54                           0.01);
     55  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01);
     56  offset = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "offset g:",
     57                             0, 1, 0.01);
     58  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
     59  pas_offset = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     60                                 "offset step:", 0, 1, .0001, 4);
    5661}
    5762
    58 PidThrust_impl::~PidThrust_impl(void) {
     63PidThrust_impl::~PidThrust_impl(void) {}
     64
     65void PidThrust_impl::UseDefaultPlot(const LayoutPosition *position) {
     66  DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
     67  plot->AddCurve(state->Element(0));
     68  plot->AddCurve(state->Element(1), DataPlot::Green);
     69  plot->AddCurve(state->Element(2), DataPlot::Blue);
     70  plot->AddCurve(state->Element(3), DataPlot::Black);
     71  plot->AddCurve(state->Element(4), DataPlot::Yellow);
    5972}
    6073
    61 void PidThrust_impl::UseDefaultPlot(const LayoutPosition* position)
    62 {
    63     DataPlot1D *plot=new DataPlot1D(position,self->ObjectName(),-1,1);
    64             plot->AddCurve(state->Element(0));
    65             plot->AddCurve(state->Element(1),DataPlot::Green);
    66             plot->AddCurve(state->Element(2),DataPlot::Blue);
    67             plot->AddCurve(state->Element(3),DataPlot::Black);
    68             plot->AddCurve(state->Element(4),DataPlot::Yellow);
     74void PidThrust_impl::UpdateFrom(const io_data *data) {
     75  float p, d, total;
     76  float delta_t;
     77  cvmatrix *input = (cvmatrix *)data;
     78
     79  if (T->Value() == 0) {
     80    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     81  } else {
     82    delta_t = T->Value();
     83  }
     84  if (first_update == true) {
     85    delta_t = 0;
     86    first_update = false;
     87  }
     88
     89  input->GetMutex();
     90  p = kp->Value() * input->ValueNoMutex(0, 0);
     91  i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
     92  if (i > sati->Value())
     93    i = sati->Value();
     94  if (i < -sati->Value())
     95    i = -sati->Value();
     96  d = kd->Value() * input->ValueNoMutex(1, 0);
     97  input->ReleaseMutex();
     98
     99  total = p + i + d;
     100  if (total > sat->Value())
     101    total = sat->Value();
     102  if (total < -sat->Value())
     103    total = -sat->Value();
     104
     105  state->GetMutex();
     106  state->SetValueNoMutex(0, 0, p);
     107  state->SetValueNoMutex(1, 0, i);
     108  state->SetValueNoMutex(2, 0, d);
     109  state->SetValueNoMutex(3, 0, total);
     110  state->SetValueNoMutex(4, 0, total - offset_g * offset_g);
     111  state->ReleaseMutex();
     112
     113  //-offset_g, car on met -u_z dans le multiplex
     114  // a revoir!
     115  self->output->SetValue(0, 0, total - offset_g * offset_g);
     116  self->output->SetDataTime(data->DataTime());
     117
     118  previous_time = data->DataTime();
    69119}
    70 
    71 void PidThrust_impl::UpdateFrom(const io_data *data)
    72 {
    73     float p,d,total;
    74     float delta_t;
    75     cvmatrix *input=(cvmatrix*)data;
    76 
    77     if(T->Value()==0)
    78     {
    79         delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    80     }
    81     else
    82     {
    83         delta_t=T->Value();
    84     }
    85     if(first_update==true)
    86     {
    87         delta_t=0;
    88         first_update=false;
    89     }
    90 
    91     input->GetMutex();
    92     p=kp->Value()*input->ValueNoMutex(0,0);
    93     i+=ki->Value()*input->ValueNoMutex(0,0)*delta_t;
    94     if(i>sati->Value()) i=sati->Value();
    95     if(i<-sati->Value()) i=-sati->Value();
    96     d=kd->Value()*input->ValueNoMutex(1,0);
    97     input->ReleaseMutex();
    98 
    99     total=p+i+d;
    100     if(total>sat->Value()) total=sat->Value();
    101     if(total<-sat->Value()) total=-sat->Value();
    102 
    103     state->GetMutex();
    104     state->SetValueNoMutex(0,0,p);
    105     state->SetValueNoMutex(1,0,i);
    106     state->SetValueNoMutex(2,0,d);
    107     state->SetValueNoMutex(3,0,total);
    108     state->SetValueNoMutex(4,0,total-offset_g*offset_g);
    109     state->ReleaseMutex();
    110 
    111     //-offset_g, car on met -u_z dans le multiplex
    112     //a revoir!
    113     self->output->SetValue(0,0,total-offset_g*offset_g);
    114     self->output->SetDataTime(data->DataTime());
    115 
    116     previous_time=data->DataTime();
    117 }

  • trunk/lib/FlairFilter/src/Pid_impl.cpp

    r10 r15  
    2828using namespace flair::filter;
    2929
    30 Pid_impl::Pid_impl(Pid* self,const LayoutPosition* position,string name) {
    31     i=0;
    32     first_update=true;
    33     this->self=self;
     30Pid_impl::Pid_impl(Pid *self, const LayoutPosition *position, string name) {
     31  i = 0;
     32  first_update = true;
     33  this->self = self;
    3434
    35     //init matrix
    36     self->input=new cvmatrix(self,2,1,floatType,name);
     35  // init matrix
     36  self->input = new cvmatrix(self, 2, 1, floatType, name);
    3737
    38     cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,1);
    39     desc->SetElementName(0,0,"p");
    40     desc->SetElementName(1,0,"i");
    41     desc->SetElementName(2,0,"d");
    42     desc->SetElementName(3,0,"p+i+d");
    43     state=new cvmatrix(self,desc,floatType,name);
     38  cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 1);
     39  desc->SetElementName(0, 0, "p");
     40  desc->SetElementName(1, 0, "i");
     41  desc->SetElementName(2, 0, "d");
     42  desc->SetElementName(3, 0, "p+i+d");
     43  state = new cvmatrix(self, desc, floatType, name);
    4444
    45     GroupBox* reglages_groupbox=new GroupBox(position,name);
    46     T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,1,0.01);
    47     kp=new DoubleSpinBox(reglages_groupbox->NewRow(),"kp:",0,90000000,0.01,3);
    48     ki=new DoubleSpinBox(reglages_groupbox->NewRow(),"ki:",0,90000000,0.01,3);
    49     sati=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"sat i:",0,1,0.01);
    50     kd=new DoubleSpinBox(reglages_groupbox->NewRow(),"kd:",0,90000000,0.01,3);
    51     sat=new DoubleSpinBox(reglages_groupbox->NewRow(),"sat:",0,1,0.1);
     45  GroupBox *reglages_groupbox = new GroupBox(position, name);
     46  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     47                        0, 1, 0.01);
     48  kp = new DoubleSpinBox(reglages_groupbox->NewRow(), "kp:", 0, 90000000, 0.01,
     49                         3);
     50  ki = new DoubleSpinBox(reglages_groupbox->NewRow(), "ki:", 0, 90000000, 0.01,
     51                         3);
     52  sati = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "sat i:", 0, 1,
     53                           0.01);
     54  kd = new DoubleSpinBox(reglages_groupbox->NewRow(), "kd:", 0, 90000000, 0.01,
     55                         3);
     56  sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "sat:", 0, 1, 0.1);
    5257}
    5358
    54 Pid_impl::~Pid_impl(void) {
    55 }
     59Pid_impl::~Pid_impl(void) {}
    5660
    57 void Pid_impl::UseDefaultPlot(const LayoutPosition* position) {
    58     DataPlot1D *plot=new DataPlot1D(position,self->ObjectName(),-1,1);
    59     plot->AddCurve(state->Element(0));
    60     plot->AddCurve(state->Element(1),DataPlot::Green);
    61     plot->AddCurve(state->Element(2),DataPlot::Blue);
    62     plot->AddCurve(state->Element(3),DataPlot::Black);
     61void Pid_impl::UseDefaultPlot(const LayoutPosition *position) {
     62  DataPlot1D *plot = new DataPlot1D(position, self->ObjectName(), -1, 1);
     63  plot->AddCurve(state->Element(0));
     64  plot->AddCurve(state->Element(1), DataPlot::Green);
     65  plot->AddCurve(state->Element(2), DataPlot::Blue);
     66  plot->AddCurve(state->Element(3), DataPlot::Black);
    6367}
    6468
    6569void Pid_impl::UpdateFrom(const io_data *data) {
    66     float p,d,total;
    67     float delta_t;
    68     cvmatrix *input=(cvmatrix*)data;
     70  float p, d, total;
     71  float delta_t;
     72  cvmatrix *input = (cvmatrix *)data;
    6973
    70     if(T->Value()==0) {
    71         delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    72     } else {
    73         delta_t=T->Value();
    74     }
    75     if(first_update==true) {
    76         delta_t=0;
    77         first_update=false;
    78     }
     74  if (T->Value() == 0) {
     75    delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     76  } else {
     77    delta_t = T->Value();
     78  }
     79  if (first_update == true) {
     80    delta_t = 0;
     81    first_update = false;
     82  }
    7983
    80     input->GetMutex();
    81     p=kp->Value()*input->ValueNoMutex(0,0);
    82     i+=ki->Value()*input->ValueNoMutex(0,0)*delta_t;
    83     if(i>sati->Value()) i=sati->Value();
    84     if(i<-sati->Value()) i=-sati->Value();
    85     d=kd->Value()*input->ValueNoMutex(1,0);
    86     input->ReleaseMutex();
     84  input->GetMutex();
     85  p = kp->Value() * input->ValueNoMutex(0, 0);
     86  i += ki->Value() * input->ValueNoMutex(0, 0) * delta_t;
     87  if (i > sati->Value())
     88    i = sati->Value();
     89  if (i < -sati->Value())
     90    i = -sati->Value();
     91  d = kd->Value() * input->ValueNoMutex(1, 0);
     92  input->ReleaseMutex();
    8793
    88     total=p+i+d;
    89     if(total>sat->Value()) total=sat->Value();
    90     if(total<-sat->Value()) total=-sat->Value();
     94  total = p + i + d;
     95  if (total > sat->Value())
     96    total = sat->Value();
     97  if (total < -sat->Value())
     98    total = -sat->Value();
    9199
    92     state->GetMutex();
    93     state->SetValueNoMutex(0,0,p);
    94     state->SetValueNoMutex(1,0,i);
    95     state->SetValueNoMutex(2,0,d);
    96     state->SetValueNoMutex(3,0,total);
    97     state->ReleaseMutex();
     100  state->GetMutex();
     101  state->SetValueNoMutex(0, 0, p);
     102  state->SetValueNoMutex(1, 0, i);
     103  state->SetValueNoMutex(2, 0, d);
     104  state->SetValueNoMutex(3, 0, total);
     105  state->ReleaseMutex();
    98106
    99     self->output->SetValue(0,0,total);
    100     self->output->SetDataTime(data->DataTime());
     107  self->output->SetValue(0, 0, total);
     108  self->output->SetDataTime(data->DataTime());
    101109
    102     previous_time=data->DataTime();
     110  previous_time = data->DataTime();
    103111}

  • trunk/lib/FlairFilter/src/SimuAhrs.cpp

    r10 r15  
    2525using namespace flair::sensor;
    2626
    27 namespace flair { namespace filter {
     27namespace flair {
     28namespace filter {
    2829
    29 SimuAhrs::SimuAhrs(const FrameworkManager* parent,string name,uint32_t dev_id,uint8_t priority) :Ahrs(new SimuImu(parent,name,dev_id,priority),name) {
    30 }
     30SimuAhrs::SimuAhrs(const FrameworkManager *parent, string name, uint32_t dev_id,
     31                   uint8_t priority)
     32    : Ahrs(new SimuImu(parent, name, dev_id, priority), name) {}
    3133
    3234SimuAhrs::~SimuAhrs() {}
    3335
    34 void SimuAhrs::Start(void) {
    35     ((SimuImu*)GetImu())->Start();
    36 }
     36void SimuAhrs::Start(void) { ((SimuImu *)GetImu())->Start(); }
    3737
    38 //datas from SimuImu are AhrsData!
     38// datas from SimuImu are AhrsData!
    3939void SimuAhrs::UpdateFrom(const io_data *data) {
    40     AhrsData *input=(AhrsData*)data;
    41     AhrsData *output;
    42     GetDatas(&output);
     40  AhrsData *input = (AhrsData *)data;
     41  AhrsData *output;
     42  GetDatas(&output);
    4343
    44     Quaternion quaternion;
    45     Vector3D filteredAngRates;
    46     input->GetQuaternionAndAngularRates(quaternion,filteredAngRates);
    47     output->SetQuaternionAndAngularRates(quaternion,filteredAngRates);
    48     output->SetDataTime(input->DataTime());
     44  Quaternion quaternion;
     45  Vector3D filteredAngRates;
     46  input->GetQuaternionAndAngularRates(quaternion, filteredAngRates);
     47  output->SetQuaternionAndAngularRates(quaternion, filteredAngRates);
     48  output->SetDataTime(input->DataTime());
    4949
    50     ProcessUpdate(output);
     50  ProcessUpdate(output);
    5151}
    5252

  • trunk/lib/FlairFilter/src/SimuAhrs.h

    r10 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21 namespace filter
    22 {
    23     /*! \class SimuAhrs
    24     *
    25     * \brief Class for a simulation Ahrs
    26     *
    27     * This class constructs a SimuImu as Imu of this Ahrs.
    28     */
    29     class SimuAhrs : public filter::Ahrs
    30     {
    31         public:
    32             /*!
    33             * \brief Constructor
    34             *
    35             * Construct a simulation Ahrs.
    36             *
    37             * \param parent parent
    38             * \param name name
    39             * \param dev_id number id of the simulated Ahrs
    40             * \param priority priority of the SimuImu Thread
    41             */
    42             SimuAhrs(const core::FrameworkManager* parent,std::string name,uint32_t dev_id,uint8_t priority);
     19namespace flair {
     20namespace filter {
     21/*! \class SimuAhrs
     22*
     23* \brief Class for a simulation Ahrs
     24*
     25* This class constructs a SimuImu as Imu of this Ahrs.
     26*/
     27class SimuAhrs : public filter::Ahrs {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct a simulation Ahrs.
     33  *
     34  * \param parent parent
     35  * \param name name
     36  * \param dev_id number id of the simulated Ahrs
     37  * \param priority priority of the SimuImu Thread
     38  */
     39  SimuAhrs(const core::FrameworkManager *parent, std::string name,
     40           uint32_t dev_id, uint8_t priority);
    4341
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~SimuAhrs();
     42  /*!
     43  * \brief Destructor
     44  *
     45  */
     46  ~SimuAhrs();
    4947
    50              /*!
    51             * \brief Start SimuImu Thread
    52             *
    53             */
    54             void Start(void);
     48  /*!
     49 * \brief Start SimuImu Thread
     50 *
     51 */
     52  void Start(void);
    5553
    56         private:
    57             /*!
    58             * \brief Update using provided datas
    59             *
    60             * Reimplemented from IODevice.
    61             *
    62             * \param data data from the parent to process
    63             */
    64             void UpdateFrom(const core::io_data *data);
    65     };
     54private:
     55  /*!
     56  * \brief Update using provided datas
     57  *
     58  * Reimplemented from IODevice.
     59  *
     60  * \param data data from the parent to process
     61  */
     62  void UpdateFrom(const core::io_data *data);
     63};
    6664} // end namespace filter
    6765} // end namespace flair

  • trunk/lib/FlairFilter/src/TrajectoryGenerator1D.cpp

    r10 r15  
    2626using namespace flair::gui;
    2727
    28 namespace flair { namespace filter {
     28namespace flair {
     29namespace filter {
    2930
    30 TrajectoryGenerator1D::TrajectoryGenerator1D(const LayoutPosition* position,string name,string unit) : IODevice(position->getLayout(),name) {
    31     pimpl_=new TrajectoryGenerator1D_impl(this,position,name,unit);
    32     AddDataToLog(pimpl_->output);
     31TrajectoryGenerator1D::TrajectoryGenerator1D(const LayoutPosition *position,
     32                                             string name, string unit)
     33    : IODevice(position->getLayout(), name) {
     34  pimpl_ = new TrajectoryGenerator1D_impl(this, position, name, unit);
     35  AddDataToLog(pimpl_->output);
    3336}
    3437
    35 TrajectoryGenerator1D::~TrajectoryGenerator1D() {
    36     delete pimpl_;
     38TrajectoryGenerator1D::~TrajectoryGenerator1D() { delete pimpl_; }
     39
     40cvmatrix *TrajectoryGenerator1D::Matrix(void) const { return pimpl_->output; }
     41
     42void TrajectoryGenerator1D::StartTraj(float start_pos, float end_pos) {
     43  pimpl_->StartTraj(start_pos, end_pos);
    3744}
    3845
    39 cvmatrix *TrajectoryGenerator1D::Matrix(void) const {
    40     return pimpl_->output;
    41 }
    42 
    43 void TrajectoryGenerator1D::StartTraj(float start_pos,float end_pos) {
    44     pimpl_->StartTraj(start_pos,end_pos);
    45 }
    46 
    47 //revoir l'interet du stop?
    48 void TrajectoryGenerator1D::StopTraj(void) {
    49     pimpl_->StopTraj();
    50 }
     46// revoir l'interet du stop?
     47void TrajectoryGenerator1D::StopTraj(void) { pimpl_->StopTraj(); }
    5148
    5249bool TrajectoryGenerator1D::IsRunning(void) const {
    53     if(pimpl_->is_started==true) {
    54         if(pimpl_->is_finished==true) {
    55             return false;
    56         } else {
    57             return true;
    58         }
     50  if (pimpl_->is_started == true) {
     51    if (pimpl_->is_finished == true) {
     52      return false;
    5953    } else {
    60         return false;
     54      return true;
    6155    }
     56  } else {
     57    return false;
     58  }
    6259}
    6360
    6461float TrajectoryGenerator1D::Position(void) const {
    65     return pimpl_->output->Value(0,0);
     62  return pimpl_->output->Value(0, 0);
    6663}
    6764
    6865void TrajectoryGenerator1D::Reset(void) {
    69     if(IsRunning()==false) {
    70         pimpl_->Reset();
    71     } else {
    72         Err("impossible while running\n");
    73     }
     66  if (IsRunning() == false) {
     67    pimpl_->Reset();
     68  } else {
     69    Err("impossible while running\n");
     70  }
    7471}
    7572
    7673void TrajectoryGenerator1D::SetPositionOffset(float value) {
    77     pimpl_->pos_off=value;
     74  pimpl_->pos_off = value;
    7875}
    7976
    8077float TrajectoryGenerator1D::Speed(void) const {
    81     return pimpl_->output->Value(1,0);
     78  return pimpl_->output->Value(1, 0);
    8279}
    8380
    8481void TrajectoryGenerator1D::SetSpeedOffset(float value) {
    85     pimpl_->vel_off=value;
     82  pimpl_->vel_off = value;
    8683}
    8784
    8885void TrajectoryGenerator1D::Update(Time time) {
    89     pimpl_->Update(time);
    90     ProcessUpdate(pimpl_->output);
     86  pimpl_->Update(time);
     87  ProcessUpdate(pimpl_->output);
    9188}
    9289

  • trunk/lib/FlairFilter/src/TrajectoryGenerator1D.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23     }
    24     namespace gui
    25     {
    26         class LayoutPosition;
    27     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class LayoutPosition;
     24}
    2825}
    2926
    3027class TrajectoryGenerator1D_impl;
    3128
    32 namespace flair
    33 {
    34 namespace filter
    35 {
    36     /*! \class TrajectoryGenerator1D
    37     *
    38     * \brief Class generating a trajectory in 1D
    39     *
    40     * This class generates position and velocity references, given
    41     * an absolute maximum velocity and an absolute acceleration. \n
    42     * When trajectory is started (see StartTraj()), position and velocity will increase
    43     * at the given acceleration. If the maximum velocity is reached,
    44     * velocity will not increase anymore. Then, velocity will decrease according
    45     * to the given acceleration until velocity is null and final position is reached. \n
    46     * Manual inputs can be introduced using SetPositionOffset() and SetSpeedOffset().
    47     *
    48     */
    49     class TrajectoryGenerator1D : public core::IODevice
    50     {
    51         public:
    52             /*!
    53             * \brief Constructor
    54             *
    55             * Construct a TrajectoryGenerator1D at given position. \n
    56             * The TrajectoryGenerator1D will automatically be child of position->getLayout() Layout. After calling this function,
    57             * position will be deleted as it is no longer usefull. \n
    58             *
    59             * \param position position to display settings
    60             * \param name name
    61             * \param unit unit of the position (for exemple m, deg, etc). Its only used for display on ground station.
    62             */
    63             TrajectoryGenerator1D(const gui::LayoutPosition* position,std::string name,std::string unit="");
     29namespace flair {
     30namespace filter {
     31/*! \class TrajectoryGenerator1D
     32*
     33* \brief Class generating a trajectory in 1D
     34*
     35* This class generates position and velocity references, given
     36* an absolute maximum velocity and an absolute acceleration. \n
     37* When trajectory is started (see StartTraj()), position and velocity will
     38*increase
     39* at the given acceleration. If the maximum velocity is reached,
     40* velocity will not increase anymore. Then, velocity will decrease according
     41* to the given acceleration until velocity is null and final position is
     42*reached. \n
     43* Manual inputs can be introduced using SetPositionOffset() and
     44*SetSpeedOffset().
     45*
     46*/
     47class TrajectoryGenerator1D : public core::IODevice {
     48public:
     49  /*!
     50  * \brief Constructor
     51  *
     52  * Construct a TrajectoryGenerator1D at given position. \n
     53  * The TrajectoryGenerator1D will automatically be child of
     54  *position->getLayout() Layout. After calling this function,
     55  * position will be deleted as it is no longer usefull. \n
     56  *
     57  * \param position position to display settings
     58  * \param name name
     59  * \param unit unit of the position (for exemple m, deg, etc). Its only used
     60  *for display on ground station.
     61  */
     62  TrajectoryGenerator1D(const gui::LayoutPosition *position, std::string name,
     63                        std::string unit = "");
    6464
    65             /*!
    66             * \brief Destructor
    67             *
    68             */
    69             ~TrajectoryGenerator1D();
     65  /*!
     66  * \brief Destructor
     67  *
     68  */
     69  ~TrajectoryGenerator1D();
    7070
    71             /*!
    72             * \brief Start trajectory
    73             *
    74             * \param start_pos start position
    75             * \param end_pos end position
    76             */
    77             void StartTraj(float start_pos,float end_pos);
     71  /*!
     72  * \brief Start trajectory
     73  *
     74  * \param start_pos start position
     75  * \param end_pos end position
     76  */
     77  void StartTraj(float start_pos, float end_pos);
    7878
    79             /*!
    80             * \brief Stop trajectory
    81             *
    82             */
    83             void StopTraj(void);
     79  /*!
     80  * \brief Stop trajectory
     81  *
     82  */
     83  void StopTraj(void);
    8484
    85             /*!
    86             * \brief Reset
    87             *
    88             * Reset all outputs to 0. This can be done only when IsRunning()==false.
    89             *
    90             */
    91             void Reset(void);
     85  /*!
     86  * \brief Reset
     87  *
     88  * Reset all outputs to 0. This can be done only when IsRunning()==false.
     89  *
     90  */
     91  void Reset(void);
    9292
    93             /*!
    94             * \brief Is trajectory running?
    95             *
    96             * \return true if trajectory is running
    97             */
    98             bool IsRunning(void) const;
     93  /*!
     94  * \brief Is trajectory running?
     95  *
     96  * \return true if trajectory is running
     97  */
     98  bool IsRunning(void) const;
    9999
    100             /*!
    101             * \brief Set position offset
    102             *
    103             * \param value position offset
    104             */
    105             void SetPositionOffset(float value);
     100  /*!
     101  * \brief Set position offset
     102  *
     103  * \param value position offset
     104  */
     105  void SetPositionOffset(float value);
    106106
    107             /*!
    108             * \brief Set speed offset
    109             *
    110             * \param value speed offset
    111             */
    112             void SetSpeedOffset(float value);
     107  /*!
     108  * \brief Set speed offset
     109  *
     110  * \param value speed offset
     111  */
     112  void SetSpeedOffset(float value);
    113113
    114             /*!
    115             * \brief Update using provided datas
    116             *
    117             * Uses values specified by StartTraj(),
    118             * SetPositionOffset() and SetSpeedOffset().
    119             *
    120             * \param time time of the update
    121             */
    122             void Update(core::Time time);
     114  /*!
     115  * \brief Update using provided datas
     116  *
     117  * Uses values specified by StartTraj(),
     118  * SetPositionOffset() and SetSpeedOffset().
     119  *
     120  * \param time time of the update
     121  */
     122  void Update(core::Time time);
    123123
    124             /*!
    125             * \brief Position
    126             *
    127             */
    128             float Position(void) const;
     124  /*!
     125  * \brief Position
     126  *
     127  */
     128  float Position(void) const;
    129129
    130             /*!
    131             * \brief Speed
    132             *
    133             */
    134             float Speed(void) const;
     130  /*!
     131  * \brief Speed
     132  *
     133  */
     134  float Speed(void) const;
    135135
    136             /*!
    137             * \brief Output matrix
    138             *
    139             * \return matrix
    140             */
    141             core::cvmatrix *Matrix(void) const;
     136  /*!
     137  * \brief Output matrix
     138  *
     139  * \return matrix
     140  */
     141  core::cvmatrix *Matrix(void) const;
    142142
    143         private:
    144             /*!
    145             * \brief Update using provided datas
    146             *
    147             * Reimplemented from IODevice. Nothing to do in this IODevice.
    148             *
    149             * \param data data from the parent to process
    150             */
    151             void UpdateFrom(const core::io_data *data){};
     143private:
     144  /*!
     145  * \brief Update using provided datas
     146  *
     147  * Reimplemented from IODevice. Nothing to do in this IODevice.
     148  *
     149  * \param data data from the parent to process
     150  */
     151  void UpdateFrom(const core::io_data *data){};
    152152
    153             TrajectoryGenerator1D_impl* pimpl_;
    154     };
     153  TrajectoryGenerator1D_impl *pimpl_;
     154};
    155155} // end namespace filter
    156156} // end namespace flair

  • trunk/lib/FlairFilter/src/TrajectoryGenerator1D_impl.cpp

    r10 r15  
    2929using namespace flair::filter;
    3030
    31 TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(TrajectoryGenerator1D* self,const LayoutPosition* position,string name,string unit) {
    32     first_update=true;
    33     is_started=false;
     31TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(
     32    TrajectoryGenerator1D *self, const LayoutPosition *position, string name,
     33    string unit) {
     34  first_update = true;
     35  is_started = false;
    3436
    35     //init UI
    36     GroupBox* reglages_groupbox=new GroupBox(position,name);
    37     T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto:"," s",0,1,0.01);
    38     if(unit=="") {
    39         max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):",0.,200000,1);
    40     } else {
    41         max_veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity max (absolute):"," "+ unit+"/s",0.,200000,1);
    42     }
    43     if(unit=="") {
    44         acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):",0.,10,1,3);
    45     } else {
    46         acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute):"," "+unit +"/s²",0.,200000,1);
    47     }
     37  // init UI
     38  GroupBox *reglages_groupbox = new GroupBox(position, name);
     39  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
     40                        " s", 0, 1, 0.01);
     41  if (unit == "") {
     42    max_veloctity =
     43        new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     44                          "velocity max (absolute):", 0., 200000, 1);
     45  } else {
     46    max_veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     47                                      "velocity max (absolute):",
     48                                      " " + unit + "/s", 0., 200000, 1);
     49  }
     50  if (unit == "") {
     51    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     52                                     "acceleration (absolute):", 0., 10, 1, 3);
     53  } else {
     54    acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     55                                     "acceleration (absolute):",
     56                                     " " + unit + "/s²", 0., 200000, 1);
     57  }
    4858
    49     Reset();
     59  Reset();
    5060
    51     //init matrix
    52     cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
    53     desc->SetElementName(0,0,"pos");
    54     desc->SetElementName(1,0,"vel");
    55     output=new cvmatrix(self,desc,floatType,name);
     61  // init matrix
     62  cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 1);
     63  desc->SetElementName(0, 0, "pos");
     64  desc->SetElementName(1, 0, "vel");
     65  output = new cvmatrix(self, desc, floatType, name);
    5666
    57     output->SetValue(0,0,pos);
    58     output->SetValue(1,0,v);
     67  output->SetValue(0, 0, pos);
     68  output->SetValue(1, 0, v);
    5969}
    6070
    61 TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {
     71TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}
     72
     73void TrajectoryGenerator1D_impl::Reset(void) {
     74  pos = 0;
     75  v = 0;
     76  pos_off = 0;
     77  vel_off = 0;
    6278}
    6379
    64 void TrajectoryGenerator1D_impl::Reset(void) {
    65     pos=0;
    66     v=0;
    67     pos_off=0;
    68     vel_off=0;
     80void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
     81  is_started = true;
     82  is_finished = false;
     83  first_update = true;
     84
     85  // configure trajectory
     86  end_position = end_pos;
     87  pos = start_pos;
     88  acc = acceleration->Value();
     89  v = 0;
     90  if (end_position < start_pos) {
     91    acc = -acc;
     92    // max_veloctity=-max_veloctity;
     93  }
    6994}
    7095
    71 void TrajectoryGenerator1D_impl::StartTraj(float start_pos,float end_pos) {
    72     is_started=true;
    73     is_finished=false;
    74     first_update=true;
    75 
    76     //configure trajectory
    77     end_position=end_pos;
    78     pos=start_pos;
    79     acc=acceleration->Value();
    80     v=0;
    81     if(end_position<start_pos) {
    82         acc=-acc;
    83         //max_veloctity=-max_veloctity;
    84     }
    85 }
    86 
    87 //revoir l'interet du stop?
     96// revoir l'interet du stop?
    8897void TrajectoryGenerator1D_impl::StopTraj(void) {
    89     is_started=false;
    90     v=0;
    91     //output->SetValue(1,0,v);
     98  is_started = false;
     99  v = 0;
     100  // output->SetValue(1,0,v);
    92101}
    93102
    94103void TrajectoryGenerator1D_impl::Update(Time time) {
    95     float delta_t;
     104  float delta_t;
    96105
    97     if(T->Value()==0) {
    98         if(first_update==true) {
    99             first_update=false;
    100             previous_time=time;
    101             output->GetMutex();
    102             output->SetValueNoMutex(0,0,pos+pos_off);
    103             output->SetValueNoMutex(1,0,v+vel_off);
    104             output->ReleaseMutex();
     106  if (T->Value() == 0) {
     107    if (first_update == true) {
     108      first_update = false;
     109      previous_time = time;
     110      output->GetMutex();
     111      output->SetValueNoMutex(0, 0, pos + pos_off);
     112      output->SetValueNoMutex(1, 0, v + vel_off);
     113      output->ReleaseMutex();
    105114
    106             output->SetDataTime(time);
    107             return;
    108         } else {
    109             delta_t=(float)(time-previous_time)/1000000000.;
    110         }
     115      output->SetDataTime(time);
     116      return;
    111117    } else {
    112         delta_t=T->Value();
     118      delta_t = (float)(time - previous_time) / 1000000000.;
    113119    }
    114     previous_time=time;
     120  } else {
     121    delta_t = T->Value();
     122  }
     123  previous_time = time;
    115124
     125  if (is_started == true) {
     126    if (is_finished == false) {
     127      v += acc * delta_t;
     128      if (fabs(v) > fabs(max_veloctity->Value())) {
     129        if (v > 0)
     130          v = max_veloctity->Value();
     131        else
     132          v = -max_veloctity->Value();
     133      }
     134      pos += v * delta_t;
     135      if (end_position - v * v / (2 * acc) <= pos && v >= 0)
     136        acc = -acc;
     137      if (end_position - v * v / (2 * acc) >= pos && v < 0)
     138        acc = -acc;
     139      if (pos >= end_position && v >= 0)
     140        is_finished = true;
     141      if (pos <= end_position && v < 0)
     142        is_finished = true;
     143    }
     144    // else
     145    if (is_finished == true) {
     146      v = 0;
     147      pos = end_position;
     148    }
     149  }
    116150
    117     if(is_started==true) {
    118         if(is_finished==false) {
    119             v+=acc*delta_t;
    120             if(fabs(v)>fabs(max_veloctity->Value())) {
    121                 if(v>0)
    122                     v=max_veloctity->Value();
    123                 else
    124                     v=-max_veloctity->Value();
    125             }
    126             pos+=v*delta_t;
    127             if(end_position-v*v/(2*acc)<=pos && v>=0) acc=-acc;
    128             if(end_position-v*v/(2*acc)>=pos && v<0) acc=-acc;
    129             if(pos>=end_position && v>=0) is_finished=true;
    130             if(pos<=end_position && v<0) is_finished=true;
    131         }
    132         //else
    133         if(is_finished==true) {
    134             v=0;
    135             pos=end_position;
    136         }
    137     }
     151  // on prend une fois pour toute les mutex et on fait des accès directs
     152  output->GetMutex();
     153  output->SetValueNoMutex(0, 0, pos + pos_off);
     154  output->SetValueNoMutex(1, 0, v + vel_off);
     155  output->ReleaseMutex();
    138156
    139     //on prend une fois pour toute les mutex et on fait des accès directs
    140     output->GetMutex();
    141     output->SetValueNoMutex(0,0,pos+pos_off);
    142     output->SetValueNoMutex(1,0,v+vel_off);
    143     output->ReleaseMutex();
    144 
    145     output->SetDataTime(time);
     157  output->SetDataTime(time);
    146158}

  • trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle.cpp

    r10 r15  
    2727using namespace flair::gui;
    2828
    29 namespace flair
    30 {
    31 namespace filter
    32 {
     29namespace flair {
     30namespace filter {
    3331
    34 TrajectoryGenerator2DCircle::TrajectoryGenerator2DCircle(const LayoutPosition* position,string name) : IODevice(position->getLayout(),name)
    35 {
    36     pimpl_=new TrajectoryGenerator2DCircle_impl(this,position,name);
    37     AddDataToLog(pimpl_->output);
     32TrajectoryGenerator2DCircle::TrajectoryGenerator2DCircle(
     33    const LayoutPosition *position, string name)
     34    : IODevice(position->getLayout(), name) {
     35  pimpl_ = new TrajectoryGenerator2DCircle_impl(this, position, name);
     36  AddDataToLog(pimpl_->output);
    3837}
    3938
     39TrajectoryGenerator2DCircle::~TrajectoryGenerator2DCircle() { delete pimpl_; }
    4040
    41 TrajectoryGenerator2DCircle::~TrajectoryGenerator2DCircle()
    42 {
    43     delete pimpl_;
     41bool TrajectoryGenerator2DCircle::IsRunning(void) const {
     42  return pimpl_->is_running;
    4443}
    4544
    46 bool TrajectoryGenerator2DCircle::IsRunning(void) const
    47 {
    48     return pimpl_->is_running;
     45cvmatrix *TrajectoryGenerator2DCircle::Matrix(void) const {
     46  return pimpl_->output;
    4947}
    5048
    51 cvmatrix *TrajectoryGenerator2DCircle::Matrix(void) const
    52 {
    53     return pimpl_->output;
     49void TrajectoryGenerator2DCircle::StartTraj(const Vector2D &start_pos,
     50                                            float nb_lap) {
     51  pimpl_->StartTraj(start_pos, nb_lap);
    5452}
    5553
    56 void TrajectoryGenerator2DCircle::StartTraj(const Vector2D &start_pos,float nb_lap)
    57 {
    58     pimpl_->StartTraj(start_pos,nb_lap);
     54void TrajectoryGenerator2DCircle::FinishTraj(void) { pimpl_->FinishTraj(); }
     55
     56void TrajectoryGenerator2DCircle::StopTraj(void) { pimpl_->is_running = false; }
     57
     58void TrajectoryGenerator2DCircle::GetPosition(Vector2D &point) const {
     59  point.x = pimpl_->output->Value(0, 0);
     60  point.y = pimpl_->output->Value(0, 1);
    5961}
    6062
    61 void TrajectoryGenerator2DCircle::FinishTraj(void)
    62 {
    63     pimpl_->FinishTraj();
     63void TrajectoryGenerator2DCircle::SetCenter(const Vector2D &value) {
     64  pimpl_->pos_off = value;
    6465}
    6566
    66 void TrajectoryGenerator2DCircle::StopTraj(void)
    67 {
    68     pimpl_->is_running=false;
     67void TrajectoryGenerator2DCircle::GetSpeed(Vector2D &point) const {
     68  point.x = pimpl_->output->Value(1, 0);
     69  point.y = pimpl_->output->Value(1, 1);
    6970}
    7071
    71 void TrajectoryGenerator2DCircle::GetPosition(Vector2D &point) const
    72 {
    73     point.x=pimpl_->output->Value(0,0);
    74     point.y=pimpl_->output->Value(0,1);
     72void TrajectoryGenerator2DCircle::SetCenterSpeed(const Vector2D &value) {
     73  pimpl_->vel_off = value;
    7574}
    7675
    77 void TrajectoryGenerator2DCircle::SetCenter(const Vector2D &value)
    78 {
    79     pimpl_->pos_off=value;
    80 }
    81 
    82 void TrajectoryGenerator2DCircle::GetSpeed(Vector2D &point) const
    83 {
    84     point.x=pimpl_->output->Value(1,0);
    85     point.y=pimpl_->output->Value(1,1);
    86 }
    87 
    88 void TrajectoryGenerator2DCircle::SetCenterSpeed(const Vector2D &value)
    89 {
    90     pimpl_->vel_off=value;
    91 }
    92 
    93 void TrajectoryGenerator2DCircle::Update(Time time)
    94 {
    95     pimpl_->Update(time);
    96     ProcessUpdate(pimpl_->output);
     76void TrajectoryGenerator2DCircle::Update(Time time) {
     77  pimpl_->Update(time);
     78  ProcessUpdate(pimpl_->output);
    9779}
    9880

  • trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23         class Vector2D;
    24     }
    25     namespace gui
    26     {
    27         class LayoutPosition;
    28     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21class Vector2D;
     22}
     23namespace gui {
     24class LayoutPosition;
     25}
    2926}
    3027
    3128class TrajectoryGenerator2DCircle_impl;
    3229
    33 namespace flair
    34 {
    35 namespace filter
    36 {
    37     /*! \class TrajectoryGenerator2DCircle
    38     *
    39     * \brief Class generating a circle trajectory in 2D
    40     *
    41     * This class generates position and velocity references, given
    42     * a maximum velocity and an absolute acceleration, for a circle. \n
    43     * When trajectory is started (StartTraj), position and velocity will increase
    44     * at the given acceleration untill maximum velocity is reached. \n
    45     * When trajectory is asked to be finished (see FinishTraj()), position
    46     * and velocity will decrease at the given acceleration untill null velocity is reached. \n
    47     * Position and velocity of the center of the circle can be manually changed
    48     * through SetCenter() and SetCenterSpeed().
    49     */
    50     class TrajectoryGenerator2DCircle : public core::IODevice
    51     {
    52         public:
    53             /*!
    54             * \brief Constructor
    55             *
    56             * Construct a TrajectoryGenerator2DCircle at position. \n
    57             * The TrajectoryGenerator2DCircle will automatically be child of position->getLayout() Layout. After calling this function,
    58             * position will be deleted as it is no longer usefull. \n
    59             *
    60             * \param position position to display settings
    61             * \param name name
    62             */
    63             TrajectoryGenerator2DCircle(const gui::LayoutPosition* position,std::string name);
     30namespace flair {
     31namespace filter {
     32/*! \class TrajectoryGenerator2DCircle
     33*
     34* \brief Class generating a circle trajectory in 2D
     35*
     36* This class generates position and velocity references, given
     37* a maximum velocity and an absolute acceleration, for a circle. \n
     38* When trajectory is started (StartTraj), position and velocity will increase
     39* at the given acceleration untill maximum velocity is reached. \n
     40* When trajectory is asked to be finished (see FinishTraj()), position
     41* and velocity will decrease at the given acceleration untill null velocity is
     42*reached. \n
     43* Position and velocity of the center of the circle can be manually changed
     44* through SetCenter() and SetCenterSpeed().
     45*/
     46class TrajectoryGenerator2DCircle : public core::IODevice {
     47public:
     48  /*!
     49  * \brief Constructor
     50  *
     51  * Construct a TrajectoryGenerator2DCircle at position. \n
     52  * The TrajectoryGenerator2DCircle will automatically be child of
     53  *position->getLayout() Layout. After calling this function,
     54  * position will be deleted as it is no longer usefull. \n
     55  *
     56  * \param position position to display settings
     57  * \param name name
     58  */
     59  TrajectoryGenerator2DCircle(const gui::LayoutPosition *position,
     60                              std::string name);
    6461
    65             /*!
    66             * \brief Destructor
    67             *
    68             */
    69             ~TrajectoryGenerator2DCircle();
     62  /*!
     63  * \brief Destructor
     64  *
     65  */
     66  ~TrajectoryGenerator2DCircle();
    7067
    71             /*!
    72             * \brief Start trajectory
    73             *
    74             * \param start_pos start position
    75             * \param nb_lap number of laps, -1 for infinite
    76             */
    77             void StartTraj(const core::Vector2D &start_pos,float nb_lap=-1);
     68  /*!
     69  * \brief Start trajectory
     70  *
     71  * \param start_pos start position
     72  * \param nb_lap number of laps, -1 for infinite
     73  */
     74  void StartTraj(const core::Vector2D &start_pos, float nb_lap = -1);
    7875
    79             /*!
    80             * \brief Stop trajectory
    81             *
    82             * Stop abruptly the trajectory.
    83             */
    84             void StopTraj(void);
     76  /*!
     77  * \brief Stop trajectory
     78  *
     79  * Stop abruptly the trajectory.
     80  */
     81  void StopTraj(void);
    8582
    86             /*!
    87             * \brief Finish trajectory
    88             *
    89             * Finish smoothly the trajectory.
    90             */
    91             void FinishTraj(void);
     83  /*!
     84  * \brief Finish trajectory
     85  *
     86  * Finish smoothly the trajectory.
     87  */
     88  void FinishTraj(void);
    9289
    93             /*!
    94             * \brief Set center position
    95             *
    96             * \param value center position
    97             */
    98             void SetCenter(const core::Vector2D &value);
     90  /*!
     91  * \brief Set center position
     92  *
     93  * \param value center position
     94  */
     95  void SetCenter(const core::Vector2D &value);
    9996
    100             /*!
    101             * \brief Set center speed
    102             *
    103             * \param value center speed
    104             */
    105             void SetCenterSpeed(const core::Vector2D &value);
     97  /*!
     98  * \brief Set center speed
     99  *
     100  * \param value center speed
     101  */
     102  void SetCenterSpeed(const core::Vector2D &value);
    106103
    107             /*!
    108             * \brief Update using provided datas
    109             *
    110             * Uses values specified by StartTraj(),
    111             * SetCenter() and SetCenterSpeed().
    112             *
    113             * \param time time of the update
    114             */
    115             void Update(core::Time time);
     104  /*!
     105  * \brief Update using provided datas
     106  *
     107  * Uses values specified by StartTraj(),
     108  * SetCenter() and SetCenterSpeed().
     109  *
     110  * \param time time of the update
     111  */
     112  void Update(core::Time time);
    116113
    117             /*!
    118             * \brief Position
    119             *
    120             * \param point returned position
    121             */
    122             void GetPosition(core::Vector2D &point) const;
     114  /*!
     115  * \brief Position
     116  *
     117  * \param point returned position
     118  */
     119  void GetPosition(core::Vector2D &point) const;
    123120
    124             /*!
    125             * \brief Speed
    126             *
    127             * \param point returned speed
    128             */
    129             void GetSpeed(core::Vector2D &point) const;
     121  /*!
     122  * \brief Speed
     123  *
     124  * \param point returned speed
     125  */
     126  void GetSpeed(core::Vector2D &point) const;
    130127
    131             /*!
    132             * \brief Output matrix
    133             *
    134             * \return matrix
    135             */
    136             core::cvmatrix *Matrix(void) const;
     128  /*!
     129  * \brief Output matrix
     130  *
     131  * \return matrix
     132  */
     133  core::cvmatrix *Matrix(void) const;
    137134
    138             /*!
    139             * \brief Is trajectory running?
    140             *
    141             * \return true if trajectory is running
    142             */
    143             bool IsRunning(void) const;
     135  /*!
     136  * \brief Is trajectory running?
     137  *
     138  * \return true if trajectory is running
     139  */
     140  bool IsRunning(void) const;
    144141
    145         private:
    146             /*!
    147             * \brief Update using provided datas
    148             *
    149             * Reimplemented from IODevice. Nothing to do in this IODevice.
    150             *
    151             * \param data data from the parent to process
    152             */
    153             void UpdateFrom(const core::io_data *data){};
     142private:
     143  /*!
     144  * \brief Update using provided datas
     145  *
     146  * Reimplemented from IODevice. Nothing to do in this IODevice.
     147  *
     148  * \param data data from the parent to process
     149  */
     150  void UpdateFrom(const core::io_data *data){};
    154151
    155             TrajectoryGenerator2DCircle_impl* pimpl_;
    156 
    157     };
     152  TrajectoryGenerator2DCircle_impl *pimpl_;
     153};
    158154} // end namespace filter
    159155} // end namespace flair

  • trunk/lib/FlairFilter/src/TrajectoryGenerator2DCircle_impl.cpp

    r10 r15  
    3131using namespace flair::filter;
    3232
    33 TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl(TrajectoryGenerator2DCircle* self,const LayoutPosition* position,string name) {
    34     first_update=true;
    35     is_running=false;
    36     is_finishing=false;
     33TrajectoryGenerator2DCircle_impl::TrajectoryGenerator2DCircle_impl(
     34    TrajectoryGenerator2DCircle *self, const LayoutPosition *position,
     35    string name) {
     36  first_update = true;
     37  is_running = false;
     38  is_finishing = false;
    3739
    38     //init UI
    39    GroupBox* reglages_groupbox=new GroupBox(position,name);
    40         T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,1,0.01);
    41         rayon=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"R"," m",0,1000,.1);
    42         veloctity=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"velocity"," m/s",-10,10,1);
    43         acceleration=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"acceleration (absolute)"," m/s²",0,10,.1);
     40  // init UI
     41  GroupBox *reglages_groupbox = new GroupBox(position, name);
     42  T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto", " s",
     43                        0, 1, 0.01);
     44  rayon = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "R", " m", 0,
     45                            1000, .1);
     46  veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "velocity",
     47                                " m/s", -10, 10, 1);
     48  acceleration =
     49      new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     50                        "acceleration (absolute)", " m/s²", 0, 10, .1);
    4451
    45     //init matrix
    46     cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,2);
    47     desc->SetElementName(0,0,"pos.x");
    48     desc->SetElementName(0,1,"pos.y");
    49     desc->SetElementName(1,0,"vel.x");
    50     desc->SetElementName(1,1,"vel.y");
    51     output=new cvmatrix(self,desc,floatType,name);
     52  // init matrix
     53  cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 2);
     54  desc->SetElementName(0, 0, "pos.x");
     55  desc->SetElementName(0, 1, "pos.y");
     56  desc->SetElementName(1, 0, "vel.x");
     57  desc->SetElementName(1, 1, "vel.y");
     58  output = new cvmatrix(self, desc, floatType, name);
    5259
    53     output->SetValue(0,0,0);
    54     output->SetValue(0,1,0);
    55     output->SetValue(1,0,0);
    56     output->SetValue(1,1,0);
     60  output->SetValue(0, 0, 0);
     61  output->SetValue(0, 1, 0);
     62  output->SetValue(1, 0, 0);
     63  output->SetValue(1, 1, 0);
    5764}
    5865
    59 
    6066TrajectoryGenerator2DCircle_impl::~TrajectoryGenerator2DCircle_impl() {
    61     delete output;
     67  delete output;
    6268}
    6369
    64 void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2D &start_pos,float nb_lap) {
    65     is_running=true;
    66     first_update=true;
    67     is_finishing=false;
    68     this->nb_lap=nb_lap;
     70void TrajectoryGenerator2DCircle_impl::StartTraj(const Vector2D &start_pos,
     71                                                 float nb_lap) {
     72  is_running = true;
     73  first_update = true;
     74  is_finishing = false;
     75  this->nb_lap = nb_lap;
    6976
    70     //configure trajectory
    71     angle_off=atan2(start_pos.y-pos_off.y,start_pos.x-pos_off.x);
    72     CurrentTime=0;
     77  // configure trajectory
     78  angle_off = atan2(start_pos.y - pos_off.y, start_pos.x - pos_off.x);
     79  CurrentTime = 0;
    7380}
    7481
    7582void TrajectoryGenerator2DCircle_impl::FinishTraj(void) {
    76     if(!is_finishing) {
    77         is_finishing=true;
    78         FinishTime=CurrentTime;
    79     }
     83  if (!is_finishing) {
     84    is_finishing = true;
     85    FinishTime = CurrentTime;
     86  }
    8087}
    8188
    8289void TrajectoryGenerator2DCircle_impl::Update(Time time) {
    83     float delta_t;
    84     float theta;
    85     float V=veloctity->Value();
    86     float A=acceleration->Value();
    87     float R=rayon->Value();
    88     Vector2D v;
     90  float delta_t;
     91  float theta;
     92  float V = veloctity->Value();
     93  float A = acceleration->Value();
     94  float R = rayon->Value();
     95  Vector2D v;
    8996
    90     if(V<0) A=-A;
     97  if (V < 0)
     98    A = -A;
    9199
    92     if(T->Value()==0) {
    93         if(first_update) {
    94             first_update=false;
    95             previous_time=time;
    96             return;
     100  if (T->Value() == 0) {
     101    if (first_update) {
     102      first_update = false;
     103      previous_time = time;
     104      return;
     105    } else {
     106      delta_t = (float)(time - previous_time) / 1000000000.;
     107    }
     108  } else {
     109    delta_t = T->Value();
     110  }
     111  previous_time = time;
     112  CurrentTime += delta_t;
     113
     114  if (is_finishing && CurrentTime > FinishTime + V / A)
     115    is_running = false;
     116
     117  if (is_running) {
     118    if (R == 0) {
     119      pos.x = 0;
     120      pos.y = 0;
     121      v.x = 0;
     122      v.y = 0;
     123    } else {
     124      if (CurrentTime < V / A) {
     125        theta = angle_off + A / 2 * CurrentTime * CurrentTime / R;
     126        pos.x = R * cos(theta);
     127        pos.y = R * sin(theta);
     128        v.x = -A * CurrentTime * sin(theta);
     129        v.y = A * CurrentTime * cos(theta);
     130      } else {
     131        if (!is_finishing) {
     132          theta =
     133              angle_off + V * V / (2 * A * R) + (CurrentTime - V / A) * V / R;
     134          pos.x = R * cos(theta);
     135          pos.y = R * sin(theta);
     136          v.x = -V * sin(theta);
     137          v.y = V * cos(theta);
    97138        } else {
    98             delta_t=(float)(time-previous_time)/1000000000.;
     139          theta = angle_off + V * V / (2 * A * R) +
     140                  (FinishTime - V / A) * V / R -
     141                  A / 2 * (FinishTime - CurrentTime) *
     142                      (FinishTime - CurrentTime) / R +
     143                  V * (CurrentTime - FinishTime) / R;
     144          pos.x = R * cos(theta);
     145          pos.y = R * sin(theta);
     146          v.x = -(V + A * (FinishTime - CurrentTime)) * sin(theta);
     147          v.y = (V + A * (FinishTime - CurrentTime)) * cos(theta);
    99148        }
    100     } else {
    101         delta_t=T->Value();
    102     }
    103     previous_time=time;
    104     CurrentTime+=delta_t;
    105 
    106     if(is_finishing && CurrentTime>FinishTime+V/A) is_running=false;
    107 
    108     if(is_running) {
    109         if(R==0) {
    110             pos.x=0;
    111             pos.y=0;
    112             v.x=0;
    113             v.y=0;
    114         } else {
    115             if(CurrentTime<V/A) {
    116                 theta=angle_off+A/2*CurrentTime*CurrentTime/R;
    117                 pos.x=R*cos(theta);
    118                 pos.y=R*sin(theta);
    119                 v.x=-A*CurrentTime*sin(theta);
    120                 v.y=A*CurrentTime*cos(theta);
    121             } else {
    122                 if(!is_finishing) {
    123                     theta=angle_off+V*V/(2*A*R)+(CurrentTime-V/A)*V/R;
    124                     pos.x=R*cos(theta);
    125                     pos.y=R*sin(theta);
    126                     v.x=-V*sin(theta);
    127                     v.y=V*cos(theta);
    128                 } else {
    129                     theta=angle_off+V*V/(2*A*R)+(FinishTime-V/A)*V/R-A/2*(FinishTime-CurrentTime)*(FinishTime-CurrentTime)/R+V*(CurrentTime-FinishTime)/R;
    130                     pos.x=R*cos(theta);
    131                     pos.y=R*sin(theta);
    132                     v.x=-(V+A*(FinishTime-CurrentTime))*sin(theta);
    133                     v.y=(V+A*(FinishTime-CurrentTime))*cos(theta);
    134                 }
    135             }
    136         }
    137 
    138         if(theta-angle_off>=nb_lap*2*PI-(-A/2*(V/A)*(V/A)/R+V*(V/A)/R) && nb_lap>0) {
    139             FinishTraj();
    140         }
    141     } else {
    142         v.x=0;
    143         v.y=0;
     149      }
    144150    }
    145151
    146     //on prend une fois pour toute les mutex et on fait des accès directs
    147     output->GetMutex();
    148     output->SetValueNoMutex(0,0,pos.x+pos_off.x);
    149     output->SetValueNoMutex(0,1,pos.y+pos_off.y);
    150     output->SetValueNoMutex(1,0,v.x+vel_off.x);
    151     output->SetValueNoMutex(1,1,v.y+vel_off.y);
    152     output->ReleaseMutex();
     152    if (theta - angle_off >= nb_lap * 2 * PI - (-A / 2 * (V / A) * (V / A) / R +
     153                                                V * (V / A) / R) &&
     154        nb_lap > 0) {
     155      FinishTraj();
     156    }
     157  } else {
     158    v.x = 0;
     159    v.y = 0;
     160  }
    153161
    154     output->SetDataTime(time);
     162  // on prend une fois pour toute les mutex et on fait des accès directs
     163  output->GetMutex();
     164  output->SetValueNoMutex(0, 0, pos.x + pos_off.x);
     165  output->SetValueNoMutex(0, 1, pos.y + pos_off.y);
     166  output->SetValueNoMutex(1, 0, v.x + vel_off.x);
     167  output->SetValueNoMutex(1, 1, v.y + vel_off.y);
     168  output->ReleaseMutex();
     169
     170  output->SetDataTime(time);
    155171}

  • trunk/lib/FlairFilter/src/UavMultiplex.cpp

    r10 r15  
    2727using namespace flair::gui;
    2828
    29 namespace flair
    30 {
    31 namespace filter
    32 {
     29namespace flair {
     30namespace filter {
    3331
    34 UavMultiplex::UavMultiplex(const core::FrameworkManager* parent,std::string name) : IODevice(parent,name)
    35 {
    36     pimpl_=new UavMultiplex_impl(parent,this,name);
     32UavMultiplex::UavMultiplex(const core::FrameworkManager *parent,
     33                           std::string name)
     34    : IODevice(parent, name) {
     35  pimpl_ = new UavMultiplex_impl(parent, this, name);
    3736}
    3837
    39 UavMultiplex::~UavMultiplex(void)
    40 {
    41     delete pimpl_;
     38UavMultiplex::~UavMultiplex(void) { delete pimpl_; }
     39
     40void UavMultiplex::Update(core::Time time) {
     41  pimpl_->input->SetDataTime(time);
     42  UpdateFrom(pimpl_->input);
    4243}
    4344
    44 void UavMultiplex::Update(core::Time time)
    45 {
    46     pimpl_->input->SetDataTime(time);
    47     UpdateFrom(pimpl_->input);
     45void UavMultiplex::SetMultiplexComboBox(string name, int index) {
     46  pimpl_->SetMultiplexComboBox(name, index);
    4847}
    4948
    50 void UavMultiplex::SetMultiplexComboBox(string name,int index)
    51 {
    52     pimpl_->SetMultiplexComboBox(name,index);
     49int UavMultiplex::MultiplexValue(int index) const {
     50  return pimpl_->MultiplexValue(index);
    5351}
    5452
    55 int UavMultiplex::MultiplexValue(int index) const
    56 {
    57     return pimpl_->MultiplexValue(index);
     53TabWidget *UavMultiplex::GetTabWidget(void) const { return pimpl_->tabwidget; }
     54
     55Layout *UavMultiplex::GetLayout(void) const { return pimpl_->setup_tab; }
     56
     57void UavMultiplex::LockUserInterface(void) const {
     58  pimpl_->setup_tab->setEnabled(false);
    5859}
    5960
    60 TabWidget* UavMultiplex::GetTabWidget(void) const
    61 {
    62     return pimpl_->tabwidget;
     61void UavMultiplex::UnlockUserInterface(void) const {
     62  pimpl_->setup_tab->setEnabled(true);
    6363}
    6464
    65 Layout* UavMultiplex::GetLayout(void) const
    66 {
    67     return pimpl_->setup_tab;
     65void UavMultiplex::SetRoll(float value) {
     66  pimpl_->input->SetValue(0, 0, value);
    6867}
    6968
    70 void UavMultiplex::LockUserInterface(void) const
    71 {
    72     pimpl_->setup_tab->setEnabled(false);
     69void UavMultiplex::SetPitch(float value) {
     70  pimpl_->input->SetValue(1, 0, value);
    7371}
    7472
    75 void UavMultiplex::UnlockUserInterface(void) const
    76 {
    77     pimpl_->setup_tab->setEnabled(true);
     73void UavMultiplex::SetYaw(float value) { pimpl_->input->SetValue(2, 0, value); }
     74
     75void UavMultiplex::SetThrust(float value) {
     76  pimpl_->input->SetValue(3, 0, value);
    7877}
    7978
    80 void UavMultiplex::SetRoll(float value)
    81 {
    82     pimpl_->input->SetValue(0,0,value);
     79void UavMultiplex::SetRollTrim(float value) {
     80  pimpl_->input->SetValue(4, 0, value);
    8381}
    8482
    85 void UavMultiplex::SetPitch(float value)
    86 {
    87     pimpl_->input->SetValue(1,0,value);
     83void UavMultiplex::SetPitchTrim(float value) {
     84  pimpl_->input->SetValue(5, 0, value);
    8885}
    8986
    90 void UavMultiplex::SetYaw(float value)
    91 {
    92     pimpl_->input->SetValue(2,0,value);
    93 }
    94 
    95 void UavMultiplex::SetThrust(float value)
    96 {
    97     pimpl_->input->SetValue(3,0,value);
    98 }
    99 
    100 void UavMultiplex::SetRollTrim(float value)
    101 {
    102     pimpl_->input->SetValue(4,0,value);
    103 }
    104 
    105 void UavMultiplex::SetPitchTrim(float value)
    106 {
    107     pimpl_->input->SetValue(5,0,value);
    108 }
    109 
    110 void UavMultiplex::SetYawTrim(float value)
    111 {
    112     pimpl_->input->SetValue(6,0,value);
     87void UavMultiplex::SetYawTrim(float value) {
     88  pimpl_->input->SetValue(6, 0, value);
    11389}
    11490

  • trunk/lib/FlairFilter/src/UavMultiplex.h

    r10 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class io_data;
    25     }
    26     namespace gui
    27     {
    28         class TabWidget;
    29         class Layout;
    30     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class io_data;
    3123}
     24namespace gui {
     25class TabWidget;
     26class Layout;
     27}
     28}
    3229
    3330class UavMultiplex_impl;
    3431
    35 namespace flair
    36 {
    37 namespace filter
    38 {
    39     /*! \class UavMultiplex
    40     *
    41     * \brief Class defining uav multiplexing
    42     */
    43     class UavMultiplex : public core::IODevice
    44     {
    45         public:
    46             /*!
    47             * \brief Constructor
    48             *
    49             * Construct a uav multiplexing
    50             *
    51             * \param parent parent
    52             * \param name name
    53             */
    54             UavMultiplex(const core::FrameworkManager* parent,std::string name);
    55 
    56             /*!
    57             * \brief Destructor
    58             *
    59             */
    60             ~UavMultiplex();
    61 
    62             /*!
    63             * \brief Set roll torque
    64             *
    65             * roll torque is placed in input(0,0)
    66             *
    67             * \param value value between -1 and 1
    68             */
    69             void SetRoll(float value);
    70 
    71             /*!
    72             * \brief Set pitch torque
    73             *
    74             * pitch torque is placed in input(1,0)
    75             *
    76             * \param value value between -1 and 1
    77             */
    78             void SetPitch(float value);
    79 
    80             /*!
    81             * \brief Set yaw torque
    82             *
    83             * yaw torque is placed in input(2,0)
    84             *
    85             * \param value value between -1 and 1
    86             */
    87             void SetYaw(float value);
    88 
    89             /*!
    90             * \brief Set thrust
    91             *
    92             * thrust is placed in input(3,0)
    93             *
    94             * \param value value between 0 and 1
    95             */
    96             void SetThrust(float value);
    97 
    98             /*!
    99             * \brief Set roll trim
    100             *
    101             * trim is placed in input(4,0)
    102             *
    103             * \param value value
    104             */
    105             void SetRollTrim(float value);
    106 
    107             /*!
    108             * \brief Set pitch trim
    109             *
    110             * trim is placed in input(5,0)
    111             *
    112             * \param value value
    113             */
    114             void SetPitchTrim(float value);
    115 
    116             /*!
    117             * \brief Set yaw trim
    118             *
    119             * trim is placed in input(6,0)
    120             *
    121             * \param value value
    122             */
    123             void SetYawTrim(float value);
    124 
    125             /*!
    126             * \brief Update using provided datas
    127             *
    128             * Uses values specified by Set*().
    129             *
    130             * \param time time of the update
    131             */
    132             void Update(core::Time time);
    133 
    134             /*!
    135             * \brief Lock user interface
    136             *
    137             * User interface will be grayed out.\n
    138             * Use it do disallow changes when flying.
    139             *
    140             */
    141             void LockUserInterface(void) const;
    142 
    143             /*!
    144             * \brief Unlock user interface
    145             *
    146             * User interface will be enabled.\n
    147             *
    148             */
    149             void UnlockUserInterface(void) const;
    150 
    151             /*!
    152             * \brief Layout
    153             *
    154             * Layout to place custom widgets.\n
    155             *
    156             * \return the layout
    157             */
    158             gui::Layout* GetLayout(void) const;
    159 
    160             /*!
    161             * \brief Use default plot
    162             *
    163             * Derived class can implement this function do draw default plot.
    164             *
    165             */
    166             virtual void UseDefaultPlot(void){};
    167 
    168             /*!
    169             * \brief Motors count
    170             *
    171             * This function must be reimplemented, in order to get the number of motors.
    172             *
    173             * \return motors count
    174             */
    175             virtual uint8_t MotorsCount(void) const=0;
    176 
    177             /*!
    178             * \brief Multiplex value
    179             *
    180             * Get the mutliplexed value of a motor, if SetMultiplexComboBox() was used.\n
    181             *
    182             * \param index index of the motor, from 0 to MotorsCount()
    183             * \return multiplexed index of the motor
    184             */
    185             int MultiplexValue(int index) const;
    186 
    187             /*!
    188             * \brief Get TabWidget
    189             *
    190             * Usefull to add tabs.\n
    191             *
    192             * \return the TabWidget
    193             */
    194             gui::TabWidget* GetTabWidget(void) const;
    195 
    196         protected:
    197             /*!
    198             * \brief Set multiplex ComboBox
    199             *
    200             * Draws a ComboBox to define motor multiplexing. \n
    201             * This is used to change the order of the output motors.
    202             *
    203             * \param name description of the motor (ex front left)
    204             * \param index index of the motor, from 0 to MotorsCount()
    205             */
    206             void SetMultiplexComboBox(std::string name,int index);
    207 
    208         private:
    209             /*!
    210             * \brief Update using provided datas
    211             *
    212             * This function is automatically called by ProcessUpdate()
    213             * of the Object::Parent's if its Object::ObjectType is "IODevice". \n
    214             * This function must be reimplemented, in order to process the data from the parent.
    215             *
    216             * \param data data from the parent to process
    217             */
    218             virtual void UpdateFrom(const core::io_data *data)=0;
    219 
    220             UavMultiplex_impl *pimpl_;
    221     };
     32namespace flair {
     33namespace filter {
     34/*! \class UavMultiplex
     35*
     36* \brief Class defining uav multiplexing
     37*/
     38class UavMultiplex : public core::IODevice {
     39public:
     40  /*!
     41  * \brief Constructor
     42  *
     43  * Construct a uav multiplexing
     44  *
     45  * \param parent parent
     46  * \param name name
     47  */
     48  UavMultiplex(const core::FrameworkManager *parent, std::string name);
     49
     50  /*!
     51  * \brief Destructor
     52  *
     53  */
     54  ~UavMultiplex();
     55
     56  /*!
     57  * \brief Set roll torque
     58  *
     59  * roll torque is placed in input(0,0)
     60  *
     61  * \param value value between -1 and 1
     62  */
     63  void SetRoll(float value);
     64
     65  /*!
     66  * \brief Set pitch torque
     67  *
     68  * pitch torque is placed in input(1,0)
     69  *
     70  * \param value value between -1 and 1
     71  */
     72  void SetPitch(float value);
     73
     74  /*!
     75  * \brief Set yaw torque
     76  *
     77  * yaw torque is placed in input(2,0)
     78  *
     79  * \param value value between -1 and 1
     80  */
     81  void SetYaw(float value);
     82
     83  /*!
     84  * \brief Set thrust
     85  *
     86  * thrust is placed in input(3,0)
     87  *
     88  * \param value value between 0 and 1
     89  */
     90  void SetThrust(float value);
     91
     92  /*!
     93  * \brief Set roll trim
     94  *
     95  * trim is placed in input(4,0)
     96  *
     97  * \param value value
     98  */
     99  void SetRollTrim(float value);
     100
     101  /*!
     102  * \brief Set pitch trim
     103  *
     104  * trim is placed in input(5,0)
     105  *
     106  * \param value value
     107  */
     108  void SetPitchTrim(float value);
     109
     110  /*!
     111  * \brief Set yaw trim
     112  *
     113  * trim is placed in input(6,0)
     114  *
     115  * \param value value
     116  */
     117  void SetYawTrim(float value);
     118
     119  /*!
     120  * \brief Update using provided datas
     121  *
     122  * Uses values specified by Set*().
     123  *
     124  * \param time time of the update
     125  */
     126  void Update(core::Time time);
     127
     128  /*!
     129  * \brief Lock user interface
     130  *
     131  * User interface will be grayed out.\n
     132  * Use it do disallow changes when flying.
     133  *
     134  */
     135  void LockUserInterface(void) const;
     136
     137  /*!
     138  * \brief Unlock user interface
     139  *
     140  * User interface will be enabled.\n
     141  *
     142  */
     143  void UnlockUserInterface(void) const;
     144
     145  /*!
     146  * \brief Layout
     147  *
     148  * Layout to place custom widgets.\n
     149  *
     150  * \return the layout
     151  */
     152  gui::Layout *GetLayout(void) const;
     153
     154  /*!
     155  * \brief Use default plot
     156  *
     157  * Derived class can implement this function do draw default plot.
     158  *
     159  */
     160  virtual void UseDefaultPlot(void){};
     161
     162  /*!
     163  * \brief Motors count
     164  *
     165  * This function must be reimplemented, in order to get the number of motors.
     166  *
     167  * \return motors count
     168  */
     169  virtual uint8_t MotorsCount(void) const = 0;
     170
     171  /*!
     172  * \brief Multiplex value
     173  *
     174  * Get the mutliplexed value of a motor, if SetMultiplexComboBox() was used.\n
     175  *
     176  * \param index index of the motor, from 0 to MotorsCount()
     177  * \return multiplexed index of the motor
     178  */
     179  int MultiplexValue(int index) const;
     180
     181  /*!
     182  * \brief Get TabWidget
     183  *
     184  * Usefull to add tabs.\n
     185  *
     186  * \return the TabWidget
     187  */
     188  gui::TabWidget *GetTabWidget(void) const;
     189
     190protected:
     191  /*!
     192  * \brief Set multiplex ComboBox
     193  *
     194  * Draws a ComboBox to define motor multiplexing. \n
     195  * This is used to change the order of the output motors.
     196  *
     197  * \param name description of the motor (ex front left)
     198  * \param index index of the motor, from 0 to MotorsCount()
     199  */
     200  void SetMultiplexComboBox(std::string name, int index);
     201
     202private:
     203  /*!
     204  * \brief Update using provided datas
     205  *
     206  * This function is automatically called by ProcessUpdate()
     207  * of the Object::Parent's if its Object::ObjectType is "IODevice". \n
     208  * This function must be reimplemented, in order to process the data from the
     209  *parent.
     210  *
     211  * \param data data from the parent to process
     212  */
     213  virtual void UpdateFrom(const core::io_data *data) = 0;
     214
     215  UavMultiplex_impl *pimpl_;
     216};
    222217} // end namespace filter
    223218} // end namespace flair

  • trunk/lib/FlairFilter/src/UavMultiplex_impl.cpp

    r10 r15  
    3333using namespace flair::filter;
    3434
    35 UavMultiplex_impl::UavMultiplex_impl(const FrameworkManager* parent,UavMultiplex* self,std::string name) {
    36     input=new cvmatrix(self,7,1,floatType);
    37     multiplexcombobox=NULL;
    38     this->self=self;
     35UavMultiplex_impl::UavMultiplex_impl(const FrameworkManager *parent,
     36                                     UavMultiplex *self, std::string name) {
     37  input = new cvmatrix(self, 7, 1, floatType);
     38  multiplexcombobox = NULL;
     39  this->self = self;
    3940
    40     //station sol
    41     main_tab=new Tab(parent->GetTabWidget(),name);
    42     tabwidget=new TabWidget(main_tab->NewRow(),"UavMultiplex");
    43         setup_tab=new Tab(tabwidget,"Setup");
     41  // station sol
     42  main_tab = new Tab(parent->GetTabWidget(), name);
     43  tabwidget = new TabWidget(main_tab->NewRow(), "UavMultiplex");
     44  setup_tab = new Tab(tabwidget, "Setup");
    4445}
    4546
    46 UavMultiplex_impl::~UavMultiplex_impl(void)
    47 {
    48     delete main_tab;
    49     if(multiplexcombobox!=NULL) free(multiplexcombobox);
     47UavMultiplex_impl::~UavMultiplex_impl(void) {
     48  delete main_tab;
     49  if (multiplexcombobox != NULL)
     50    free(multiplexcombobox);
    5051}
    5152
    52 void UavMultiplex_impl::SetMultiplexComboBox(string name,int index)
    53 {
    54     //we do not know motorcount at constructor time, so allocation is done here
    55     if(multiplexcombobox==NULL)
    56     {
    57         multiplexcombobox=(ComboBox**)malloc(self->MotorsCount()*sizeof(ComboBox*));
    58         for(int i=0;i<self->MotorsCount();i++) multiplexcombobox[i]=NULL;
    59         groupbox=new GroupBox(setup_tab->NewRow(),"motor attribution");
    60     }
    61     if(index>self->MotorsCount())
    62     {
    63         self->Err("index out of bound %i/%i\n",index,self->MotorsCount());
    64         return;
    65     }
    66     if(multiplexcombobox[index]!=NULL)
    67     {
    68         self->Err("index already setup\n");
    69         return;
    70     }
     53void UavMultiplex_impl::SetMultiplexComboBox(string name, int index) {
     54  // we do not know motorcount at constructor time, so allocation is done here
     55  if (multiplexcombobox == NULL) {
     56    multiplexcombobox =
     57        (ComboBox **)malloc(self->MotorsCount() * sizeof(ComboBox *));
     58    for (int i = 0; i < self->MotorsCount(); i++)
     59      multiplexcombobox[i] = NULL;
     60    groupbox = new GroupBox(setup_tab->NewRow(), "motor attribution");
     61  }
     62  if (index > self->MotorsCount()) {
     63    self->Err("index out of bound %i/%i\n", index, self->MotorsCount());
     64    return;
     65  }
     66  if (multiplexcombobox[index] != NULL) {
     67    self->Err("index already setup\n");
     68    return;
     69  }
    7170
    72     multiplexcombobox[index]=new ComboBox(groupbox->At(index/4,index%4),name);
     71  multiplexcombobox[index] =
     72      new ComboBox(groupbox->At(index / 4, index % 4), name);
    7373
    74     for(int i=0;i<self->MotorsCount();i++)
    75     {
    76         ostringstream oss;
    77         oss << i;
    78         multiplexcombobox[index]->AddItem(oss.str());
    79     }
     74  for (int i = 0; i < self->MotorsCount(); i++) {
     75    ostringstream oss;
     76    oss << i;
     77    multiplexcombobox[index]->AddItem(oss.str());
     78  }
    8079}
    8180
    82 int UavMultiplex_impl::MultiplexValue(int index) const
    83 {
    84     if(multiplexcombobox[index]!=NULL)
    85     {
    86         return multiplexcombobox[index]->CurrentIndex();
    87     }
    88     else
    89     {
    90         self->Err("multiplex not setup for motor %i\n",index);
    91         return 0;
    92     }
    93 
     81int UavMultiplex_impl::MultiplexValue(int index) const {
     82  if (multiplexcombobox[index] != NULL) {
     83    return multiplexcombobox[index]->CurrentIndex();
     84  } else {
     85    self->Err("multiplex not setup for motor %i\n", index);
     86    return 0;
     87  }
    9488}

  • trunk/lib/FlairFilter/src/X4X8Multiplex.cpp

    r10 r15  
    2323using namespace flair::gui;
    2424
    25 namespace flair
    26 {
    27 namespace filter
    28 {
     25namespace flair {
     26namespace filter {
    2927
    30 X4X8Multiplex::X4X8Multiplex(const FrameworkManager* parent,std::string name,UavType_t type) : UavMultiplex(parent,name)
    31 {
    32     int nb_mot;
     28X4X8Multiplex::X4X8Multiplex(const FrameworkManager *parent, std::string name,
     29                             UavType_t type)
     30    : UavMultiplex(parent, name) {
     31  int nb_mot;
    3332
    34     switch(type)
    35     {
    36         case X4:
    37             nb_mot=4;
    38             break;
    39         case X8:
    40             nb_mot=8;
    41             break;
    42         default:
    43             Err("uav type not supported\n");
    44             break;
    45     }
     33  switch (type) {
     34  case X4:
     35    nb_mot = 4;
     36    break;
     37  case X8:
     38    nb_mot = 8;
     39    break;
     40  default:
     41    Err("uav type not supported\n");
     42    break;
     43  }
    4644
    47     pimpl_=new X4X8Multiplex_impl(this,nb_mot);
     45  pimpl_ = new X4X8Multiplex_impl(this, nb_mot);
    4846
    49     for(int i=0;i<nb_mot;i++)
    50     {
    51         SetMultiplexComboBox(pimpl_->MotorName(i),i);
    52     }
     47  for (int i = 0; i < nb_mot; i++) {
     48    SetMultiplexComboBox(pimpl_->MotorName(i), i);
     49  }
    5350}
    5451
    55 X4X8Multiplex::~X4X8Multiplex(void)
    56 {
    57     delete pimpl_;
    58 }
     52X4X8Multiplex::~X4X8Multiplex(void) { delete pimpl_; }
    5953
    60 uint8_t X4X8Multiplex::MotorsCount(void) const
    61 {
    62     return pimpl_->nb_mot;
    63 }
     54uint8_t X4X8Multiplex::MotorsCount(void) const { return pimpl_->nb_mot; }
    6455
    65 void X4X8Multiplex::UseDefaultPlot(void)
    66 {
    67     pimpl_->UseDefaultPlot();
    68 }
     56void X4X8Multiplex::UseDefaultPlot(void) { pimpl_->UseDefaultPlot(); }
    6957
    70 void X4X8Multiplex::UpdateFrom(const io_data *data)
    71 {
    72     pimpl_->UpdateFrom(data);
     58void X4X8Multiplex::UpdateFrom(const io_data *data) {
     59  pimpl_->UpdateFrom(data);
    7360}
    7461

  • trunk/lib/FlairFilter/src/X4X8Multiplex.h

    r10 r15  
    1818class X4X8Multiplex_impl;
    1919
    20 namespace flair
    21 {
    22 namespace filter
    23 {
    24     /*! \class X4X8Multiplex
    25     *
    26     * \brief Class defining X4 and X8 multiplexing
    27     *
    28     * The output order of the motors can be changed through ground station.
    29     */
    30     class X4X8Multiplex : public UavMultiplex
    31     {
    32         friend class ::X4X8Multiplex_impl;
     20namespace flair {
     21namespace filter {
     22/*! \class X4X8Multiplex
     23*
     24* \brief Class defining X4 and X8 multiplexing
     25*
     26* The output order of the motors can be changed through ground station.
     27*/
     28class X4X8Multiplex : public UavMultiplex {
     29  friend class ::X4X8Multiplex_impl;
    3330
    34         public:
    35              /*!
    36             \enum UavType_t
    37             \brief type of UAV
    38             */
    39             typedef enum {
    40                 X4,/*!< 4 motors: front left, front right, rear left, rear right */
    41                 X8/*!< 8 motors: top front left, top front right, top rear left, top rear right, bottom front left, bottom front right, bottom rear left, bottom rear right */
    42                 } UavType_t;
     31public:
     32  /*!
     33 \enum UavType_t
     34 \brief type of UAV
     35 */
     36  typedef enum {
     37    X4, /*!< 4 motors: front left, front right, rear left, rear right */
     38    X8  /*!< 8 motors: top front left, top front right, top rear left, top rear
     39           right, bottom front left, bottom front right, bottom rear left, bottom
     40           rear right */
     41  } UavType_t;
    4342
    44             /*!
    45             \enum MotorNames_t
    46             \brief Motor names
    47             */
    48             typedef enum {
    49                 TopFrontLeft=0,/*!< Top front left, X4 or X8 */
    50                 TopFrontRight=1,/*!< Top front right, X4 or X8 */
    51                 TopRearLeft=2,/*!< Top rear left, X4 or X8 */
    52                 TopRearRight=3,/*!< Top rear right, X4 or X8 */
    53                 BottomFrontLeft=4,/*!< Bottom front left, X8 */
    54                 BottomFrontRight=5,/*!< Bottom front right, X8 */
    55                 BottomRearLeft=6,/*!< Bottom rear left, X8 */
    56                 BottomRearRight=7,/*!< Bottom rear right, X8 */
    57                 } MotorNames_t;
     43  /*!
     44  \enum MotorNames_t
     45  \brief Motor names
     46  */
     47  typedef enum {
     48    TopFrontLeft = 0,     /*!< Top front left, X4 or X8 */
     49    TopFrontRight = 1,    /*!< Top front right, X4 or X8 */
     50    TopRearLeft = 2,      /*!< Top rear left, X4 or X8 */
     51    TopRearRight = 3,     /*!< Top rear right, X4 or X8 */
     52    BottomFrontLeft = 4,  /*!< Bottom front left, X8 */
     53    BottomFrontRight = 5, /*!< Bottom front right, X8 */
     54    BottomRearLeft = 6,   /*!< Bottom rear left, X8 */
     55    BottomRearRight = 7,  /*!< Bottom rear right, X8 */
     56  } MotorNames_t;
    5857
    59             /*!
    60             * \brief Constructor
    61             *
    62             * Construct a X4 and X8 multiplexing
    63             *
    64             * \param parent parent
    65             * \param name name
    66             * \param type type
    67             */
    68             X4X8Multiplex(const core::FrameworkManager* parent,std::string name,UavType_t type);
     58  /*!
     59  * \brief Constructor
     60  *
     61  * Construct a X4 and X8 multiplexing
     62  *
     63  * \param parent parent
     64  * \param name name
     65  * \param type type
     66  */
     67  X4X8Multiplex(const core::FrameworkManager *parent, std::string name,
     68                UavType_t type);
    6969
    70             /*!
    71             * \brief Destructor
    72             *
    73             */
    74             ~X4X8Multiplex();
     70  /*!
     71  * \brief Destructor
     72  *
     73  */
     74  ~X4X8Multiplex();
    7575
    76             /*!
    77             * \brief Use default plot
    78             *
    79             * Plot the output values.
    80             *
    81             */
    82             void UseDefaultPlot(void);
     76  /*!
     77  * \brief Use default plot
     78  *
     79  * Plot the output values.
     80  *
     81  */
     82  void UseDefaultPlot(void);
    8383
    84             /*!
    85             * \brief Motors count
    86             *
    87             * Reimplemented from UavMultiplex.
    88             *
    89             * \return motors count
    90             */
    91             uint8_t MotorsCount(void) const;
     84  /*!
     85  * \brief Motors count
     86  *
     87  * Reimplemented from UavMultiplex.
     88  *
     89  * \return motors count
     90  */
     91  uint8_t MotorsCount(void) const;
    9292
    93         private:
    94             /*!
    95             * \brief Update using provided datas
    96             *
    97             * Reimplemented from IODevice.
    98             *
    99             * \param data data from the parent to process
    100             */
    101             void UpdateFrom(const core::io_data *data);
     93private:
     94  /*!
     95  * \brief Update using provided datas
     96  *
     97  * Reimplemented from IODevice.
     98  *
     99  * \param data data from the parent to process
     100  */
     101  void UpdateFrom(const core::io_data *data);
    102102
    103             X4X8Multiplex_impl *pimpl_;
    104 
    105     };
     103  X4X8Multiplex_impl *pimpl_;
     104};
    106105} // end namespace filter
    107106} // end namespace flair

  • trunk/lib/FlairFilter/src/X4X8Multiplex_impl.cpp

    r10 r15  
    3333using namespace flair::filter;
    3434
    35 X4X8Multiplex_impl::X4X8Multiplex_impl(flair::filter::X4X8Multiplex* self,int nb_mot)
    36 {
    37     this->nb_mot=nb_mot;
    38     this->self=self;
    39 
    40     if(nb_mot==4)
    41     {
    42         GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x4 multiplex");
    43         pas=new ComboBox(groupbox->NewRow(),"front left blade pitch:");
     35X4X8Multiplex_impl::X4X8Multiplex_impl(flair::filter::X4X8Multiplex *self,
     36                                       int nb_mot) {
     37  this->nb_mot = nb_mot;
     38  this->self = self;
     39
     40  if (nb_mot == 4) {
     41    GroupBox *groupbox =
     42        new GroupBox(self->GetLayout()->NewRow(), "x4 multiplex");
     43    pas = new ComboBox(groupbox->NewRow(), "front left blade pitch:");
     44  } else {
     45    GroupBox *groupbox =
     46        new GroupBox(self->GetLayout()->NewRow(), "x8 multiplex");
     47    pas = new ComboBox(groupbox->NewRow(), "top front left blade pitch:");
     48  }
     49  pas->AddItem("normal");
     50  pas->AddItem("inverted");
     51
     52  cvmatrix_descriptor *desc = new cvmatrix_descriptor(nb_mot, 1);
     53  for (int i = 0; i < nb_mot; i++) {
     54    desc->SetElementName(i, 0, MotorName(i));
     55  }
     56
     57  output = new cvmatrix(self, desc, floatType);
     58
     59  self->AddDataToLog(output);
     60}
     61
     62X4X8Multiplex_impl::~X4X8Multiplex_impl(void) {}
     63
     64void X4X8Multiplex_impl::UseDefaultPlot(void) {
     65  Tab *plot_tab = new Tab(self->GetTabWidget(), "Values");
     66  plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 1);
     67  plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 1);
     68  plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 1);
     69  plots[3] = new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 1);
     70
     71  for (int i = 0; i < 4; i++)
     72    plots[i]->AddCurve(output->Element(i));
     73
     74  if (nb_mot == 8) {
     75    for (int i = 0; i < 4; i++)
     76      plots[i]->AddCurve(output->Element(i + 4), DataPlot::Blue);
     77  }
     78}
     79
     80void X4X8Multiplex_impl::UpdateFrom(const io_data *data) {
     81  float u_roll, u_pitch, u_yaw, u_thrust;
     82  float trim_roll, trim_pitch, trim_yaw;
     83  float value[MAX_MOTORS];
     84
     85  cvmatrix *input = (cvmatrix *)data;
     86
     87  // on prend une fois pour toute le mutex et on fait des accès directs
     88  input->GetMutex();
     89
     90  u_roll = input->ValueNoMutex(0, 0);
     91  u_pitch = input->ValueNoMutex(1, 0);
     92  u_yaw = input->ValueNoMutex(2, 0);
     93  u_thrust = input->ValueNoMutex(3, 0);
     94  trim_roll = input->ValueNoMutex(4, 0);
     95  trim_pitch = input->ValueNoMutex(5, 0);
     96  trim_yaw = input->ValueNoMutex(6, 0);
     97
     98  input->ReleaseMutex();
     99
     100  if (pas->CurrentIndex() == 1) {
     101    trim_yaw = -trim_yaw;
     102    u_yaw = -u_yaw;
     103  }
     104
     105  if (nb_mot == 2) {
     106    //(top) front left
     107    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
     108        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
     109
     110    //(top) front right
     111    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
     112        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
     113  }
     114  if (nb_mot == 4 || nb_mot == 8) {
     115    //(top) front left
     116    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
     117        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
     118
     119    //(top) front right
     120    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
     121        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
     122
     123    //(top) rear left
     124    value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)] =
     125        Set(-trim_pitch + trim_roll - trim_yaw,
     126            u_thrust - u_pitch + u_roll - u_yaw);
     127
     128    //(top) rear right
     129    value[self->MultiplexValue(X4X8Multiplex::TopRearRight)] =
     130        Set(-trim_pitch - trim_roll + trim_yaw,
     131            u_thrust - u_pitch - u_roll + u_yaw);
     132  }
     133
     134  if (nb_mot == 8) {
     135    // bottom front left
     136    value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)] = Set(
     137        trim_pitch + trim_roll - trim_yaw, u_thrust + u_pitch + u_roll - u_yaw);
     138
     139    // bottom front right
     140    value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)] = Set(
     141        trim_pitch - trim_roll + trim_yaw, u_thrust + u_pitch - u_roll + u_yaw);
     142
     143    // bottom rear left
     144    value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)] =
     145        Set(-trim_pitch + trim_roll + trim_yaw,
     146            u_thrust - u_pitch + u_roll + u_yaw);
     147
     148    // bottom rear right
     149    value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)] =
     150        Set(-trim_pitch - trim_roll - trim_yaw,
     151            u_thrust - u_pitch - u_roll - u_yaw);
     152  }
     153
     154  // on prend une fois pour toute le mutex et on fait des accès directs
     155  output->GetMutex();
     156  for (int i = 0; i < nb_mot; i++)
     157    output->SetValueNoMutex(i, 0, value[i]);
     158  output->ReleaseMutex();
     159
     160  output->SetDataTime(data->DataTime());
     161
     162  self->ProcessUpdate(output);
     163}
     164
     165float X4X8Multiplex_impl::Set(float trim, float u) {
     166  float value = trim;
     167
     168  if (u > 0) {
     169    value += sqrtf(u);
     170  }
     171
     172  return value;
     173}
     174
     175string X4X8Multiplex_impl::MotorName(int index) {
     176  switch (nb_mot) {
     177  case 4: {
     178    switch (index) {
     179    case 0:
     180      return "front left";
     181    case 1:
     182      return "front rigth";
     183    case 2:
     184      return "rear left";
     185    case 3:
     186      return "rear rigth";
     187    default:
     188      return "unammed motor";
    44189    }
    45     else
    46     {
    47         GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x8 multiplex");
    48         pas=new ComboBox(groupbox->NewRow(),"top front left blade pitch:");
     190  }
     191  case 8: {
     192    switch (index) {
     193    case 0:
     194      return "top front left";
     195    case 1:
     196      return "top front rigth";
     197    case 2:
     198      return "top rear left";
     199    case 3:
     200      return "top rear rigth";
     201    case 4:
     202      return "bottom front left";
     203    case 5:
     204      return "bottom front rigth";
     205    case 6:
     206      return "bottom rear left";
     207    case 7:
     208      return "bottom rear rigth";
     209    default:
     210      return "unammed motor";
    49211    }
    50     pas->AddItem("normal");
    51     pas->AddItem("inverted");
    52 
    53     cvmatrix_descriptor* desc=new cvmatrix_descriptor(nb_mot,1);
    54     for(int i=0;i<nb_mot;i++)
    55     {
    56         desc->SetElementName(i,0,MotorName(i));
    57     }
    58 
    59     output=new cvmatrix(self,desc,floatType);
    60 
    61     self->AddDataToLog(output);
    62 }
    63 
    64 X4X8Multiplex_impl::~X4X8Multiplex_impl(void)
    65 {
    66 }
    67 
    68 void X4X8Multiplex_impl::UseDefaultPlot(void)
    69 {
    70     Tab *plot_tab=new Tab(self->GetTabWidget(),"Values");
    71     plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,1);
    72     plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,1);
    73     plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,1);
    74     plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,1);
    75 
    76     for(int i=0;i<4;i++) plots[i]->AddCurve(output->Element(i));
    77 
    78     if(nb_mot==8)
    79     {
    80         for(int i=0;i<4;i++) plots[i]->AddCurve(output->Element(i+4),DataPlot::Blue);
    81     }
    82 }
    83 
    84 void X4X8Multiplex_impl::UpdateFrom(const io_data *data)
    85 {
    86     float u_roll,u_pitch,u_yaw,u_thrust;
    87     float trim_roll,trim_pitch,trim_yaw;
    88     float value[MAX_MOTORS];
    89 
    90     cvmatrix* input=(cvmatrix*)data;
    91 
    92     //on prend une fois pour toute le mutex et on fait des accès directs
    93     input->GetMutex();
    94 
    95     u_roll=input->ValueNoMutex(0,0);
    96     u_pitch=input->ValueNoMutex(1,0);
    97     u_yaw=input->ValueNoMutex(2,0);
    98     u_thrust=input->ValueNoMutex(3,0);
    99     trim_roll=input->ValueNoMutex(4,0);
    100     trim_pitch=input->ValueNoMutex(5,0);
    101     trim_yaw=input->ValueNoMutex(6,0);
    102 
    103     input->ReleaseMutex();