Changeset 15 in flair-src for trunk/lib


Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

Location:
trunk/lib
Files:
334 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairCore/src/AhrsData.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair { namespace core {
     26namespace flair {
     27namespace core {
    2728
    2829/*! \class AhrsDataElement
    2930    */
    30     class AhrsDataElement: public IODataElement {
    31         public:
    32 
    33             AhrsDataElement(const AhrsData *inAhrsData,string name,AhrsData::PlotableData_t inPlotableData):
    34                             IODataElement(inAhrsData,name),
    35                             ahrsData(inAhrsData),
    36                             plotableData(inPlotableData) {
    37 
    38                 size=sizeof(float);
    39             }
    40 
    41             ~AhrsDataElement() {}
    42 
    43             void CopyData(char* destination) const {
    44                 float data;
    45                 Vector3D angularRates;
    46                 Euler eulerAngles;
    47                 Quaternion quaternion;
    48                 ahrsData->GetQuaternionAndAngularRates(quaternion,angularRates);
    49                 quaternion.ToEuler(eulerAngles);
    50                 switch(plotableData) {
    51                 case AhrsData::Roll:
    52                     data=eulerAngles.roll;
    53                     break;
    54                 case AhrsData::Pitch:
    55                     data=eulerAngles.pitch;
    56                     break;
    57                 case AhrsData::Yaw:
    58                     data=eulerAngles.yaw;
    59                     break;
    60                 case AhrsData::RollDeg:
    61                     data=Euler::ToDegree(eulerAngles.roll);
    62                     break;
    63                 case AhrsData::PitchDeg:
    64                     data=Euler::ToDegree(eulerAngles.pitch);
    65                     break;
    66                 case AhrsData::YawDeg:
    67                     data=Euler::ToDegree(eulerAngles.yaw);
    68                     break;
    69                 case AhrsData::Q0:
    70                     data=quaternion.q0;
    71                     break;
    72                 case AhrsData::Q1:
    73                     data=quaternion.q1;
    74                     break;
    75                 case AhrsData::Q2:
    76                     data=quaternion.q2;
    77                     break;
    78                 case AhrsData::Q3:
    79                     data=quaternion.q3;
    80                     break;
    81                 case AhrsData::Wx:
    82                     data=angularRates.x;
    83                     break;
    84                 case AhrsData::Wy:
    85                     data=angularRates.y;
    86                     break;
    87                 case AhrsData::Wz:
    88                     data=angularRates.z;
    89                     break;
    90                 case AhrsData::WxDeg:
    91                     data=Euler::ToDegree(angularRates.x);
    92                     break;
    93                 case AhrsData::WyDeg:
    94                     data=Euler::ToDegree(angularRates.y);
    95                     break;
    96                 case AhrsData::WzDeg:
    97                     data=Euler::ToDegree(angularRates.z);
    98                     break;
    99                 default:
    100                     ahrsData->Err("data type unavailable.\n");
    101                     data=0;
    102                     break;
    103                 }
    104                 memcpy(destination,&data,sizeof(float));
    105             }
    106 
    107             const FloatType &GetDataType(void) const {
    108                 return dataType;
    109             }
    110 
    111         private:
    112             const AhrsData *ahrsData;
    113             AhrsData::PlotableData_t plotableData;
    114             FloatType dataType;
    115 
    116     };
    117 
    118 AhrsData::AhrsData(const Object *parent,std::string name,int n): io_data(parent,name,n), dataType(floatType) {
    119     if(n>1) Warn("n>1 not supported\n");
    120 
    121     AppendLogDescription("q0",floatType);
    122     AppendLogDescription("q1",floatType);
    123     AppendLogDescription("q2",floatType);
    124     AppendLogDescription("q3",floatType);
    125 
    126     AppendLogDescription("wx",floatType);
    127     AppendLogDescription("wy",floatType);
    128     AppendLogDescription("wz",floatType);
     31class AhrsDataElement : public IODataElement {
     32public:
     33  AhrsDataElement(const AhrsData *inAhrsData, string name,
     34                  AhrsData::PlotableData_t inPlotableData)
     35      : IODataElement(inAhrsData, name), ahrsData(inAhrsData),
     36        plotableData(inPlotableData) {
     37
     38    size = sizeof(float);
     39  }
     40
     41  ~AhrsDataElement() {}
     42
     43  void CopyData(char *destination) const {
     44    float data;
     45    Vector3D angularRates;
     46    Euler eulerAngles;
     47    Quaternion quaternion;
     48    ahrsData->GetQuaternionAndAngularRates(quaternion, angularRates);
     49    quaternion.ToEuler(eulerAngles);
     50    switch (plotableData) {
     51    case AhrsData::Roll:
     52      data = eulerAngles.roll;
     53      break;
     54    case AhrsData::Pitch:
     55      data = eulerAngles.pitch;
     56      break;
     57    case AhrsData::Yaw:
     58      data = eulerAngles.yaw;
     59      break;
     60    case AhrsData::RollDeg:
     61      data = Euler::ToDegree(eulerAngles.roll);
     62      break;
     63    case AhrsData::PitchDeg:
     64      data = Euler::ToDegree(eulerAngles.pitch);
     65      break;
     66    case AhrsData::YawDeg:
     67      data = Euler::ToDegree(eulerAngles.yaw);
     68      break;
     69    case AhrsData::Q0:
     70      data = quaternion.q0;
     71      break;
     72    case AhrsData::Q1:
     73      data = quaternion.q1;
     74      break;
     75    case AhrsData::Q2:
     76      data = quaternion.q2;
     77      break;
     78    case AhrsData::Q3:
     79      data = quaternion.q3;
     80      break;
     81    case AhrsData::Wx:
     82      data = angularRates.x;
     83      break;
     84    case AhrsData::Wy:
     85      data = angularRates.y;
     86      break;
     87    case AhrsData::Wz:
     88      data = angularRates.z;
     89      break;
     90    case AhrsData::WxDeg:
     91      data = Euler::ToDegree(angularRates.x);
     92      break;
     93    case AhrsData::WyDeg:
     94      data = Euler::ToDegree(angularRates.y);
     95      break;
     96    case AhrsData::WzDeg:
     97      data = Euler::ToDegree(angularRates.z);
     98      break;
     99    default:
     100      ahrsData->Err("data type unavailable.\n");
     101      data = 0;
     102      break;
     103    }
     104    memcpy(destination, &data, sizeof(float));
     105  }
     106
     107  const FloatType &GetDataType(void) const { return dataType; }
     108
     109private:
     110  const AhrsData *ahrsData;
     111  AhrsData::PlotableData_t plotableData;
     112  FloatType dataType;
     113};
     114
     115AhrsData::AhrsData(const Object *parent, std::string name, int n)
     116    : io_data(parent, name, n), dataType(floatType) {
     117  if (n > 1)
     118    Warn("n>1 not supported\n");
     119
     120  AppendLogDescription("q0", floatType);
     121  AppendLogDescription("q1", floatType);
     122  AppendLogDescription("q2", floatType);
     123  AppendLogDescription("q3", floatType);
     124
     125  AppendLogDescription("wx", floatType);
     126  AppendLogDescription("wy", floatType);
     127  AppendLogDescription("wz", floatType);
    129128}
    130129
    … …  
    132131
    133132Quaternion AhrsData::GetQuaternion(void) const {
    134     Quaternion out;
    135     GetMutex();
    136     out=quaternion;
    137     ReleaseMutex();
    138     return out;
     133  Quaternion out;
     134  GetMutex();
     135  out = quaternion;
     136  ReleaseMutex();
     137  return out;
    139138}
    140139
    141140Vector3D AhrsData::GetAngularRates(void) const {
    142     Vector3D out;
    143     GetMutex();
    144     out=angularRates;
    145     ReleaseMutex();
    146     return out;
    147 }
    148 
    149 void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,Vector3D &outAngularRates) const {
    150     GetMutex();
    151     outQuaternion=quaternion;
    152     outAngularRates=angularRates;
    153     ReleaseMutex();
    154 }
    155 
    156 void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,const Vector3D &inAngularRates) {
    157     GetMutex();
    158     quaternion=inQuaternion;
    159     angularRates=inAngularRates;
    160     ReleaseMutex();
     141  Vector3D out;
     142  GetMutex();
     143  out = angularRates;
     144  ReleaseMutex();
     145  return out;
     146}
     147
     148void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,
     149                                            Vector3D &outAngularRates) const {
     150  GetMutex();
     151  outQuaternion = quaternion;
     152  outAngularRates = angularRates;
     153  ReleaseMutex();
     154}
     155
     156void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,
     157                                            const Vector3D &inAngularRates) {
     158  GetMutex();
     159  quaternion = inQuaternion;
     160  angularRates = inAngularRates;
     161  ReleaseMutex();
    161162}
    162163
    163164void AhrsData::SetQuaternion(const Quaternion &inQuaternion) {
    164     GetMutex();
    165     quaternion=inQuaternion;
    166     ReleaseMutex();
     165  GetMutex();
     166  quaternion = inQuaternion;
     167  ReleaseMutex();
    167168}
    168169
    169170void AhrsData::SetAngularRates(const Vector3D &inAngularRates) {
    170     GetMutex();
    171     angularRates=inAngularRates;
    172     ReleaseMutex();
    173 }
    174 
    175 IODataElement* AhrsData::Element(PlotableData_t plotableData) const {
    176     string name;
    177     switch(plotableData) {
    178     case AhrsData::Roll:
    179         name="Roll";
    180         break;
    181     case AhrsData::Pitch:
    182         name="Pitch";
    183         break;
    184     case AhrsData::Yaw:
    185         name="Yaw";
    186         break;
    187      case AhrsData::RollDeg:
    188         name="Roll degree";
    189         break;
    190     case AhrsData::PitchDeg:
    191         name="Pitch degree";
    192         break;
    193     case AhrsData::YawDeg:
    194         name="Yaw degree";
    195         break;
    196     case AhrsData::Q0:
    197         name="Q0";
    198         break;
    199     case AhrsData::Q1:
    200         name="Q1";
    201         break;
    202     case AhrsData::Q2:
    203         name="Q2";
    204         break;
    205     case AhrsData::Q3:
    206         name="Q3";
    207         break;
    208     case AhrsData::Wx:
    209         name="Wx";
    210         break;
    211     case AhrsData::Wy:
    212         name="Wy";
    213         break;
    214     case AhrsData::Wz:
    215         name="Wz";
    216         break;
    217     case AhrsData::WxDeg:
    218         name="Wx degree";
    219         break;
    220     case AhrsData::WyDeg:
    221         name="Wy degree";
    222         break;
    223     case AhrsData::WzDeg:
    224         name="Wz degree";
    225         break;
    226     default:
    227         Err("data type unavailable.\n");
    228     }
    229 
    230     return new AhrsDataElement(this,name,plotableData);
     171  GetMutex();
     172  angularRates = inAngularRates;
     173  ReleaseMutex();
     174}
     175
     176IODataElement *AhrsData::Element(PlotableData_t plotableData) const {
     177  string name;
     178  switch (plotableData) {
     179  case AhrsData::Roll:
     180    name = "Roll";
     181    break;
     182  case AhrsData::Pitch:
     183    name = "Pitch";
     184    break;
     185  case AhrsData::Yaw:
     186    name = "Yaw";
     187    break;
     188  case AhrsData::RollDeg:
     189    name = "Roll degree";
     190    break;
     191  case AhrsData::PitchDeg:
     192    name = "Pitch degree";
     193    break;
     194  case AhrsData::YawDeg:
     195    name = "Yaw degree";
     196    break;
     197  case AhrsData::Q0:
     198    name = "Q0";
     199    break;
     200  case AhrsData::Q1:
     201    name = "Q1";
     202    break;
     203  case AhrsData::Q2:
     204    name = "Q2";
     205    break;
     206  case AhrsData::Q3:
     207    name = "Q3";
     208    break;
     209  case AhrsData::Wx:
     210    name = "Wx";
     211    break;
     212  case AhrsData::Wy:
     213    name = "Wy";
     214    break;
     215  case AhrsData::Wz:
     216    name = "Wz";
     217    break;
     218  case AhrsData::WxDeg:
     219    name = "Wx degree";
     220    break;
     221  case AhrsData::WyDeg:
     222    name = "Wy degree";
     223    break;
     224  case AhrsData::WzDeg:
     225    name = "Wz degree";
     226    break;
     227  default:
     228    Err("data type unavailable.\n");
     229  }
     230
     231  return new AhrsDataElement(this, name, plotableData);
    231232}
    232233
    233234void AhrsData::CopyDatas(char *dst) const {
    234     GetMutex();
    235     Queue(&dst,&quaternion.q0,sizeof(quaternion.q0));
    236     Queue(&dst,&quaternion.q1,sizeof(quaternion.q1));
    237     Queue(&dst,&quaternion.q2,sizeof(quaternion.q2));
    238     Queue(&dst,&quaternion.q3,sizeof(quaternion.q3));
    239 
    240     Queue(&dst,&angularRates.x,sizeof(angularRates.x));
    241     Queue(&dst,&angularRates.y,sizeof(angularRates.y));
    242     Queue(&dst,&angularRates.z,sizeof(angularRates.z));
    243     ReleaseMutex();
    244 }
    245 
    246 void AhrsData::Queue(char **dst,const void *src,size_t size) const {
    247     memcpy(*dst,src,size);
    248     *dst+=size;
     235  GetMutex();
     236  Queue(&dst, &quaternion.q0, sizeof(quaternion.q0));
     237  Queue(&dst, &quaternion.q1, sizeof(quaternion.q1));
     238  Queue(&dst, &quaternion.q2, sizeof(quaternion.q2));
     239  Queue(&dst, &quaternion.q3, sizeof(quaternion.q3));
     240
     241  Queue(&dst, &angularRates.x, sizeof(angularRates.x));
     242  Queue(&dst, &angularRates.y, sizeof(angularRates.y));
     243  Queue(&dst, &angularRates.z, sizeof(angularRates.z));
     244  ReleaseMutex();
     245}
     246
     247void AhrsData::Queue(char **dst, const void *src, size_t size) const {
     248  memcpy(*dst, src, size);
     249  *dst += size;
    249250}
    250251

  • trunk/lib/FlairCore/src/AhrsData.h

    r2 r15  
    1818#include <Quaternion.h>
    1919
    20 namespace flair { namespace core {
     20namespace flair {
     21namespace core {
    2122
    22     /*! \class AhrsData
    23     *
    24     * \brief Class defining AHRS datas
    25     *
    26     * AHRS datas consist of quaternion and rotational angles values. \n
    27     *
    28     */
    29     class AhrsData: public io_data {
    30         public:
    31             class Type: public DataType {
    32             public:
    33                 Type(const ScalarType &inElementDataType):
    34                     elementDataType(inElementDataType) {}
    35                 const ScalarType  &GetElementDataType() const {return elementDataType;}
    36                 std::string GetDescription() const {return "ahrs data";}
    37                 size_t GetSize() const {
    38                     return 7*elementDataType.GetSize();
    39                 }
    40             private:
    41                 const ScalarType &elementDataType;
    42             };
     23/*! \class AhrsData
     24*
     25* \brief Class defining AHRS datas
     26*
     27* AHRS datas consist of quaternion and rotational angles values. \n
     28*
     29*/
     30class AhrsData : public io_data {
     31public:
     32  class Type : public DataType {
     33  public:
     34    Type(const ScalarType &inElementDataType)
     35        : elementDataType(inElementDataType) {}
     36    const ScalarType &GetElementDataType() const { return elementDataType; }
     37    std::string GetDescription() const { return "ahrs data"; }
     38    size_t GetSize() const { return 7 * elementDataType.GetSize(); }
    4339
    44             /*!
    45             \enum PlotableData_t
    46             \brief Datas wich can be plotted in a DataPlot1D
    47             */
    48             typedef enum {
    49                 Roll/*! roll */, Pitch/*! pitch */, Yaw/*! yaw */,
    50                 RollDeg/*! roll degree*/, PitchDeg/*! pitch degree */, YawDeg/*! yaw degree */,
    51                 Q0/*! quaternion 0 */, Q1/*! quaternion 1 */, Q2/*! quaternion 2 */, Q3/*! quaternion 3 */,
    52                 Wx/*! x filtered angular rate */, Wy/*! y filtered angular rate */, Wz/*! z filtered angular rate */,
    53                 WxDeg/*! x filtered angular rate degree*/, WyDeg/*! y filtered angular rate degree*/, WzDeg/*! z filtered angular rate degree*/,
    54                 } PlotableData_t;
     40  private:
     41    const ScalarType &elementDataType;
     42  };
    5543
    56             /*!
    57             * \brief Constructor
    58             *
    59             * Construct an io_data representing AHRS datas. \n
    60             *
    61             * \param parent parent
    62             * \param name name
    63             * \param n number of samples
    64             */
    65             AhrsData(const Object* parent,std::string name="",int n=1);
     44  /*!
     45  \enum PlotableData_t
     46  \brief Datas wich can be plotted in a DataPlot1D
     47  */
     48  typedef enum {
     49    Roll /*! roll */,
     50    Pitch /*! pitch */,
     51    Yaw /*! yaw */,
     52    RollDeg /*! roll degree*/,
     53    PitchDeg /*! pitch degree */,
     54    YawDeg /*! yaw degree */,
     55    Q0 /*! quaternion 0 */,
     56    Q1 /*! quaternion 1 */,
     57    Q2 /*! quaternion 2 */,
     58    Q3 /*! quaternion 3 */,
     59    Wx /*! x filtered angular rate */,
     60    Wy /*! y filtered angular rate */,
     61    Wz /*! z filtered angular rate */,
     62    WxDeg /*! x filtered angular rate degree*/,
     63    WyDeg /*! y filtered angular rate degree*/,
     64    WzDeg /*! z filtered angular rate degree*/,
     65  } PlotableData_t;
    6666
    67             /*!
    68             * \brief Destructor
    69             *
    70             */
    71             ~AhrsData();
     67  /*!
     68  * \brief Constructor
     69  *
     70  * Construct an io_data representing AHRS datas. \n
     71  *
     72  * \param parent parent
     73  * \param name name
     74  * \param n number of samples
     75  */
     76  AhrsData(const Object *parent, std::string name = "", int n = 1);
    7277
    73             /*!
    74             * \brief Element
    75             *
    76             * Get a pointer to a specific element. This pointer can be used for plotting.
    77             *
    78             * \param plotableData data type
    79             *
    80             * \return pointer to the element
    81             */
    82             IODataElement *Element(PlotableData_t plotableData) const;
     78  /*!
     79  * \brief Destructor
     80  *
     81  */
     82  ~AhrsData();
    8383
    84             /*!
    85             * \brief Set quaternion
    86             *
    87             * This method is mutex protected.
    88             *
    89             * \param quaternion quaternion
    90             *
    91             */
    92             void SetQuaternion(const Quaternion &quaternion);
     84  /*!
     85  * \brief Element
     86  *
     87  * Get a pointer to a specific element. This pointer can be used for plotting.
     88  *
     89  * \param plotableData data type
     90  *
     91  * \return pointer to the element
     92  */
     93  IODataElement *Element(PlotableData_t plotableData) const;
    9394
    94             /*!
    95             * \brief Get quaternion
    96             *
    97             * This method is mutex protected.
    98             *
    99             * \return quaternion
    100             *
    101             */
    102             Quaternion GetQuaternion(void) const;
     95  /*!
     96  * \brief Set quaternion
     97  *
     98  * This method is mutex protected.
     99  *
     100  * \param quaternion quaternion
     101  *
     102  */
     103  void SetQuaternion(const Quaternion &quaternion);
    103104
    104             /*!
    105             * \brief Set angular rates
    106             *
    107             * This method is mutex protected.
    108             *
    109             * \param angularRates angular rates
    110             *
    111             */
    112             void SetAngularRates(const Vector3D &angularRates);
     105  /*!
     106  * \brief Get quaternion
     107  *
     108  * This method is mutex protected.
     109  *
     110  * \return quaternion
     111  *
     112  */
     113  Quaternion GetQuaternion(void) const;
    113114
    114             /*!
    115             * \brief Get angular rates
    116             *
    117             * This method is mutex protected.
    118             *
    119             * \return angular rates
    120             *
    121             */
    122             Vector3D GetAngularRates(void) const;
     115  /*!
     116  * \brief Set angular rates
     117  *
     118  * This method is mutex protected.
     119  *
     120  * \param angularRates angular rates
     121  *
     122  */
     123  void SetAngularRates(const Vector3D &angularRates);
    123124
    124             /*!
    125             * \brief Get both quaternion and angular rates
    126             *
    127             * This method is mutex protected.
    128             *
    129             * \param quaternion quaternion
    130             * \param angularRates angular rates
    131             *
    132             */
    133             void GetQuaternionAndAngularRates(Quaternion &quaternion,Vector3D &angularRates) const;
     125  /*!
     126  * \brief Get angular rates
     127  *
     128  * This method is mutex protected.
     129  *
     130  * \return angular rates
     131  *
     132  */
     133  Vector3D GetAngularRates(void) const;
    134134
    135             /*!
    136             * \brief Set both quaternion and angular rates
    137             *
    138             * This method is mutex protected.
    139             *
    140             * \param quaternion quaternion
    141             * \param angularRates angular rates
    142             *
    143             */
    144             void SetQuaternionAndAngularRates(const Quaternion &quaternion,const Vector3D &angularRates);
     135  /*!
     136  * \brief Get both quaternion and angular rates
     137  *
     138  * This method is mutex protected.
     139  *
     140  * \param quaternion quaternion
     141  * \param angularRates angular rates
     142  *
     143  */
     144  void GetQuaternionAndAngularRates(Quaternion &quaternion,
     145                                    Vector3D &angularRates) const;
    145146
    146             const Type &GetDataType() const {return dataType;}
    147         private:
    148             /*!
    149             * \brief Copy datas
    150             *
    151             * Reimplemented from io_data. \n
    152             * See io_data::CopyDatas.
    153             *
    154             * \param dst destination buffer
    155             */
    156             void CopyDatas(char *dst) const;
     147  /*!
     148  * \brief Set both quaternion and angular rates
     149  *
     150  * This method is mutex protected.
     151  *
     152  * \param quaternion quaternion
     153  * \param angularRates angular rates
     154  *
     155  */
     156  void SetQuaternionAndAngularRates(const Quaternion &quaternion,
     157                                    const Vector3D &angularRates);
    157158
    158             void Queue(char **dst,const void *src,size_t size) const;
     159  const Type &GetDataType() const { return dataType; }
    159160
    160             /*!
    161             * \brief %Quaternion
    162             *
    163             */
    164             Quaternion quaternion;
     161private:
     162  /*!
     163  * \brief Copy datas
     164  *
     165  * Reimplemented from io_data. \n
     166  * See io_data::CopyDatas.
     167  *
     168  * \param dst destination buffer
     169  */
     170  void CopyDatas(char *dst) const;
    165171
    166             /*!
    167             * \brief Angular rates
    168             *
    169             */
    170             Vector3D angularRates;
     172  void Queue(char **dst, const void *src, size_t size) const;
    171173
    172             Type dataType;
    173     };
     174  /*!
     175  * \brief %Quaternion
     176  *
     177  */
     178  Quaternion quaternion;
     179
     180  /*!
     181  * \brief Angular rates
     182  *
     183  */
     184  Vector3D angularRates;
     185
     186  Type dataType;
     187};
    174188
    175189} // end namespace core

  • trunk/lib/FlairCore/src/Box.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair
    24 {
    25 namespace gui
    26 {
     23namespace flair {
     24namespace gui {
    2725
    28 Box::Box(const LayoutPosition* position,string name,string type): Widget(position->getLayout(),name,type)
    29 {
    30     value_changed=true;
    31     SetVolatileXmlProp("row",position->Row());
    32     SetVolatileXmlProp("col",position->Col());
    33     delete position;
     26Box::Box(const LayoutPosition *position, string name, string type)
     27    : Widget(position->getLayout(), name, type) {
     28  value_changed = true;
     29  SetVolatileXmlProp("row", position->Row());
     30  SetVolatileXmlProp("col", position->Col());
     31  delete position;
    3432}
    3533
    36 Box::~Box()
    37 {
    38 core::Object::ObjectName();
     34Box::~Box() { core::Object::ObjectName(); }
     35
     36bool Box::ValueChanged(void) {
     37  bool ret;
     38
     39  GetMutex();
     40  if (value_changed == true) {
     41    value_changed = false;
     42    ret = true;
     43  } else {
     44    ret = false;
     45  }
     46  ReleaseMutex();
     47
     48  return ret;
    3949}
    4050
    41 bool Box::ValueChanged(void)
    42 {
    43     bool ret;
     51void Box::SetValueChanged(void) { value_changed = true; }
    4452
    45     GetMutex();
    46     if(value_changed==true)
    47     {
    48        value_changed=false;
    49        ret=true;
    50     }
    51     else
    52     {
    53         ret=false;
    54     }
    55     ReleaseMutex();
     53void Box::GetMutex(void) const { ((Layout *)Parent())->mutex->GetMutex(); }
    5654
    57     return ret;
    58 }
    59 
    60 void Box::SetValueChanged(void)
    61 {
    62     value_changed=true;
    63 }
    64 
    65 void Box::GetMutex(void) const
    66 {
    67     ((Layout*)Parent())->mutex->GetMutex();
    68 }
    69 
    70 void Box::ReleaseMutex(void) const
    71 {
    72     ((Layout*)Parent())->mutex->ReleaseMutex();
     55void Box::ReleaseMutex(void) const {
     56  ((Layout *)Parent())->mutex->ReleaseMutex();
    7357}
    7458

  • trunk/lib/FlairCore/src/Box.h

    r2 r15  
    1616#include <Widget.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class Layout;
    24     class LayoutPosition;
     21class Layout;
     22class LayoutPosition;
    2523
    26     /*! \class Box
    27     *
    28     * \brief Abstract class to display a box on the ground station
    29     *
    30     * This is an abstract class to display boxes (like CheckBox, SpinBox, etc). \n
    31     * To access reimplemented box's value, use Box::GetMutex and Box::ReleaseMutex. \n
    32     * Note that this mutex is in reality the one from the parent Layout. To minimize memory
    33     * footprint, each Box does not have its own Mutex.
    34     */
    35     class Box: public Widget
    36     {
    37         public:
    38             /*!
    39             * \brief Constructor
    40             *
    41             * Construct a Box. \n
    42             * Type must agree with predifined (hard coded) types
    43             * in ground station code. \n
    44             * The Box will automatically be child of position->getLayout() Layout. After calling this method,
    45             * position will be deleted as it is no longer usefull.
    46             *
    47             * \param position position
    48             * \param name name
    49             * \param type type
    50             */
    51             Box(const LayoutPosition* position,std::string name,std::string type);
     24/*! \class Box
     25*
     26* \brief Abstract class to display a box on the ground station
     27*
     28* This is an abstract class to display boxes (like CheckBox, SpinBox, etc). \n
     29* To access reimplemented box's value, use Box::GetMutex and Box::ReleaseMutex.
     30*\n
     31* Note that this mutex is in reality the one from the parent Layout. To minimize
     32*memory
     33* footprint, each Box does not have its own Mutex.
     34*/
     35class Box : public Widget {
     36public:
     37  /*!
     38  * \brief Constructor
     39  *
     40  * Construct a Box. \n
     41  * Type must agree with predifined (hard coded) types
     42  * in ground station code. \n
     43  * The Box will automatically be child of position->getLayout() Layout. After
     44  *calling this method,
     45  * position will be deleted as it is no longer usefull.
     46  *
     47  * \param position position
     48  * \param name name
     49  * \param type type
     50  */
     51  Box(const LayoutPosition *position, std::string name, std::string type);
    5252
    53             /*!
    54             * \brief Destructor
    55             *
    56             */
    57             ~Box();
     53  /*!
     54  * \brief Destructor
     55  *
     56  */
     57  ~Box();
    5858
    59             /*!
    60             * \brief Has the value changed since last call?
    61             *
    62             * This method returns the value of an internal flag
    63             * which is set through SetValueChanged(). \n
    64             * After calling this method, the internal flag is
    65             * set to false.
    66             *
    67             * \return true is valued has changed since last call
    68             */
    69             bool ValueChanged(void);
     59  /*!
     60  * \brief Has the value changed since last call?
     61  *
     62  * This method returns the value of an internal flag
     63  * which is set through SetValueChanged(). \n
     64  * After calling this method, the internal flag is
     65  * set to false.
     66  *
     67  * \return true is valued has changed since last call
     68  */
     69  bool ValueChanged(void);
    7070
    71         protected:
    72             /*!
    73             * \brief Set the value changed flag
    74             *
    75             * The reimplemented class must call this method when Box's value is changed. \n
    76             * This method must be called with Mutex locked. Indeed, as reimplemented class
    77             * also has to lock the Mutex to change the Box value, this mecanism avoid two successives
    78             * lock and unlock.
    79             *
    80             */
    81             void SetValueChanged(void);
     71protected:
     72  /*!
     73  * \brief Set the value changed flag
     74  *
     75  * The reimplemented class must call this method when Box's value is changed.
     76  *\n
     77  * This method must be called with Mutex locked. Indeed, as reimplemented class
     78  * also has to lock the Mutex to change the Box value, this mecanism avoid two
     79  *successives
     80  * lock and unlock.
     81  *
     82  */
     83  void SetValueChanged(void);
    8284
    83             /*!
    84             * \brief Get Mutex
    85             *
    86             * This method must be called before changing Box's value or
    87             * calling SetValueChanged().
    88             *
    89             */
    90             void GetMutex(void) const;
     85  /*!
     86  * \brief Get Mutex
     87  *
     88  * This method must be called before changing Box's value or
     89  * calling SetValueChanged().
     90  *
     91  */
     92  void GetMutex(void) const;
    9193
    92             /*!
    93             * \brief Release Mutex
    94             *
    95              * This method must be called after changing Box's value or
    96             *  calling SetValueChanged().
    97             *
    98             */
    99             void ReleaseMutex(void) const;
     94  /*!
     95  * \brief Release Mutex
     96  *
     97   * This method must be called after changing Box's value or
     98  *  calling SetValueChanged().
     99  *
     100  */
     101  void ReleaseMutex(void) const;
    100102
    101 
    102         private:
    103             bool value_changed;
    104     };
     103private:
     104  bool value_changed;
     105};
    105106
    106107} // end namespace gui

  • trunk/lib/FlairCore/src/CheckBox.cpp

    r2 r15  
    1919using std::string;
    2020
    21 namespace flair
    22 {
    23 namespace gui
    24 {
     21namespace flair {
     22namespace gui {
    2523
    26 CheckBox::CheckBox(const LayoutPosition* position,string name,bool default_value):Box(position,name,"CheckBox")
    27 {
    28     //update value from xml file
    29     box_value=default_value;
     24CheckBox::CheckBox(const LayoutPosition *position, string name,
     25                   bool default_value)
     26    : Box(position, name, "CheckBox") {
     27  // update value from xml file
     28  box_value = default_value;
    3029
    31     GetPersistentXmlProp("value",box_value);
    32     SetPersistentXmlProp("value",Value());
    33     SendXml();
     30  GetPersistentXmlProp("value", box_value);
     31  SetPersistentXmlProp("value", Value());
     32  SendXml();
    3433}
    3534
    36 CheckBox::~CheckBox()
    37 {
     35CheckBox::~CheckBox() {}
    3836
     37bool CheckBox::IsChecked(void) const {
     38  bool tmp;
     39
     40  GetMutex();
     41  tmp = box_value;
     42  ReleaseMutex();
     43
     44  return tmp;
    3945}
    4046
    41 bool CheckBox::IsChecked(void) const
    42 {
    43     bool tmp;
    44 
    45     GetMutex();
    46     tmp=box_value;
    47     ReleaseMutex();
    48 
    49     return tmp;
     47int CheckBox::Value(void) const {
     48  if (IsChecked() == true)
     49    return 1;
     50  else
     51    return 0;
    5052}
    5153
    52 int CheckBox::Value(void) const
    53 {
    54     if(IsChecked()==true)
    55         return 1;
    56     else
    57         return 0;
    58 }
    59 
    60 void CheckBox::XmlEvent(void)
    61 {
    62     GetMutex();
    63     if(GetPersistentXmlProp("value",box_value)) SetValueChanged();
    64     ReleaseMutex();
     54void CheckBox::XmlEvent(void) {
     55  GetMutex();
     56  if (GetPersistentXmlProp("value", box_value))
     57    SetValueChanged();
     58  ReleaseMutex();
    6559}
    6660

  • trunk/lib/FlairCore/src/CheckBox.h

    r2 r15  
    1616#include <Box.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class LayoutPosition;
     21class LayoutPosition;
    2422
    25     /*! \class CheckBox
    26     *
    27     * \brief Class displaying a QCheckBox on the ground station
    28     *
    29     */
    30     class CheckBox: public Box
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a QCheckBox at given position.
    37             *
    38             * \param position position to display the QCheckBox
    39             * \param name name
    40             * \param default_value default value if not in the xml config file
    41             */
    42             CheckBox(const LayoutPosition* position,std::string name,bool default_value=true);
     23/*! \class CheckBox
     24*
     25* \brief Class displaying a QCheckBox on the ground station
     26*
     27*/
     28class CheckBox : public Box {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a QCheckBox at given position.
     34  *
     35  * \param position position to display the QCheckBox
     36  * \param name name
     37  * \param default_value default value if not in the xml config file
     38  */
     39  CheckBox(const LayoutPosition *position, std::string name,
     40           bool default_value = true);
    4341
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~CheckBox();
     42  /*!
     43  * \brief Destructor
     44  *
     45  */
     46  ~CheckBox();
    4947
    50             /*!
    51             * \brief Is checked?
    52             *
    53             * \return true if checked
    54             */
    55             bool IsChecked(void) const;
     48  /*!
     49  * \brief Is checked?
     50  *
     51  * \return true if checked
     52  */
     53  bool IsChecked(void) const;
    5654
    57             /*!
    58             * \brief Value
    59             *
    60             * \return 1 if checked, 0 otherwise
    61             */
    62             int Value(void) const;
     55  /*!
     56  * \brief Value
     57  *
     58  * \return 1 if checked, 0 otherwise
     59  */
     60  int Value(void) const;
    6361
    64         private:
    65             /*!
    66             * \brief XmlEvent from ground station
    67             *
    68             * Reimplemented from Widget.
    69             *
    70             */
    71             void XmlEvent(void);
     62private:
     63  /*!
     64  * \brief XmlEvent from ground station
     65  *
     66  * Reimplemented from Widget.
     67  *
     68  */
     69  void XmlEvent(void);
    7270
    73             bool box_value;
    74     };
     71  bool box_value;
     72};
    7573
    7674} // end namespace gui

  • trunk/lib/FlairCore/src/ComboBox.cpp

    r2 r15  
    1919using std::string;
    2020
    21 namespace flair
    22 {
    23 namespace gui
    24 {
     21namespace flair {
     22namespace gui {
    2523
    26 ComboBox::ComboBox(const LayoutPosition* position,string name):Box(position,name,"ComboBox")
    27 {
    28     //update value from xml file
    29     box_value=0;
    30     GetPersistentXmlProp("value",box_value);
    31     SetPersistentXmlProp("value",box_value);
     24ComboBox::ComboBox(const LayoutPosition *position, string name)
     25    : Box(position, name, "ComboBox") {
     26  // update value from xml file
     27  box_value = 0;
     28  GetPersistentXmlProp("value", box_value);
     29  SetPersistentXmlProp("value", box_value);
    3230
    33     SendXml();
     31  SendXml();
    3432}
    3533
    36 ComboBox::~ComboBox()
    37 {
     34ComboBox::~ComboBox() {}
    3835
     36void ComboBox::AddItem(string name) {
     37  SetVolatileXmlProp("item", name);
     38  SendXml();
    3939}
    4040
    41 void ComboBox::AddItem(string name)
    42 {
    43     SetVolatileXmlProp("item",name);
    44     SendXml();
     41int ComboBox::CurrentIndex(void) const {
     42  int tmp;
     43
     44  GetMutex();
     45  tmp = box_value;
     46  ReleaseMutex();
     47
     48  return tmp;
    4549}
    4650
    47 int ComboBox::CurrentIndex(void) const
    48 {
    49     int tmp;
    50 
    51     GetMutex();
    52     tmp=box_value;
    53     ReleaseMutex();
    54 
    55     return tmp;
    56 }
    57 
    58 void ComboBox::XmlEvent(void)
    59 {
    60     GetMutex();
    61     if(GetPersistentXmlProp("value",box_value)) SetValueChanged();
    62     ReleaseMutex();
     51void ComboBox::XmlEvent(void) {
     52  GetMutex();
     53  if (GetPersistentXmlProp("value", box_value))
     54    SetValueChanged();
     55  ReleaseMutex();
    6356}
    6457

  • trunk/lib/FlairCore/src/ComboBox.h

    r2 r15  
    1616#include <Box.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class LayoutPosition;
     21class LayoutPosition;
    2422
    25     /*! \class ComboBox
    26     *
    27     * \brief Class displaying a QComboBox on the ground station
    28     *
    29     */
    30     class ComboBox: public Box
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a QComboBox at given position.
    37             *
    38             * \param position position to display the QComboBox
    39             * \param name name
    40             */
    41             ComboBox(const LayoutPosition* position,std::string name);
     23/*! \class ComboBox
     24*
     25* \brief Class displaying a QComboBox on the ground station
     26*
     27*/
     28class ComboBox : public Box {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a QComboBox at given position.
     34  *
     35  * \param position position to display the QComboBox
     36  * \param name name
     37  */
     38  ComboBox(const LayoutPosition *position, std::string name);
    4239
    43             /*!
    44             * \brief Destructor
    45             *
    46             */
    47             ~ComboBox();
     40  /*!
     41  * \brief Destructor
     42  *
     43  */
     44  ~ComboBox();
    4845
    49              /*!
    50             * \brief Add an item
    51             *
    52             * Add an item to the end of the list.
    53             *
    54             * \param name item nam
    55             */
    56             void AddItem(std::string name);
     46  /*!
     47 * \brief Add an item
     48 *
     49 * Add an item to the end of the list.
     50 *
     51 * \param name item nam
     52 */
     53  void AddItem(std::string name);
    5754
    58             /*!
    59             * \brief Currend index
    60             *
    61             * Index of the currently selected item. Items are numbered starting to 0.
    62             *
    63             * \return index number
    64             */
    65             int CurrentIndex(void) const;
     55  /*!
     56  * \brief Currend index
     57  *
     58  * Index of the currently selected item. Items are numbered starting to 0.
     59  *
     60  * \return index number
     61  */
     62  int CurrentIndex(void) const;
    6663
    67         private:
    68             /*!
    69             * \brief XmlEvent from ground station
    70             *
    71             * Reimplemented from Widget.
    72             *
    73             */
    74             void XmlEvent(void);
     64private:
     65  /*!
     66  * \brief XmlEvent from ground station
     67  *
     68  * Reimplemented from Widget.
     69  *
     70  */
     71  void XmlEvent(void);
    7572
    76             int box_value;
    77     };
     73  int box_value;
     74};
    7875
    7976} // end namespace gui

  • trunk/lib/FlairCore/src/ConditionVariable.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair { namespace core {
     23namespace flair {
     24namespace core {
    2425
    25 ConditionVariable::ConditionVariable(const Object* parent,string name): Mutex(parent,name) {
    26      pimpl_=new ConditionVariable_impl(this);
     26ConditionVariable::ConditionVariable(const Object *parent, string name)
     27    : Mutex(parent, name) {
     28  pimpl_ = new ConditionVariable_impl(this);
    2729}
    2830
    29 ConditionVariable::~ConditionVariable() {
    30     delete pimpl_;
     31ConditionVariable::~ConditionVariable() { delete pimpl_; }
     32
     33void ConditionVariable::CondWait(void) { pimpl_->CondWait(); }
     34
     35bool ConditionVariable::CondWaitUntil(Time date) {
     36  return pimpl_->CondWaitUntil(date);
    3137}
    3238
    33 void ConditionVariable::CondWait(void) {
    34     pimpl_->CondWait();
    35 }
    36 
    37 bool ConditionVariable::CondWaitUntil(Time date) {
    38     return pimpl_->CondWaitUntil(date);
    39 }
    40 
    41 void ConditionVariable::CondSignal(void) {
    42     pimpl_->CondSignal();
    43 }
     39void ConditionVariable::CondSignal(void) { pimpl_->CondSignal(); }
    4440
    4541} // end namespace core

  • trunk/lib/FlairCore/src/ConditionVariable.h

    r2 r15  
    1818class ConditionVariable_impl;
    1919
    20 namespace flair
    21 {
    22 namespace core
    23 {
     20namespace flair {
     21namespace core {
    2422
    25     /*! \class ConditionVariable
    26     *
    27     * \brief Class defining a condition variable
    28     *
    29     */
     23/*! \class ConditionVariable
     24*
     25* \brief Class defining a condition variable
     26*
     27*/
    3028
    31     class ConditionVariable: public Mutex
    32     {
    33         public:
    34             /*!
    35             * \brief Constructor
    36             *
    37             * Construct a condition variable with its associated mutex.
    38             *
    39             * \param parent parent
    40             * \param name name
    41             */
    42             ConditionVariable(const Object* parent,std::string name);
     29class ConditionVariable : public Mutex {
     30public:
     31  /*!
     32  * \brief Constructor
     33  *
     34  * Construct a condition variable with its associated mutex.
     35  *
     36  * \param parent parent
     37  * \param name name
     38  */
     39  ConditionVariable(const Object *parent, std::string name);
    4340
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~ConditionVariable();
     41  /*!
     42  * \brief Destructor
     43  *
     44  */
     45  ~ConditionVariable();
    4946
    50             /*!
    51             * \brief Block on the condition variable
    52             *
    53             * This method must be called with mutex locked (see Mutex::GetMutex) by the calling thread or undefined behaviour will result. \n
    54             * It atomically releases mutex and causes the calling thread to block on the condition variable. \n
    55             * Only one thread can be blocked at the same time. \n
    56             * Upon successful return, the mutex has been locked and is owned by the calling thread which should unlock it (see Mutex::ReleaseMutex).
    57             */
    58             void CondWait(void);
     47  /*!
     48  * \brief Block on the condition variable
     49  *
     50  * This method must be called with mutex locked (see Mutex::GetMutex) by the
     51  *calling thread or undefined behaviour will result. \n
     52  * It atomically releases mutex and causes the calling thread to block on the
     53  *condition variable. \n
     54  * Only one thread can be blocked at the same time. \n
     55  * Upon successful return, the mutex has been locked and is owned by the
     56  *calling thread which should unlock it (see Mutex::ReleaseMutex).
     57  */
     58  void CondWait(void);
    5959
    60             /*!
    61             * \brief Block on the condition variable with a timeout
    62             *
    63             * This method must be called with mutex locked (see Mutex::GetMutex) by the calling thread or undefined behaviour will result. \n
    64             * It atomically releases mutex and causes the calling thread to block on the condition variable. \n
    65             * Only one thread can be blocked at the same time. \n
    66             * Upon successful return, the mutex has been locked and is owned by the calling thread which should unlock it (see Mutex::ReleaseMutex).
    67             *
    68             * \param date absolute date
    69             * \return true if the condition variable is signaled before the date specified in parameter elapses, false otherwise
    70             */
    71             bool CondWaitUntil(Time date);
     60  /*!
     61  * \brief Block on the condition variable with a timeout
     62  *
     63  * This method must be called with mutex locked (see Mutex::GetMutex) by the
     64  *calling thread or undefined behaviour will result. \n
     65  * It atomically releases mutex and causes the calling thread to block on the
     66  *condition variable. \n
     67  * Only one thread can be blocked at the same time. \n
     68  * Upon successful return, the mutex has been locked and is owned by the
     69  *calling thread which should unlock it (see Mutex::ReleaseMutex).
     70  *
     71  * \param date absolute date
     72  * \return true if the condition variable is signaled before the date specified
     73  *in parameter elapses, false otherwise
     74  */
     75  bool CondWaitUntil(Time date);
    7276
    73             /*!
    74             * \brief Unblock threads blocked on the condition variable
    75             *
    76             * This method should be called with mutex locked (see Mutex::GetMutex) by the calling thread. \n
    77             * In this case, upon return, the calling thread should unlock the mutex (see Mutex::ReleaseMutex).
    78             *
    79             */
    80             void CondSignal(void);
     77  /*!
     78  * \brief Unblock threads blocked on the condition variable
     79  *
     80  * This method should be called with mutex locked (see Mutex::GetMutex) by the
     81  *calling thread. \n
     82  * In this case, upon return, the calling thread should unlock the mutex (see
     83  *Mutex::ReleaseMutex).
     84  *
     85  */
     86  void CondSignal(void);
    8187
    82         private:
    83             class ConditionVariable_impl* pimpl_;
    84     };
     88private:
     89  class ConditionVariable_impl *pimpl_;
     90};
    8591
    8692} // end namespace core

  • trunk/lib/FlairCore/src/ConditionVariable_impl.cpp

    r2 r15  
    2424using namespace flair::core;
    2525
    26 ConditionVariable_impl::ConditionVariable_impl(ConditionVariable* self) {
    27     this->self=self;
    28     int status;
     26ConditionVariable_impl::ConditionVariable_impl(ConditionVariable *self) {
     27  this->self = self;
     28  int status;
    2929
    3030#ifdef __XENO__
    31     status=rt_cond_create(&m_ResumeCond,NULL);
     31  status = rt_cond_create(&m_ResumeCond, NULL);
    3232#else
    33     status=pthread_cond_init(&m_ResumeCond, 0);
     33  status = pthread_cond_init(&m_ResumeCond, 0);
    3434#endif
    35     if(status!=0) self->Err("error creating condition variable (%s)\n",strerror(-status));
     35  if (status != 0)
     36    self->Err("error creating condition variable (%s)\n", strerror(-status));
    3637}
    3738
    3839ConditionVariable_impl::~ConditionVariable_impl() {
    39     int status;
     40  int status;
    4041
    4142#ifdef __XENO__
    42     status=rt_cond_delete(&m_ResumeCond);
     43  status = rt_cond_delete(&m_ResumeCond);
    4344#else
    44     status=pthread_cond_destroy(&m_ResumeCond);
     45  status = pthread_cond_destroy(&m_ResumeCond);
    4546#endif
    46     if(status!=0) self->Err("error destroying condition variable (%s)\n",strerror(-status));
     47  if (status != 0)
     48    self->Err("error destroying condition variable (%s)\n", strerror(-status));
    4749}
    4850
    4951void ConditionVariable_impl::CondWait(void) {
    50     int status;
     52  int status;
    5153#ifdef __XENO__
    52     status=rt_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex,TM_INFINITE);
     54  status =
     55      rt_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex, TM_INFINITE);
    5356#else
    54     status=pthread_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex);
     57  status = pthread_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex);
    5558#endif
    56     if(status!=0) self->Err("error (%s)\n",strerror(-status));
     59  if (status != 0)
     60    self->Err("error (%s)\n", strerror(-status));
    5761}
    5862
    5963bool ConditionVariable_impl::CondWaitUntil(Time date) {
    60     int status;
     64  int status;
    6165#ifdef __XENO__
    62     status=rt_cond_wait_until(&m_ResumeCond, &self->Mutex::pimpl_->mutex,date);
     66  status = rt_cond_wait_until(&m_ResumeCond, &self->Mutex::pimpl_->mutex, date);
    6367#else
    64     struct timespec restrict;
    65     restrict.tv_sec=date/1000000000;
    66     restrict.tv_nsec=date%1000000000;
    67     status=pthread_cond_timedwait(&m_ResumeCond, &self->Mutex::pimpl_->mutex,&restrict);
     68  struct timespec restrict;
     69  restrict.tv_sec = date / 1000000000;
     70  restrict.tv_nsec = date % 1000000000;
     71  status = pthread_cond_timedwait(&m_ResumeCond, &self->Mutex::pimpl_->mutex,
     72                                  &restrict);
    6873#endif
    69     if (status==0) return true;
    70     if(status!=ETIMEDOUT) self->Err("error (%s)\n",strerror(-status));
    71     return false;
     74  if (status == 0)
     75    return true;
     76  if (status != ETIMEDOUT)
     77    self->Err("error (%s)\n", strerror(-status));
     78  return false;
    7279}
    7380
    7481void ConditionVariable_impl::CondSignal(void) {
    75      int status;
     82  int status;
    7683#ifdef __XENO__
    77     status=rt_cond_signal(&m_ResumeCond);
     84  status = rt_cond_signal(&m_ResumeCond);
    7885#else
    79     status=pthread_cond_signal(&m_ResumeCond);
     86  status = pthread_cond_signal(&m_ResumeCond);
    8087#endif
    81     if(status!=0) self->Err("error (%s)\n",strerror(-status));
     88  if (status != 0)
     89    self->Err("error (%s)\n", strerror(-status));
    8290}

  • trunk/lib/FlairCore/src/ConnectedSocket.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair
    24 {
    25 namespace core
    26 {
     23namespace flair {
     24namespace core {
    2725
    28 ConnectedSocket::ConnectedSocket(const Object* parent,const std::string name):Object(parent,name) {};
     26ConnectedSocket::ConnectedSocket(const Object *parent, const std::string name)
     27    : Object(parent, name){};
    2928
    30 ConnectedSocket::~ConnectedSocket() {};
     29ConnectedSocket::~ConnectedSocket(){};
    3130
    32 uint16_t ConnectedSocket::ReadUInt16(Time const& timeout) {
    33     char buffer[sizeof(uint16_t)];
    34     size_t alreadyReceived=0;
    35     Time remainingTimeout=timeout; //ms
    36     do {
    37         Time beforeTime=GetTime(); //ns
    38         ssize_t received=RecvMessage(buffer+alreadyReceived,sizeof(uint16_t)-alreadyReceived,remainingTimeout);
    39         remainingTimeout-=(GetTime()-beforeTime)/100000;
    40         if ((received<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout");
    41         alreadyReceived+=received;
    42     } while (alreadyReceived!=sizeof(uint16_t));
    43     uint16_t *dataInNetworkEndianness=(uint16_t*)buffer;
    44     uint16_t myinteger=NetworkToHost16(*dataInNetworkEndianness);
    45     return myinteger;
     31uint16_t ConnectedSocket::ReadUInt16(Time const &timeout) {
     32  char buffer[sizeof(uint16_t)];
     33  size_t alreadyReceived = 0;
     34  Time remainingTimeout = timeout; // ms
     35  do {
     36    Time beforeTime = GetTime(); // ns
     37    ssize_t received =
     38        RecvMessage(buffer + alreadyReceived,
     39                    sizeof(uint16_t) - alreadyReceived, remainingTimeout);
     40    remainingTimeout -= (GetTime() - beforeTime) / 100000;
     41    if ((received < 0) || (remainingTimeout < 0))
     42      throw std::runtime_error("Timeout");
     43    alreadyReceived += received;
     44  } while (alreadyReceived != sizeof(uint16_t));
     45  uint16_t *dataInNetworkEndianness = (uint16_t *)buffer;
     46  uint16_t myinteger = NetworkToHost16(*dataInNetworkEndianness);
     47  return myinteger;
    4648}
    4749
    48 void ConnectedSocket::WriteUInt16(uint16_t const& data,Time const& timeout) {
    49     uint16_t dataInNetworkEndianness=HostToNetwork16(data);
    50     char *buffer=(char *)&dataInNetworkEndianness;
    51     size_t alreadySent=0;
    52     Time remainingTimeout=timeout; //ms
    53     do {
    54         Time beforeTime=GetTime(); //ns
    55         ssize_t sent=SendMessage(buffer+alreadySent,sizeof(uint16_t)-alreadySent,remainingTimeout);
    56         remainingTimeout-=(GetTime()-beforeTime)/100000;
    57         if ((sent<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout");
    58         alreadySent+=sent;
    59     } while (alreadySent!=sizeof(uint16_t));
     50void ConnectedSocket::WriteUInt16(uint16_t const &data, Time const &timeout) {
     51  uint16_t dataInNetworkEndianness = HostToNetwork16(data);
     52  char *buffer = (char *)&dataInNetworkEndianness;
     53  size_t alreadySent = 0;
     54  Time remainingTimeout = timeout; // ms
     55  do {
     56    Time beforeTime = GetTime(); // ns
     57    ssize_t sent = SendMessage(
     58        buffer + alreadySent, sizeof(uint16_t) - alreadySent, remainingTimeout);
     59    remainingTimeout -= (GetTime() - beforeTime) / 100000;
     60    if ((sent < 0) || (remainingTimeout < 0))
     61      throw std::runtime_error("Timeout");
     62    alreadySent += sent;
     63  } while (alreadySent != sizeof(uint16_t));
    6064}
    6165
    62 uint32_t ConnectedSocket::ReadUInt32(Time const& timeout) {
    63     char buffer[sizeof(uint32_t)];
    64     size_t alreadyReceived=0;
    65     Time remainingTimeout=timeout; //ms
    66     do {
    67         Time beforeTime=GetTime(); //ns
    68         ssize_t received=RecvMessage(buffer+alreadyReceived,sizeof(uint32_t)-alreadyReceived,remainingTimeout);
    69         remainingTimeout-=(GetTime()-beforeTime)/100000;
    70         if ((received<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout");
    71         alreadyReceived+=received;
    72     } while (alreadyReceived!=sizeof(uint32_t));
    73     uint32_t *dataInNetworkEndianness=(uint32_t*)buffer;
    74     uint32_t myinteger=NetworkToHost32(*dataInNetworkEndianness);
    75     return myinteger;
     66uint32_t ConnectedSocket::ReadUInt32(Time const &timeout) {
     67  char buffer[sizeof(uint32_t)];
     68  size_t alreadyReceived = 0;
     69  Time remainingTimeout = timeout; // ms
     70  do {
     71    Time beforeTime = GetTime(); // ns
     72    ssize_t received =
     73        RecvMessage(buffer + alreadyReceived,
     74                    sizeof(uint32_t) - alreadyReceived, remainingTimeout);
     75    remainingTimeout -= (GetTime() - beforeTime) / 100000;
     76    if ((received < 0) || (remainingTimeout < 0))
     77      throw std::runtime_error("Timeout");
     78    alreadyReceived += received;
     79  } while (alreadyReceived != sizeof(uint32_t));
     80  uint32_t *dataInNetworkEndianness = (uint32_t *)buffer;
     81  uint32_t myinteger = NetworkToHost32(*dataInNetworkEndianness);
     82  return myinteger;
    7683}
    7784
    78 void ConnectedSocket::WriteUInt32(uint32_t const& data,Time const& timeout) {
    79     uint32_t dataInNetworkEndianness=HostToNetwork32(data);
    80     char *buffer=(char *)&dataInNetworkEndianness;
    81     size_t alreadySent=0;
    82     Time remainingTimeout=timeout; //ms
    83     do {
    84         Time beforeTime=GetTime(); //ns
    85         ssize_t sent=SendMessage(buffer+alreadySent,sizeof(uint32_t)-alreadySent,remainingTimeout);
    86         remainingTimeout-=(GetTime()-beforeTime)/100000;
    87         if ((sent<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout");
    88         alreadySent+=sent;
    89     } while (alreadySent!=sizeof(uint32_t));
     85void ConnectedSocket::WriteUInt32(uint32_t const &data, Time const &timeout) {
     86  uint32_t dataInNetworkEndianness = HostToNetwork32(data);
     87  char *buffer = (char *)&dataInNetworkEndianness;
     88  size_t alreadySent = 0;
     89  Time remainingTimeout = timeout; // ms
     90  do {
     91    Time beforeTime = GetTime(); // ns
     92    ssize_t sent = SendMessage(
     93        buffer + alreadySent, sizeof(uint32_t) - alreadySent, remainingTimeout);
     94    remainingTimeout -= (GetTime() - beforeTime) / 100000;
     95    if ((sent < 0) || (remainingTimeout < 0))
     96      throw std::runtime_error("Timeout");
     97    alreadySent += sent;
     98  } while (alreadySent != sizeof(uint32_t));
    9099}
    91100
    92101string ConnectedSocket::ReadString(const size_t &stringSize, Time timeout) {
    93     char buffer[stringSize+1];
    94     size_t alreadyReceived=0;
    95     Time remainingTimeout=timeout; //ms
    96     do {
    97         Time beforeTime=GetTime(); //ns
    98         ssize_t received=RecvMessage(buffer+alreadyReceived,stringSize-alreadyReceived,remainingTimeout);
    99         remainingTimeout-=(GetTime()-beforeTime)/100000;
    100         if ((received<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout");
    101         alreadyReceived+=received;
    102     } while (alreadyReceived!=stringSize);
    103     buffer[stringSize]='\0';
     102  char buffer[stringSize + 1];
     103  size_t alreadyReceived = 0;
     104  Time remainingTimeout = timeout; // ms
     105  do {
     106    Time beforeTime = GetTime(); // ns
     107    ssize_t received =
     108        RecvMessage(buffer + alreadyReceived, stringSize - alreadyReceived,
     109                    remainingTimeout);
     110    remainingTimeout -= (GetTime() - beforeTime) / 100000;
     111    if ((received < 0) || (remainingTimeout < 0))
     112      throw std::runtime_error("Timeout");
     113    alreadyReceived += received;
     114  } while (alreadyReceived != stringSize);
     115  buffer[stringSize] = '\0';
    104116
    105     string mystring=buffer;
    106     return mystring;
     117  string mystring = buffer;
     118  return mystring;
    107119}
    108120

  • trunk/lib/FlairCore/src/ConnectedSocket.h

    r2 r15  
    1616#include <Object.h>
    1717
    18 namespace flair
    19 {
    20 namespace core
    21 {
     18namespace flair {
     19namespace core {
    2220
    23     /*! \class ConnectedSocket
    24     *
    25     * \brief Interface class encapsulating a connected socket. Preserves packets order and guaranty delivery.
    26     *
    27     */
    28     class ConnectedSocket: public Object {
    29     public:
    30         /*!
    31         * \brief Constructor
    32         *
    33         */
    34         ConnectedSocket(const Object* parent,const std::string name);
     21/*! \class ConnectedSocket
     22*
     23* \brief Interface class encapsulating a connected socket. Preserves packets
     24*order and guaranty delivery.
     25*
     26*/
     27class ConnectedSocket : public Object {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  */
     33  ConnectedSocket(const Object *parent, const std::string name);
    3534
    36         /*!
    37         * \brief Destructor
    38         *
    39         */
    40         ~ConnectedSocket();
     35  /*!
     36  * \brief Destructor
     37  *
     38  */
     39  ~ConnectedSocket();
    4140
    42         /*!
    43         * \brief Returns a socket which listens on a specific port/address
    44         *
    45         * \param const Object* parentObject
    46         * \param const string name
    47         * \param unsigned int port
    48         * \param const localAddress (defaults to any)
    49         */
    50         virtual void Listen(const unsigned int port,const std::string localAddress="ANY")=0;
     41  /*!
     42  * \brief Returns a socket which listens on a specific port/address
     43  *
     44  * \param const Object* parentObject
     45  * \param const string name
     46  * \param unsigned int port
     47  * \param const localAddress (defaults to any)
     48  */
     49  virtual void Listen(const unsigned int port,
     50                      const std::string localAddress = "ANY") = 0;
    5151
    52         /*!
    53         * \brief Returns a socket on a new incoming connexion
    54         *
    55         * \param ConnectedSocket &listeningSocket
    56         */
    57         virtual ConnectedSocket *Accept(Time timeout)=0; //should throw an exception if not a listening socket
     52  /*!
     53  * \brief Returns a socket on a new incoming connexion
     54  *
     55  * \param ConnectedSocket &listeningSocket
     56  */
     57  virtual ConnectedSocket *Accept(
     58      Time timeout) = 0; // should throw an exception if not a listening socket
    5859
    59         /*!
    60         * \brief Returns a socket connected to a distant host
    61         *
    62         * \param const Object* parentObject
    63         * \param const string name
    64         * \param unsigned int port
    65         * \param const distantAddress
    66         * \param timeout timeout (in milliseconds)
    67         */
    68         virtual bool Connect(const unsigned int port,const std::string distantAddress,Time timeout)=0;
     60  /*!
     61  * \brief Returns a socket connected to a distant host
     62  *
     63  * \param const Object* parentObject
     64  * \param const string name
     65  * \param unsigned int port
     66  * \param const distantAddress
     67  * \param timeout timeout (in milliseconds)
     68  */
     69  virtual bool Connect(const unsigned int port,
     70                       const std::string distantAddress, Time timeout) = 0;
    6971
    70         /*!
    71         * \brief Send a message
    72         *
    73         * \param message message
    74         * \param message_len message length
    75         * \param timeout timeout (in milliseconds)
    76         */
    77         virtual ssize_t SendMessage(const char* message,size_t message_len,Time timeout)=0;
     72  /*!
     73  * \brief Send a message
     74  *
     75  * \param message message
     76  * \param message_len message length
     77  * \param timeout timeout (in milliseconds)
     78  */
     79  virtual ssize_t SendMessage(const char *message, size_t message_len,
     80                              Time timeout) = 0;
    7881
    79         /*!
    80         * \brief Receive a message
    81         *
    82         * Receive a message and wait up to timeout. \n
    83         *
    84         * \param buf buffer to put the message
    85         * \param buf_len buffer length
    86         * \param timeout timeout (in milliseconds)
    87         *
    88         * \return size of the received message
    89         */
    90         virtual ssize_t RecvMessage(char* buf,size_t buf_len,Time timeout)=0;
     82  /*!
     83  * \brief Receive a message
     84  *
     85  * Receive a message and wait up to timeout. \n
     86  *
     87  * \param buf buffer to put the message
     88  * \param buf_len buffer length
     89  * \param timeout timeout (in milliseconds)
     90  *
     91  * \return size of the received message
     92  */
     93  virtual ssize_t RecvMessage(char *buf, size_t buf_len, Time timeout) = 0;
    9194
    92         std::string ReadString(const size_t &stringLength, Time timeout);
    93         uint16_t ReadUInt16(Time const& timeout);
    94         void WriteUInt16(uint16_t const& data,Time const& timeout);
    95         uint32_t ReadUInt32(Time const& timeout);
    96         void WriteUInt32(uint32_t const& data,Time const& timeout);
     95  std::string ReadString(const size_t &stringLength, Time timeout);
     96  uint16_t ReadUInt16(Time const &timeout);
     97  void WriteUInt16(uint16_t const &data, Time const &timeout);
     98  uint32_t ReadUInt32(Time const &timeout);
     99  void WriteUInt32(uint32_t const &data, Time const &timeout);
    97100
    98         //!! See Socket.h for a more generic implementation of network/host endianness conversion
    99         virtual uint16_t NetworkToHost16(uint16_t data)=0;
    100         virtual uint16_t HostToNetwork16(uint16_t data)=0;
    101         virtual uint32_t NetworkToHost32(uint32_t data)=0;
    102         virtual uint32_t HostToNetwork32(uint32_t data)=0;
     101  //!! See Socket.h for a more generic implementation of network/host endianness
     102  //conversion
     103  virtual uint16_t NetworkToHost16(uint16_t data) = 0;
     104  virtual uint16_t HostToNetwork16(uint16_t data) = 0;
     105  virtual uint32_t NetworkToHost32(uint32_t data) = 0;
     106  virtual uint32_t HostToNetwork32(uint32_t data) = 0;
    103107};
    104108

  • trunk/lib/FlairCore/src/DataPlot.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair
    27 {
    28 namespace gui
    29 {
     26namespace flair {
     27namespace gui {
    3028
    3129using namespace core;
    3230
    33 void RGBFromColor(DataPlot::Color_t color,uint8_t &r,uint8_t &g,uint8_t &b)
    34 {
    35     switch(color)
    36     {
    37         case DataPlot::Red:
    38             r=255;
    39             g=0;
    40             b=0;
    41             break;
    42         case DataPlot::Blue:
    43             r=0;
    44             g=0;
    45             b=255;
    46             break;
    47         case DataPlot::Green:
    48             r=0;
    49             g=255;
    50             b=0;
    51             break;
    52         case DataPlot::Yellow:
    53             r=255;
    54             g=255;
    55             b=0;
    56             break;
    57         case DataPlot::Black:
    58             r=0;
    59             g=0;
    60             b=0;
    61             break;
    62         case DataPlot::White:
    63             r=255;
    64             g=255;
    65             b=255;
    66             break;
    67     }
     31void RGBFromColor(DataPlot::Color_t color, uint8_t &r, uint8_t &g, uint8_t &b) {
     32  switch (color) {
     33  case DataPlot::Red:
     34    r = 255;
     35    g = 0;
     36    b = 0;
     37    break;
     38  case DataPlot::Blue:
     39    r = 0;
     40    g = 0;
     41    b = 255;
     42    break;
     43  case DataPlot::Green:
     44    r = 0;
     45    g = 255;
     46    b = 0;
     47    break;
     48  case DataPlot::Yellow:
     49    r = 255;
     50    g = 255;
     51    b = 0;
     52    break;
     53  case DataPlot::Black:
     54    r = 0;
     55    g = 0;
     56    b = 0;
     57    break;
     58  case DataPlot::White:
     59    r = 255;
     60    g = 255;
     61    b = 255;
     62    break;
     63  }
    6864}
    6965
    70 DataPlot::DataPlot(const LayoutPosition* position,string name,string type):SendData(position,name,type)
    71 {
    72     pimpl_=new DataPlot_impl();
     66DataPlot::DataPlot(const LayoutPosition *position, string name, string type)
     67    : SendData(position, name, type) {
     68  pimpl_ = new DataPlot_impl();
    7369}
    7470
    75 DataPlot::~DataPlot()
    76 {
    77     delete pimpl_;
     71DataPlot::~DataPlot() { delete pimpl_; }
     72
     73void DataPlot::AddDataToSend(const IODataElement *element) {
     74  pimpl_->tosend.push_back(element);
     75
     76  SetSendSize(SendSize() + element->Size());
    7877}
    7978
    80 void DataPlot::AddDataToSend(const IODataElement *element)
    81 {
    82     pimpl_->tosend.push_back(element);
     79void DataPlot::CopyDatas(char *buf) const {
     80  int buf_size = 0;
    8381
    84     SetSendSize(SendSize()+element->Size());
    85 }
    86 
    87 void DataPlot::CopyDatas(char* buf) const {
    88     int buf_size=0;
    89 
    90     for(size_t i=0;i<pimpl_->tosend.size();i++) {
    91         pimpl_->tosend[i]->CopyData(buf+buf_size);
    92         buf_size+=pimpl_->tosend[i]->Size();
    93     }
     82  for (size_t i = 0; i < pimpl_->tosend.size(); i++) {
     83    pimpl_->tosend[i]->CopyData(buf + buf_size);
     84    buf_size += pimpl_->tosend[i]->Size();
     85  }
    9486}
    9587

  • trunk/lib/FlairCore/src/DataPlot.h

    r2 r15  
    1818class DataPlot_impl;
    1919
    20 namespace flair
    21 {
    22     namespace core
    23     {
    24         class IODataElement;
    25     }
     20namespace flair {
     21namespace core {
     22class IODataElement;
    2623}
    27 namespace flair
    28 {
    29 namespace gui
    30 {
    31     class LayoutPosition;
     24}
     25namespace flair {
     26namespace gui {
     27class LayoutPosition;
    3228
    33     /*! \class DataPlot
    34     *
    35     * \brief Abstract class to display plots on ground station
    36     *
    37     */
    38     class DataPlot: public SendData
    39     {
    40         public:
    41             /*!
    42             \enum Color_t
    43             \brief Types of colors
    44             */
    45             typedef enum {
    46                 Red/*! red  */,
    47                 Blue/*! blue  */,
    48                 Green/*! green  */,
    49                 Yellow/*! yellow  */,
    50                 Black/*! black  */,
    51                 White/*! white  */,
    52                 } Color_t;
     29/*! \class DataPlot
     30*
     31* \brief Abstract class to display plots on ground station
     32*
     33*/
     34class DataPlot : public SendData {
     35public:
     36  /*!
     37  \enum Color_t
     38  \brief Types of colors
     39  */
     40  typedef enum {
     41    Red /*! red  */,
     42    Blue /*! blue  */,
     43    Green /*! green  */,
     44    Yellow /*! yellow  */,
     45    Black /*! black  */,
     46    White /*! white  */,
     47  } Color_t;
    5348
    54             /*!
    55             * \brief Constructor
    56             *
    57             * Type must agree with predifined (hard coded) types
    58             * in ground station code. After calling this constructor,
    59             * position will be deleted as it is no longer usefull.
    60             * The DataPlot will automatically be child of position->getLayout() Layout.
    61             *
    62             * \param position position
    63             * \param name nom
    64             * \param type type
    65             */
    66             DataPlot(const LayoutPosition* position,std::string name,std::string type);
     49  /*!
     50  * \brief Constructor
     51  *
     52  * Type must agree with predifined (hard coded) types
     53  * in ground station code. After calling this constructor,
     54  * position will be deleted as it is no longer usefull.
     55  * The DataPlot will automatically be child of position->getLayout() Layout.
     56  *
     57  * \param position position
     58  * \param name nom
     59  * \param type type
     60  */
     61  DataPlot(const LayoutPosition *position, std::string name, std::string type);
    6762
    68             /*!
    69             * \brief Destructor
    70             *
    71             */
    72             ~DataPlot();
     63  /*!
     64  * \brief Destructor
     65  *
     66  */
     67  ~DataPlot();
    7368
    74         protected:
    75             /*!
    76             * \brief Add an IODataElement to the plot.
    77             *
    78             * This method registers element for sending.
    79             *
    80             * \param element element to plot
    81             */
    82             void AddDataToSend(const core::IODataElement *element);
     69protected:
     70  /*!
     71  * \brief Add an IODataElement to the plot.
     72  *
     73  * This method registers element for sending.
     74  *
     75  * \param element element to plot
     76  */
     77  void AddDataToSend(const core::IODataElement *element);
    8378
    84         private:
    85             /*!
    86             * \brief Copy datas to specified buffer
    87             *
    88             * Reimplemented from SendData.
    89             *
    90             * \param buf output buffer
    91             */
    92             void CopyDatas(char* buf) const;
     79private:
     80  /*!
     81  * \brief Copy datas to specified buffer
     82  *
     83  * Reimplemented from SendData.
     84  *
     85  * \param buf output buffer
     86  */
     87  void CopyDatas(char *buf) const;
    9388
    94             /*!
    95             * \brief Extra Xml event
    96             *
    97             * Reimplemented from SendData.
    98             */
    99             void ExtraXmlEvent(void){};
     89  /*!
     90  * \brief Extra Xml event
     91  *
     92  * Reimplemented from SendData.
     93  */
     94  void ExtraXmlEvent(void){};
    10095
    101             class DataPlot_impl* pimpl_;
    102     };
     96  class DataPlot_impl *pimpl_;
     97};
    10398
    104     /*!
    105     * \brief Get RGB components from color type
    106     *
    107     *
    108     * \param color input color
    109     * \param r output component
    110     * \param g output component
    111     * \param b output component
    112     */
    113     void RGBFromColor(DataPlot::Color_t color,uint8_t &r,uint8_t &g,uint8_t &b);
     99/*!
     100* \brief Get RGB components from color type
     101*
     102*
     103* \param color input color
     104* \param r output component
     105* \param g output component
     106* \param b output component
     107*/
     108void RGBFromColor(DataPlot::Color_t color, uint8_t &r, uint8_t &g, uint8_t &b);
    114109
    115110} // end namespace gui

  • trunk/lib/FlairCore/src/DataPlot1D.cpp

    r2 r15  
    2323using std::string;
    2424
    25 namespace flair
    26 {
    27 namespace gui
    28 {
     25namespace flair {
     26namespace gui {
    2927
    3028using namespace core;
    3129
    32 DataPlot1D::DataPlot1D(const LayoutPosition* position,string name,float ymin,float ymax):DataPlot(position,name,"DataPlot1D")
    33 {
    34     SetVolatileXmlProp("min",ymin);
    35     SetVolatileXmlProp("max",ymax);
    36     SendXml();
     30DataPlot1D::DataPlot1D(const LayoutPosition *position, string name, float ymin,
     31                       float ymax)
     32    : DataPlot(position, name, "DataPlot1D") {
     33  SetVolatileXmlProp("min", ymin);
     34  SetVolatileXmlProp("max", ymax);
     35  SendXml();
    3736}
    3837
    39 DataPlot1D::~DataPlot1D() {
     38DataPlot1D::~DataPlot1D() {}
     39
     40void DataPlot1D::AddCurve(const core::IODataElement *element, uint8_t r,
     41                          uint8_t g, uint8_t b, string curve_name) {
     42  if (curve_name != "") {
     43    SetVolatileXmlProp("curve", curve_name);
     44  } else {
     45    SetVolatileXmlProp("curve", element->Parent()->ObjectName() + "\\" +
     46                                    element->ObjectName());
     47  }
     48  SetVolatileXmlProp("type", element->GetDataType().GetDescription());
     49  SetVolatileXmlProp("r", r);
     50  SetVolatileXmlProp("g", g);
     51  SetVolatileXmlProp("b", b);
     52
     53  SendXml();
     54
     55  // save data information
     56  AddDataToSend(element);
    4057}
    4158
    42 void DataPlot1D::AddCurve(const core::IODataElement* element,uint8_t r,uint8_t g,uint8_t b,string curve_name)
    43 {
    44     if(curve_name!="")
    45     {
    46         SetVolatileXmlProp("curve",curve_name);
    47     }
    48     else
    49     {
    50         SetVolatileXmlProp("curve",element->Parent()->ObjectName() + "\\" +element->ObjectName());
    51     }
    52     SetVolatileXmlProp("type",element->GetDataType().GetDescription());
    53     SetVolatileXmlProp("r",r);
    54     SetVolatileXmlProp("g",g);
    55     SetVolatileXmlProp("b",b);
    56 
    57     SendXml();
    58 
    59     //save data information
    60     AddDataToSend(element);
    61 }
    62 
    63 void DataPlot1D::AddCurve(const core::IODataElement* element,Color_t color,string curve_name) {
    64     uint8_t r,g,b;
    65     RGBFromColor(color,r,g,b);
    66     AddCurve(element,r,g,b,curve_name);
     59void DataPlot1D::AddCurve(const core::IODataElement *element, Color_t color,
     60                          string curve_name) {
     61  uint8_t r, g, b;
     62  RGBFromColor(color, r, g, b);
     63  AddCurve(element, r, g, b, curve_name);
    6764}
    6865

  • trunk/lib/FlairCore/src/DataPlot1D.h

    r2 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21 namespace core
    22 {
    23     class IODataElement;
     19namespace flair {
     20namespace core {
     21class IODataElement;
    2422}
    2523
    26 namespace gui
    27 {
     24namespace gui {
    2825
    29     class LayoutPosition;
     26class LayoutPosition;
    3027
    31     /*! \class DataPlot1D
    32     *
    33     * \brief Class displaying a 1D plot on the ground station
    34     *
    35     */
    36     class DataPlot1D: private DataPlot
    37     {
    38         public:
    39             /*!
    40             * \brief Constructor
    41             *
    42             * Construct a 1D plot at given position.
    43             *
    44             * \param position position to display the plot
    45             * \param name name
    46             * \param ymin default ymin of the plot
    47             * \param ymax default ymax of the plot
    48             */
    49             DataPlot1D(const LayoutPosition* position,std::string name,float ymin,float ymax);
     28/*! \class DataPlot1D
     29*
     30* \brief Class displaying a 1D plot on the ground station
     31*
     32*/
     33class DataPlot1D : private DataPlot {
     34public:
     35  /*!
     36  * \brief Constructor
     37  *
     38  * Construct a 1D plot at given position.
     39  *
     40  * \param position position to display the plot
     41  * \param name name
     42  * \param ymin default ymin of the plot
     43  * \param ymax default ymax of the plot
     44  */
     45  DataPlot1D(const LayoutPosition *position, std::string name, float ymin,
     46             float ymax);
    5047
    51             /*!
    52             * \brief Destructor
    53             *
    54             */
    55             ~DataPlot1D();
     48  /*!
     49  * \brief Destructor
     50  *
     51  */
     52  ~DataPlot1D();
    5653
    57             /*!
    58             * \brief Add a curve from an IODataElement to the plot.
    59             *
    60             * Curve's color can be selected by its name.
    61             *
    62             * \param element element to plot
    63             * \param color color of the curve
    64             * \param curve_name name of the curve for the legend, if unspecified, element->ObjectName() will be used
    65             */
    66             void AddCurve(const core::IODataElement* element,Color_t color,std::string curve_name="");
     54  /*!
     55  * \brief Add a curve from an IODataElement to the plot.
     56  *
     57  * Curve's color can be selected by its name.
     58  *
     59  * \param element element to plot
     60  * \param color color of the curve
     61  * \param curve_name name of the curve for the legend, if unspecified,
     62  *element->ObjectName() will be used
     63  */
     64  void AddCurve(const core::IODataElement *element, Color_t color,
     65                std::string curve_name = "");
    6766
    68             /*!
    69             * \brief Add a curve from an IODataElement to the plot.
    70             *
    71             * Curve's color can be selected by its RGB components.
    72             *
    73             * \param element element to plot
    74             * \param r red component of the curve
    75             * \param g green component of the curve
    76             * \param b blue component of the curve
    77             * \param curve_name name of the curve for the legend, if unspecified, element->ObjectName() will be used
    78             */
    79             void AddCurve(const core::IODataElement* element,uint8_t r=255,uint8_t g=0,uint8_t b=0,std::string curve_name="");
    80     };
     67  /*!
     68  * \brief Add a curve from an IODataElement to the plot.
     69  *
     70  * Curve's color can be selected by its RGB components.
     71  *
     72  * \param element element to plot
     73  * \param r red component of the curve
     74  * \param g green component of the curve
     75  * \param b blue component of the curve
     76  * \param curve_name name of the curve for the legend, if unspecified,
     77  *element->ObjectName() will be used
     78  */
     79  void AddCurve(const core::IODataElement *element, uint8_t r = 255,
     80                uint8_t g = 0, uint8_t b = 0, std::string curve_name = "");
     81};
    8182
    8283} // end namespace gui

  • trunk/lib/FlairCore/src/DataPlot2D.cpp

    r2 r15  
    2323using std::string;
    2424
    25 namespace flair { namespace gui {
     25namespace flair {
     26namespace gui {
    2627
    27 DataPlot2D::DataPlot2D(const LayoutPosition* position,string name,string x_name,float xmin,float xmax,string y_name,float ymin,float ymax):DataPlot(position,name,"DataPlot2D") {
    28     SetVolatileXmlProp("xmin",xmin);
    29     SetVolatileXmlProp("xmax",xmax);
    30     SetVolatileXmlProp("ymin",ymin);
    31     SetVolatileXmlProp("ymax",ymax);
    32     SetVolatileXmlProp("x_name",x_name);
    33     SetVolatileXmlProp("y_name",y_name);
    34     SendXml();
     28DataPlot2D::DataPlot2D(const LayoutPosition *position, string name,
     29                       string x_name, float xmin, float xmax, string y_name,
     30                       float ymin, float ymax)
     31    : DataPlot(position, name, "DataPlot2D") {
     32  SetVolatileXmlProp("xmin", xmin);
     33  SetVolatileXmlProp("xmax", xmax);
     34  SetVolatileXmlProp("ymin", ymin);
     35  SetVolatileXmlProp("ymax", ymax);
     36  SetVolatileXmlProp("x_name", x_name);
     37  SetVolatileXmlProp("y_name", y_name);
     38  SendXml();
    3539}
    3640
    3741DataPlot2D::~DataPlot2D() {}
    3842
    39 void DataPlot2D::AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,Color_t color,string curve_name) {
    40     uint8_t r,g,b;
    41     RGBFromColor(color,r,g,b);
    42     AddCurve(x_element,y_element,r,g,b,curve_name);
     43void DataPlot2D::AddCurve(const core::IODataElement *x_element,
     44                          const core::IODataElement *y_element, Color_t color,
     45                          string curve_name) {
     46  uint8_t r, g, b;
     47  RGBFromColor(color, r, g, b);
     48  AddCurve(x_element, y_element, r, g, b, curve_name);
    4349}
    4450
    45 void DataPlot2D::AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,uint8_t r,uint8_t g,uint8_t b,string curve_name) {
    46     if(typeid(x_element->GetDataType())!=typeid(y_element->GetDataType())) {
    47         Err("x_element type is different from y_element type (%i-%i)\n",x_element->GetDataType().GetDescription().c_str(),y_element->GetDataType().GetDescription().c_str());
    48         //gerer erreur
    49         return;
    50     }
     51void DataPlot2D::AddCurve(const core::IODataElement *x_element,
     52                          const core::IODataElement *y_element, uint8_t r,
     53                          uint8_t g, uint8_t b, string curve_name) {
     54  if (typeid(x_element->GetDataType()) != typeid(y_element->GetDataType())) {
     55    Err("x_element type is different from y_element type (%i-%i)\n",
     56        x_element->GetDataType().GetDescription().c_str(),
     57        y_element->GetDataType().GetDescription().c_str());
     58    // gerer erreur
     59    return;
     60  }
    5161
    52     if(curve_name!="") {
    53         SetVolatileXmlProp("curve",curve_name);
    54     } else {
    55         SetVolatileXmlProp("curve",x_element->Parent()->ObjectName() + "\\" +x_element->ObjectName()+ "\\" +y_element->ObjectName());
    56     }
    57     SetVolatileXmlProp("type",x_element->GetDataType().GetDescription());
    58     SetVolatileXmlProp("r",r);
    59     SetVolatileXmlProp("g",g);
    60     SetVolatileXmlProp("b",b);
     62  if (curve_name != "") {
     63    SetVolatileXmlProp("curve", curve_name);
     64  } else {
     65    SetVolatileXmlProp("curve", x_element->Parent()->ObjectName() + "\\" +
     66                                    x_element->ObjectName() + "\\" +
     67                                    y_element->ObjectName());
     68  }
     69  SetVolatileXmlProp("type", x_element->GetDataType().GetDescription());
     70  SetVolatileXmlProp("r", r);
     71  SetVolatileXmlProp("g", g);
     72  SetVolatileXmlProp("b", b);
    6173
    62     SendXml();
     74  SendXml();
    6375
    64     //save data information
    65     AddDataToSend(x_element);
    66     AddDataToSend(y_element);
     76  // save data information
     77  AddDataToSend(x_element);
     78  AddDataToSend(y_element);
    6779}
    6880

  • trunk/lib/FlairCore/src/DataPlot2D.h

    r2 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class IODataElement;
    24     }
     19namespace flair {
     20namespace core {
     21class IODataElement;
    2522}
    26 namespace flair
    27 {
    28 namespace gui
    29 {
     23}
     24namespace flair {
     25namespace gui {
    3026
    31     class LayoutPosition;
     27class LayoutPosition;
    3228
    33     /*! \class DataPlot2D
    34     *
    35     * \brief Class displaying a 2D plot on the ground station
    36     *
    37     */
    38     class DataPlot2D: private DataPlot
    39     {
    40         public:
    41             /*!
    42             * \brief Constructor
    43             *
    44             * Construct a 2D plot at given place position.
    45             *
    46             * \param position position to display the plot
    47             * \param name name
    48             * \param x_name name of x axis
    49             * \param xmin default xmin of the plot
    50             * \param xmax default xmax of the plot
    51             * \param y_name name of y axis
    52             * \param ymin default ymin of the plot
    53             * \param ymax default ymax of the plot
    54             */
    55             DataPlot2D(const LayoutPosition* position,std::string name,std::string x_name,float xmin,float xmax,std::string y_name,float ymin,float ymax);
     29/*! \class DataPlot2D
     30*
     31* \brief Class displaying a 2D plot on the ground station
     32*
     33*/
     34class DataPlot2D : private DataPlot {
     35public:
     36  /*!
     37  * \brief Constructor
     38  *
     39  * Construct a 2D plot at given place position.
     40  *
     41  * \param position position to display the plot
     42  * \param name name
     43  * \param x_name name of x axis
     44  * \param xmin default xmin of the plot
     45  * \param xmax default xmax of the plot
     46  * \param y_name name of y axis
     47  * \param ymin default ymin of the plot
     48  * \param ymax default ymax of the plot
     49  */
     50  DataPlot2D(const LayoutPosition *position, std::string name,
     51             std::string x_name, float xmin, float xmax, std::string y_name,
     52             float ymin, float ymax);
    5653
    57             /*!
    58             * \brief Destructor
    59             *
    60             */
    61             ~DataPlot2D();
     54  /*!
     55  * \brief Destructor
     56  *
     57  */
     58  ~DataPlot2D();
    6259
    63             /*!
    64             * \brief Add a curve from elements to the plot.
    65             *
    66             * Curve's color can be selected by its name.
    67             *
    68             * \param x_element element to plot for x coordinate
    69             * \param y_element element to plot for y coordinate
    70             * \param color color of the curve
    71             * \param curve_name name of the curve ofr the legend
    72             */
    73             void AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,Color_t color,std::string curve_name="");
     60  /*!
     61  * \brief Add a curve from elements to the plot.
     62  *
     63  * Curve's color can be selected by its name.
     64  *
     65  * \param x_element element to plot for x coordinate
     66  * \param y_element element to plot for y coordinate
     67  * \param color color of the curve
     68  * \param curve_name name of the curve ofr the legend
     69  */
     70  void AddCurve(const core::IODataElement *x_element,
     71                const core::IODataElement *y_element, Color_t color,
     72                std::string curve_name = "");
    7473
    75             /*!
    76             * \brief Add a curve from elements to the plot.
    77             *
    78             * Curve's color can be selected by its RGB components.
    79             *
    80             * \param x_element element to plot for x coordinate
    81             * \param y_element element to plot for y coordinate
    82             * \param r red component of the curve
    83             * \param g green component of the curve
    84             * \param b blue component of the curve
    85             * \param curve_name name of the curve ofr the legend
    86             */
    87             void AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,uint8_t r=255,uint8_t g=0,uint8_t b=0,std::string curve_name="");
    88     };
     74  /*!
     75  * \brief Add a curve from elements to the plot.
     76  *
     77  * Curve's color can be selected by its RGB components.
     78  *
     79  * \param x_element element to plot for x coordinate
     80  * \param y_element element to plot for y coordinate
     81  * \param r red component of the curve
     82  * \param g green component of the curve
     83  * \param b blue component of the curve
     84  * \param curve_name name of the curve ofr the legend
     85  */
     86  void AddCurve(const core::IODataElement *x_element,
     87                const core::IODataElement *y_element, uint8_t r = 255,
     88                uint8_t g = 0, uint8_t b = 0, std::string curve_name = "");
     89};
    8990
    9091} // end namespace gui

  • trunk/lib/FlairCore/src/DataPlot_impl.cpp

    r2 r15  
    1818#include "DataPlot_impl.h"
    1919
    20 DataPlot_impl::DataPlot_impl()
    21 {
    22 }
     20DataPlot_impl::DataPlot_impl() {}
    2321
    24 DataPlot_impl::~DataPlot_impl()
    25 {
    26 }
     22DataPlot_impl::~DataPlot_impl() {}

  • trunk/lib/FlairCore/src/DoubleSpinBox.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair
    23 {
    24 namespace gui
    25 {
     22namespace flair {
     23namespace gui {
    2624
    27 DoubleSpinBox::DoubleSpinBox(const LayoutPosition* position,string name,double min,double max,double step,int decimals,double default_value):Box(position,name,"DoubleSpinBox")
    28 {
    29     //update value from xml file
    30     if(default_value<min) default_value=min;
    31     if(default_value>max) default_value=max;
    32     box_value=default_value;
     25DoubleSpinBox::DoubleSpinBox(const LayoutPosition *position, string name,
     26                             double min, double max, double step, int decimals,
     27                             double default_value)
     28    : Box(position, name, "DoubleSpinBox") {
     29  // update value from xml file
     30  if (default_value < min)
     31    default_value = min;
     32  if (default_value > max)
     33    default_value = max;
     34  box_value = default_value;
    3335
    34     SetVolatileXmlProp("min",min);
    35     SetVolatileXmlProp("max",max);
    36     SetVolatileXmlProp("step",step);
    37     SetVolatileXmlProp("decimals",decimals);
    38     GetPersistentXmlProp("value",box_value);
    39     SetPersistentXmlProp("value",box_value);
     36  SetVolatileXmlProp("min", min);
     37  SetVolatileXmlProp("max", max);
     38  SetVolatileXmlProp("step", step);
     39  SetVolatileXmlProp("decimals", decimals);
     40  GetPersistentXmlProp("value", box_value);
     41  SetPersistentXmlProp("value", box_value);
    4042
    41     SendXml();
     43  SendXml();
    4244}
    4345
    44 DoubleSpinBox::DoubleSpinBox(const LayoutPosition* position,string name,string suffix,double min,double max,double step,int decimals,double default_value):Box(position,name,"DoubleSpinBox")
    45 {
    46     //update value from xml file
    47     if(default_value<min) default_value=min;
    48     if(default_value>max) default_value=max;
    49     box_value=default_value;
     46DoubleSpinBox::DoubleSpinBox(const LayoutPosition *position, string name,
     47                             string suffix, double min, double max, double step,
     48                             int decimals, double default_value)
     49    : Box(position, name, "DoubleSpinBox") {
     50  // update value from xml file
     51  if (default_value < min)
     52    default_value = min;
     53  if (default_value > max)
     54    default_value = max;
     55  box_value = default_value;
    5056
    51     SetVolatileXmlProp("suffix",suffix);
    52     SetVolatileXmlProp("min",min);
    53     SetVolatileXmlProp("max",max);
    54     SetVolatileXmlProp("step",step);
    55     SetVolatileXmlProp("decimals",decimals);
    56     GetPersistentXmlProp("value",box_value);
    57     SetPersistentXmlProp("value",box_value);
     57  SetVolatileXmlProp("suffix", suffix);
     58  SetVolatileXmlProp("min", min);
     59  SetVolatileXmlProp("max", max);
     60  SetVolatileXmlProp("step", step);
     61  SetVolatileXmlProp("decimals", decimals);
     62  GetPersistentXmlProp("value", box_value);
     63  SetPersistentXmlProp("value", box_value);
    5864
    59     SendXml();
     65  SendXml();
    6066}
    6167
    62 DoubleSpinBox::~DoubleSpinBox()
    63 {
     68DoubleSpinBox::~DoubleSpinBox() {}
    6469
     70double DoubleSpinBox::Value(void) const {
     71  double tmp;
     72
     73  GetMutex();
     74  tmp = box_value;
     75  ReleaseMutex();
     76
     77  return tmp;
    6578}
    6679
    67 double DoubleSpinBox::Value(void) const
    68 {
    69     double tmp;
    70 
    71     GetMutex();
    72     tmp=box_value;
    73     ReleaseMutex();
    74 
    75     return tmp;
    76 }
    77 
    78 void DoubleSpinBox::XmlEvent(void)
    79 {
    80     GetMutex();
    81     if(GetPersistentXmlProp("value",box_value)) SetValueChanged();
    82     ReleaseMutex();
     80void DoubleSpinBox::XmlEvent(void) {
     81  GetMutex();
     82  if (GetPersistentXmlProp("value", box_value))
     83    SetValueChanged();
     84  ReleaseMutex();
    8385}
    8486

  • trunk/lib/FlairCore/src/DoubleSpinBox.h

    r2 r15  
    1616#include <Box.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class Layout;
     21class Layout;
    2422
    25     /*! \class DoubleSpinBox
    26     *
    27     * \brief Class displaying a QDoubleSpinBox on the ground station
    28     *
    29     */
    30     class DoubleSpinBox: public Box
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a QDoubleSpinBox at given position. \n
    37             * The QDoubleSpinBox is saturated to min and max values.
    38             *
    39             * \param position position to display the QDoubleSpinBox
    40             * \param name name
    41             * \param min minimum value
    42             * \param max maximum value
    43             * \param step step
    44             * \param decimals number of decimals
    45             * \param default_value default value if not in the xml config file
    46             */
    47             DoubleSpinBox(const LayoutPosition* position,std::string name,double min,double max,double step,int decimals=2,double default_value=0);
     23/*! \class DoubleSpinBox
     24*
     25* \brief Class displaying a QDoubleSpinBox on the ground station
     26*
     27*/
     28class DoubleSpinBox : public Box {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a QDoubleSpinBox at given position. \n
     34  * The QDoubleSpinBox is saturated to min and max values.
     35  *
     36  * \param position position to display the QDoubleSpinBox
     37  * \param name name
     38  * \param min minimum value
     39  * \param max maximum value
     40  * \param step step
     41  * \param decimals number of decimals
     42  * \param default_value default value if not in the xml config file
     43  */
     44  DoubleSpinBox(const LayoutPosition *position, std::string name, double min,
     45                double max, double step, int decimals = 2,
     46                double default_value = 0);
    4847
    49             /*!
    50             * \brief Constructor
    51             *
    52             * Construct a QDoubleSpinBox at given position. \n
    53             * The QDoubleSpinBox is saturated to min and max values.
    54             *
    55             * \param position position to display the QDoubleSpinBox
    56             * \param name name
    57             * \param suffix suffix for the value (eg unit)
    58             * \param min minimum value
    59             * \param max maximum value
    60             * \param step step
    61             * \param decimals number of decimals
    62             * \param default_value default value if not in the xml config file
    63             */
    64             DoubleSpinBox(const LayoutPosition* position,std::string name,std::string suffix,double min,double max,double step,int decimals=2,double default_value=0);
     48  /*!
     49  * \brief Constructor
     50  *
     51  * Construct a QDoubleSpinBox at given position. \n
     52  * The QDoubleSpinBox is saturated to min and max values.
     53  *
     54  * \param position position to display the QDoubleSpinBox
     55  * \param name name
     56  * \param suffix suffix for the value (eg unit)
     57  * \param min minimum value
     58  * \param max maximum value
     59  * \param step step
     60  * \param decimals number of decimals
     61  * \param default_value default value if not in the xml config file
     62  */
     63  DoubleSpinBox(const LayoutPosition *position, std::string name,
     64                std::string suffix, double min, double max, double step,
     65                int decimals = 2, double default_value = 0);
    6566
    66             /*!
    67             * \brief Destructor
    68             *
    69             */
    70             ~DoubleSpinBox();
     67  /*!
     68  * \brief Destructor
     69  *
     70  */
     71  ~DoubleSpinBox();
    7172
    72             /*!
    73             * \brief Value
    74             *
    75             * \return value
    76             */
    77             double Value(void) const;
     73  /*!
     74  * \brief Value
     75  *
     76  * \return value
     77  */
     78  double Value(void) const;
    7879
    79         private:
    80             /*!
    81             * \brief XmlEvent from ground station
    82             *
    83             * Reimplemented from Widget.
    84             *
    85             */
    86             void XmlEvent(void);
     80private:
     81  /*!
     82  * \brief XmlEvent from ground station
     83  *
     84  * Reimplemented from Widget.
     85  *
     86  */
     87  void XmlEvent(void);
    8788
    88             double box_value;
    89     };
     89  double box_value;
     90};
    9091
    9192} // end namespace gui

  • trunk/lib/FlairCore/src/Euler.cpp

    r2 r15  
    2323#define PI_D ((double)3.14159265358979323846)
    2424
    25 namespace flair
    26 {
    27 namespace core
    28 {
     25namespace flair {
     26namespace core {
    2927
    30 Euler::Euler(float inRoll,float inPitch,float inYaw): roll(inRoll), pitch(inPitch),yaw(inYaw) {
    31 }
     28Euler::Euler(float inRoll, float inPitch, float inYaw)
     29    : roll(inRoll), pitch(inPitch), yaw(inYaw) {}
    3230
    33 Euler::~Euler() {
     31Euler::~Euler() {}
    3432
    35 }
    36 
    37 Euler& Euler::operator=(const Euler &euler) {
    38     roll=euler.roll;
    39     pitch=euler.pitch;
    40     yaw=euler.yaw;
    41     return (*this);
     33Euler &Euler::operator=(const Euler &euler) {
     34  roll = euler.roll;
     35  pitch = euler.pitch;
     36  yaw = euler.yaw;
     37  return (*this);
    4238}
    4339/*
    … …  
    8581*/
    8682void Euler::ToQuaternion(Quaternion &quaternion) const {
    87     quaternion.q0=cos(yaw/2)*cos(pitch/2)*cos(roll/2)
    88         +sin(yaw/2)*sin(pitch/2)*sin(roll/2);
     83  quaternion.q0 = cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) +
     84                  sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2);
    8985
    90     quaternion.q1=cos(yaw/2)*cos(pitch/2)*sin(roll/2)
    91         -sin(yaw/2)*sin(pitch/2)*cos(roll/2);
     86  quaternion.q1 = cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) -
     87                  sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2);
    9288
    93     quaternion.q2=cos(yaw/2)*sin(pitch/2)*cos(roll/2)
    94         +sin(yaw/2)*cos(pitch/2)*sin(roll/2);
     89  quaternion.q2 = cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) +
     90                  sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2);
    9591
    96     quaternion.q3=sin(yaw/2)*cos(pitch/2)*cos(roll/2)
    97         -cos(yaw/2)*sin(pitch/2)*sin(roll/2);
     92  quaternion.q3 = sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) -
     93                  cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2);
    9894}
    9995
    10096Quaternion Euler::ToQuaternion(void) const {
    101     Quaternion quaternion;
    102     ToQuaternion(quaternion);
    103     return quaternion;
     97  Quaternion quaternion;
     98  ToQuaternion(quaternion);
     99  return quaternion;
    104100}
    105101
    106 float Euler::ToDegree(float radianValue) {
    107     return radianValue*180.0f/PI;
    108 }
     102float Euler::ToDegree(float radianValue) { return radianValue * 180.0f / PI; }
    109103
    110 float Euler::ToRadian(float degreeValue) {
    111     return degreeValue/180.0f*PI;
    112 }
     104float Euler::ToRadian(float degreeValue) { return degreeValue / 180.0f * PI; }
    113105
    114106float Euler::YawDistanceFrom(float angle) const {
    115     float rot1,rot2;
    116     if(angle > yaw ) {
    117         rot1=angle-yaw;
    118         rot2=2*PI-angle+yaw;
    119     } else {
    120         rot1=2*PI+angle-yaw;
    121         rot2=yaw-angle;
    122     }
    123     if(rot2<rot1) rot1=-rot2;
    124     rot1=-rot1;//pour avoir rot1=yaw-angle
     107  float rot1, rot2;
     108  if (angle > yaw) {
     109    rot1 = angle - yaw;
     110    rot2 = 2 * PI - angle + yaw;
     111  } else {
     112    rot1 = 2 * PI + angle - yaw;
     113    rot2 = yaw - angle;
     114  }
     115  if (rot2 < rot1)
     116    rot1 = -rot2;
     117  rot1 = -rot1; // pour avoir rot1=yaw-angle
    125118
    126     return rot1;
     119  return rot1;
    127120}
    128121

  • trunk/lib/FlairCore/src/Euler.h

    r2 r15  
    1414#define EULER_H
    1515
    16 namespace flair { namespace core {
    17     class Quaternion;
     16namespace flair {
     17namespace core {
     18class Quaternion;
    1819
    19     /*! \class Euler
    20     *
    21     * \brief Class defining euler angles
    22     *
    23     * Euler angles are expressed in radians.
    24     *
    25     */
    26     class Euler {
    27         public:
    28             /*!
    29             * \brief Constructor
    30             *
    31             * Construct euler angles using specified values.
    32             *
    33             * \param roll roll value
    34             * \param pitch pitch value
    35             * \param yaw yaw value
    36             */
    37             Euler(float roll=0,float pitch=0,float yaw=0);
     20/*! \class Euler
     21*
     22* \brief Class defining euler angles
     23*
     24* Euler angles are expressed in radians.
     25*
     26*/
     27class Euler {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct euler angles using specified values.
     33  *
     34  * \param roll roll value
     35  * \param pitch pitch value
     36  * \param yaw yaw value
     37  */
     38  Euler(float roll = 0, float pitch = 0, float yaw = 0);
    3839
    39             /*!
    40             * \brief Destructor
    41             *
    42             */
    43             ~Euler();
     40  /*!
     41  * \brief Destructor
     42  *
     43  */
     44  ~Euler();
    4445
    45             /*!
    46             * \brief x axis rotation
    47             *
    48             * \param value rotation value in radians
    49             */
    50             //todo: passer par un quaternion
    51             //void RotateX(float value);
     46  /*!
     47  * \brief x axis rotation
     48  *
     49  * \param value rotation value in radians
     50  */
     51  // todo: passer par un quaternion
     52  // void RotateX(float value);
    5253
    53             /*!
    54             * \brief x axis rotation
    55             *
    56             * \param value rotation value in degrees
    57             */
    58             //void RotateXDeg(float value);
     54  /*!
     55  * \brief x axis rotation
     56  *
     57  * \param value rotation value in degrees
     58  */
     59  // void RotateXDeg(float value);
    5960
    60             /*!
    61             * \brief y axis rotation
    62             *
    63             * \param value rotation value in radians
    64             */
    65             //void RotateY(float value);
     61  /*!
     62  * \brief y axis rotation
     63  *
     64  * \param value rotation value in radians
     65  */
     66  // void RotateY(float value);
    6667
    67             /*!
    68             * \brief y axis rotation
    69             *
    70             * \param value rotation value in degrees
    71             */
    72             //void RotateYDeg(float value);
     68  /*!
     69  * \brief y axis rotation
     70  *
     71  * \param value rotation value in degrees
     72  */
     73  // void RotateYDeg(float value);
    7374
    74             /*!
    75             * \brief z axis rotation
    76             *
    77             * \param value rotation value in radians
    78             */
    79             //void RotateZ(float value);
     75  /*!
     76  * \brief z axis rotation
     77  *
     78  * \param value rotation value in radians
     79  */
     80  // void RotateZ(float value);
    8081
    81             /*!
    82             * \brief z axis rotation
    83             *
    84             * \param value rotation value in degrees
    85             */
    86             //void RotateZDeg(float value);
     82  /*!
     83  * \brief z axis rotation
     84  *
     85  * \param value rotation value in degrees
     86  */
     87  // void RotateZDeg(float value);
    8788
    88             /*!
    89             * \brief Convert to quaternion
    90             *
    91             * \param quaternion output quaternion
    92             */
    93             void ToQuaternion(Quaternion &quaternion) const;
     89  /*!
     90  * \brief Convert to quaternion
     91  *
     92  * \param quaternion output quaternion
     93  */
     94  void ToQuaternion(Quaternion &quaternion) const;
    9495
    95             /*!
    96             * \brief Convert to quaternion
    97             *
    98             * \return quaternion
    99             */
    100             Quaternion ToQuaternion(void) const;
    101             /*!
    102             * \brief Convert from radian to degree
    103             *
    104             * \param radianValue value in radian
    105             *
    106             * \return value in degree
    107             */
    108             static float ToDegree(float radianValue);
     96  /*!
     97  * \brief Convert to quaternion
     98  *
     99  * \return quaternion
     100  */
     101  Quaternion ToQuaternion(void) const;
     102  /*!
     103  * \brief Convert from radian to degree
     104  *
     105  * \param radianValue value in radian
     106  *
     107  * \return value in degree
     108  */
     109  static float ToDegree(float radianValue);
    109110
    110             /*!
    111             * \brief Convert from degree to radian
    112             *
    113             * \param degreeValue value in degree
    114             *
    115             * \return value in radian
    116             */
    117             static float ToRadian(float degreeValue);
     111  /*!
     112  * \brief Convert from degree to radian
     113  *
     114  * \param degreeValue value in degree
     115  *
     116  * \return value in radian
     117  */
     118  static float ToRadian(float degreeValue);
    118119
    119             /*!
    120             * \brief Compute yaw distance
    121             *
    122             * Compute yaw distance from given angle. This is the minimum distance.
    123             *
    124             * \param angle starting angle
    125             *
    126             * \return value distance in radian
    127             */
    128             float YawDistanceFrom(float angle) const;
     120  /*!
     121  * \brief Compute yaw distance
     122  *
     123  * Compute yaw distance from given angle. This is the minimum distance.
     124  *
     125  * \param angle starting angle
     126  *
     127  * \return value distance in radian
     128  */
     129  float YawDistanceFrom(float angle) const;
    129130
    130             /*!
    131             * \brief roll value
    132             */
    133             float roll;
     131  /*!
     132  * \brief roll value
     133  */
     134  float roll;
    134135
    135             /*!
    136             * \brief pitch value
    137             */
    138             float pitch;
     136  /*!
     137  * \brief pitch value
     138  */
     139  float pitch;
    139140
    140             /*!
    141             * \brief yaw value
    142             */
    143             float yaw;
     141  /*!
     142  * \brief yaw value
     143  */
     144  float yaw;
    144145
    145             Euler& operator=(const Euler &euler);
    146 
    147     };
     146  Euler &operator=(const Euler &euler);
     147};
    148148
    149149} // end namespace core

  • trunk/lib/FlairCore/src/FrameworkManager.cpp

    r2 r15  
    2626using namespace flair::gui;
    2727
    28 namespace
    29 {
    30     flair::core::FrameworkManager* frameworkmanager=NULL;
     28namespace {
     29flair::core::FrameworkManager *frameworkmanager = NULL;
    3130}
    3231
     32namespace flair {
     33namespace core {
    3334
    34 namespace flair
    35 {
    36 namespace core
    37 {
     35FrameworkManager *getFrameworkManager(void) { return frameworkmanager; }
    3836
     37bool IsBigEndian(void) {
     38  union {
     39    uint32_t i;
     40    char c[4];
     41  } bint = {0x01020304};
    3942
    40 FrameworkManager* getFrameworkManager(void) {
    41     return frameworkmanager;
     43  if (bint.c[0] == 1) {
     44    return true;
     45  } else {
     46    return false;
     47  }
    4248}
    4349
    44 bool IsBigEndian(void) {
    45     union {
    46         uint32_t i;
    47         char c[4];
    48     } bint = {0x01020304};
     50FrameworkManager::FrameworkManager(string name)
     51    : Object(NULL, name, XML_ROOT_TYPE) {
     52  if (frameworkmanager != NULL) {
     53    Err("FrameworkManager should be instanced only one time\n");
     54    return;
     55  }
    4956
    50     if(bint.c[0] == 1) {
    51         return true;
    52     } else {
    53         return false;
    54     }
    55 }
    56 
    57 FrameworkManager::FrameworkManager(string name): Object(NULL,name,XML_ROOT_TYPE)
    58 {
    59     if(frameworkmanager!=NULL) {
    60         Err("FrameworkManager should be instanced only one time\n");
    61         return;
    62     }
    63 
    64     Printf(SVN_REV);
    65     frameworkmanager=this;
    66     pimpl_=new FrameworkManager_impl(this,name);
     57  Printf(SVN_REV);
     58  frameworkmanager = this;
     59  pimpl_ = new FrameworkManager_impl(this, name);
    6760}
    6861
    6962FrameworkManager::~FrameworkManager() {
    70     //Printf("destruction FrameworkManager\n");
     63  // Printf("destruction FrameworkManager\n");
    7164
    72     //stop ui_com thread (which sends datas for graphs), we do not need it as graphs will be destroyed
    73     if(getUiCom()!=NULL) {
    74         getUiCom()->SafeStop();
    75         getUiCom()->Join();
     65  // stop ui_com thread (which sends datas for graphs), we do not need it as
     66  // graphs will be destroyed
     67  if (getUiCom() != NULL) {
     68    getUiCom()->SafeStop();
     69    getUiCom()->Join();
     70  }
     71
     72  // we start by deleting threads (except pimpl which must be deleted at last)
     73  // (before deleting objects that could be used by the threads)
     74  // Printf("delete Threads\n");
     75  for (vector<const Object *>::iterator it = Childs()->begin();
     76       it < Childs()->end(); it++) {
     77    // Printf("child %i %s
     78    // %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
     79    if ((*it)->ObjectType() == "Thread") {
     80      if (*it != pimpl_) {
     81        // Printf("del\n");
     82        delete (Thread *)(*it);
     83        // Childs() vector has been modified, we start from beginning again
     84        // it will be incremented by the loop, so in fact we start again at
     85        // begin()+1
     86        // it is not a problem since begin is pimpl
     87        it = Childs()->begin();
     88        // Printf("del ok\n");
     89      }
    7690    }
     91  }
    7792
    78     //we start by deleting threads (except pimpl which must be deleted at last)
    79     // (before deleting objects that could be used by the threads)
    80     //Printf("delete Threads\n");
    81     for(vector<const Object*>::iterator it=Childs()->begin() ; it < Childs()->end(); it++ ) {
    82         //Printf("child %i %s %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
    83         if((*it)->ObjectType()=="Thread") {
    84             if(*it!=pimpl_) {
    85                 //Printf("del\n");
    86                 delete (Thread*)(*it);
    87                 //Childs() vector has been modified, we start from beginning again
    88                 //it will be incremented by the loop, so in fact we start again at begin()+1
    89                 //it is not a problem since begin is pimpl
    90                 it=Childs()->begin();
    91                 //Printf("del ok\n");
    92             }
    93         }
     93  // next we delete IODevice
     94  // (before deleting objects that could be used by the IODevice)
     95  // Printf("delete IODevices\n");
     96  for (vector<const Object *>::iterator it = Childs()->begin();
     97       it < Childs()->end(); it++) {
     98    // Printf("child %i %s
     99    // %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
     100    if ((*it)->ObjectType() == "IODevice") {
     101      // Printf("del\n");
     102      delete (IODevice *)*it;
     103      // Printf("del ok\n");
     104      // Childs() vector has been modified, we start from beginning again
     105      // it will be incremented by the loop, so in fact we start again at
     106      // begin()+1
     107      // it is not a problem since begin is pimpl (not an IODevice)
     108      it = Childs()->begin();
     109      // Printf("del ok\n");
    94110    }
     111  }
    95112
    96     //next we delete IODevice
    97     // (before deleting objects that could be used by the IODevice)
    98     //Printf("delete IODevices\n");
    99     for(vector<const Object*>::iterator it=Childs()->begin() ; it < Childs()->end(); it++ ) {
    100         //Printf("child %i %s %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
    101         if((*it)->ObjectType()=="IODevice") {
    102             //Printf("del\n");
    103             delete (IODevice*)*it;
    104             //Printf("del ok\n");
    105             //Childs() vector has been modified, we start from beginning again
    106             //it will be incremented by the loop, so in fact we start again at begin()+1
    107             //it is not a problem since begin is pimpl (not an IODevice)
    108             it=Childs()->begin();
    109             //Printf("del ok\n");
    110         }
    111     }
     113  // Printf("delete childs\n");
     114  // on efface les enfants en commencant par la fin
     115  // le ui_com puis FrameworkManager_impl est détruit en dernier
     116  // permet de garder l'uicom pour notifer des destructions
     117  while (Childs()->size() != 0) {
     118    // Printf("child %i %s
     119    // %s\n",Childs()->size(),Childs()->back()->ObjectName().c_str(),Childs()->back()->ObjectType().c_str());
     120    if (Childs()->back() != NULL)
     121      delete Childs()->back();
     122  }
    112123
    113 //Printf("delete childs\n");
    114     //on efface les enfants en commencant par la fin
    115     //le ui_com puis FrameworkManager_impl est détruit en dernier
    116     //permet de garder l'uicom pour notifer des destructions
    117     while(Childs()->size()!=0) {
    118         //Printf("child %i %s %s\n",Childs()->size(),Childs()->back()->ObjectName().c_str(),Childs()->back()->ObjectType().c_str());
    119         if(Childs()->back()!=NULL) delete Childs()->back();
    120     }
     124  // childs.clear();
    121125
    122     //childs.clear();
    123 
    124 //Printf("destruction FrameworkManager ok\n");
     126  // Printf("destruction FrameworkManager ok\n");
    125127}
    126128
    127 void FrameworkManager::SetupConnection(string address,uint16_t port,size_t rcv_buf_size) {
    128     pimpl_->SetupConnection(address,port,rcv_buf_size);
     129void FrameworkManager::SetupConnection(string address, uint16_t port,
     130                                       size_t rcv_buf_size) {
     131  pimpl_->SetupConnection(address, port, rcv_buf_size);
    129132}
    130133
    131134void FrameworkManager::SetupUserInterface(string xml_file) {
    132     pimpl_->SetupUserInterface(xml_file);
     135  pimpl_->SetupUserInterface(xml_file);
    133136}
    134137
    135 gui::TabWidget* FrameworkManager::GetTabWidget(void) const {
    136     return pimpl_->tabwidget;
     138gui::TabWidget *FrameworkManager::GetTabWidget(void) const {
     139  return pimpl_->tabwidget;
    137140}
    138141
    139142void FrameworkManager::UpdateSendData(const SendData *obj) {
    140     if(getUiCom()!=NULL) getUiCom()->UpdateSendData(obj);
     143  if (getUiCom() != NULL)
     144    getUiCom()->UpdateSendData(obj);
    141145}
    142146
    143147void FrameworkManager::BlockCom(void) {
    144     if(getUiCom()!=NULL) getUiCom()->Block();
     148  if (getUiCom() != NULL)
     149    getUiCom()->Block();
    145150}
    146151
    147152void FrameworkManager::UnBlockCom(void) {
    148     if(getUiCom()!=NULL) getUiCom()->UnBlock();
     153  if (getUiCom() != NULL)
     154    getUiCom()->UnBlock();
    149155}
    150156
    151157bool FrameworkManager::ConnectionLost(void) const {
    152     if(getUiCom()!=NULL) {
    153         return (getUiCom()->ConnectionLost()| pimpl_->connection_lost);
    154     } else {
    155         return false;
    156     }
     158  if (getUiCom() != NULL) {
     159    return (getUiCom()->ConnectionLost() | pimpl_->connection_lost);
     160  } else {
     161    return false;
     162  }
    157163}
    158164
    159165void FrameworkManager::SetupLogger(string log_path) {
    160     pimpl_->SetupLogger(log_path);
     166  pimpl_->SetupLogger(log_path);
    161167}
    162168
    163169void FrameworkManager::AddDeviceToLog(IODevice *device) {
    164     pimpl_->AddDeviceToLog(device);
     170  pimpl_->AddDeviceToLog(device);
    165171}
    166172
    167 void FrameworkManager::StartLog(void) {
    168     pimpl_->StartLog();
    169 }
     173void FrameworkManager::StartLog(void) { pimpl_->StartLog(); }
    170174
    171 void FrameworkManager::StopLog(void) {
    172     pimpl_->StopLog();
    173 }
     175void FrameworkManager::StopLog(void) { pimpl_->StopLog(); }
    174176
    175 bool FrameworkManager::IsLogging(void) const {
    176     return pimpl_->is_logging;
    177 }
     177bool FrameworkManager::IsLogging(void) const { return pimpl_->is_logging; }
    178178
    179179void FrameworkManager::DisableErrorsDisplay(bool value) {
    180     pimpl_->disable_errors=value;
     180  pimpl_->disable_errors = value;
    181181}
    182182
    183183bool FrameworkManager::IsDisplayingErrors(void) const {
    184     return !(pimpl_->disable_errors);
     184  return !(pimpl_->disable_errors);
    185185}
    186186

  • trunk/lib/FlairCore/src/FrameworkManager.h

    r2 r15  
    1818class FrameworkManager_impl;
    1919
    20 namespace flair
    21 {
    22     namespace gui
    23     {
    24         class TabWidget;
    25         class SendData;
    26     }
     20namespace flair {
     21namespace gui {
     22class TabWidget;
     23class SendData;
    2724}
    28 namespace flair
    29 {
    30 namespace core
    31 {
    32     class IODevice;
    33 
    34     /*! \class FrameworkManager
    35     *
    36     * \brief Main class of the Framework library
    37     *
    38     * This is the main class of the library. Only one instance of this class is allowed
    39     * by program. Morevoer, its name must be unique if more than one program using this class
    40     * is running on the same system (a control and a simlator for example). \n
    41     * This class allows: \n
    42     * -connexion with ground station, \n
    43     * -creation of a QTabWidget on ground station, \n
    44     * -handling of xml files, used for default values of Widgets, \n
    45     * -logging of datas.
    46     */
    47     class FrameworkManager: public Object
    48     {
    49         public:
    50             /*!
    51             * \brief Constructor
    52             *
    53             * Construct a FrameworkManager. \n
    54             * Call SetupConnection method just after this constructor to setup the conection with a ground station.
    55             *
    56             * \param name name, must be unique
    57             */
    58             FrameworkManager(std::string name);
    59 
    60             /*!
    61             * \brief Destructor
    62             *
    63             * Calling it will automatically destruct all childs. \n
    64             * Destruction implies destruction of the QTabWidget on ground station.
    65             *
    66             */
    67             ~FrameworkManager();
    68 
    69             /*!
    70             * \brief Setup the connection with ground station
    71             *
    72             * Call this method just after the constructor of this class. If this method is not called, the program will run headless.
    73             *
    74             * \param address address of ground station
    75             * \param port port of ground station
    76             * \param rcv_buf_size receive buffer size
    77             */
    78             void SetupConnection(std::string address,uint16_t port,size_t rcv_buf_size=10000);
    79 
    80             /*!
    81             * \brief Setup the user interface
    82             *
    83             * If this method is called after SetupConnection, Widgets will be displayed in the ground station.
    84             * Otherwise, it will run headless, but default values of Widgets will be taken from the xml file.
    85             *
    86             * \param xml_file xml file for default values of Widgets
    87             */
    88             void SetupUserInterface(std::string xml_file);
    89 
    90             /*!
    91             * \brief Get TabWidget
    92             *
    93             * \return TabWidget
    94             */
    95             gui::TabWidget* GetTabWidget(void) const;
    96 
    97             /*!
    98             * \brief Logger setup
    99             *
    100             * Setup path of log files. \n
    101             * No logging will be performed if this method is not called. \n
    102             *
    103             * \param log_path path to store logs
    104             */
    105             void SetupLogger(std::string log_path);
    106 
    107             /*!
    108             * \brief Add log element
    109             *
    110             * The added element will be automatically logged once
    111             * logging started (see StartLog()). \n
    112             * This element must define on its side the io_data
    113             * to log, trough IODevice::SetDataToLog method.
    114             *
    115             * \param device IODevice to add
    116             */
    117             void AddDeviceToLog(IODevice *device);
    118 
    119             /*!
    120             * \brief Start logging
    121             *
    122             * All IODevice added through AddDeviceToLog() method
    123             * will automatically be logged. \n
    124             * SetupLogger() must have been called before.
    125             */
    126             void StartLog(void);
    127 
    128             /*!
    129             * \brief Stop logging
    130             *
    131             * Logs will automatically be sent to ground station.
    132             */
    133             void StopLog(void);
    134 
    135             /*!
    136             * \brief Is logging?
    137             *
    138             * \return true if is logging
    139             */
    140             bool IsLogging(void) const;
    141 
    142             /*!
    143             * \brief Notify that SendData's period has changed
    144             *
    145             * This funtion must be called when the period has changed. \n
    146             * Normally, it occurs in the Widget::XmlEvent method. \n
    147             * This method must be called with communication blocked (see BlockCom()).
    148             *
    149             * \param obj SendData which changed
    150             */
    151             void UpdateSendData(const gui::SendData *obj);
    152 
    153             /*!
    154             * \brief Block communication
    155             *
    156             * This funtion blocks the communication beetween the program and ground station. \n
    157             * It must be called before changing datas or parameters exchanged between the program
    158             * and the ground station.
    159             *
    160             */
    161             void BlockCom(void);
    162 
    163              /*!
    164             * \brief Unblock communication
    165             *
    166             * This funtion unblocks the communication beetween the program and ground station. \n
    167             * It must be called after changing datas or parameters exchanged between the program
    168             * and the ground station.
    169             *
    170             */
    171             void UnBlockCom(void);
    172 
    173             /*!
    174             * \brief Is connection lost?
    175             *
    176             * Once this method returns true, it will never return false back. \n
    177             * Note that this method return false if no connection is defined (see SetupConnection).
    178             *
    179             * \return true if connection with ground station is lost
    180             */
    181             bool ConnectionLost(void) const;
    182 
    183             /*!
    184             * \brief Disable errors display
    185             *
    186             * Disable errors display, if you do not want to saturate console for exemple.
    187             * By defaults errors disply is enabled.
    188             *
    189             * \param value true to disable errors display
    190             */
    191             void DisableErrorsDisplay(bool value);
    192 
    193             /*!
    194             * \brief Is displaying errors?
    195             *
    196             *
    197             * \return true if errors display is enabled
    198             */
    199             bool IsDisplayingErrors(void) const;
    200 
    201         private:
    202             class FrameworkManager_impl* pimpl_;
    203     };
    204 
    205     /*!
    206     * \brief get FrameworkManager
    207     *
    208     * \return the FrameworkManager
    209     */
    210     FrameworkManager* getFrameworkManager(void);
    211 
    212     /*!
    213     * \brief is big endian?
    214     *
    215     * \return true if big endian, false if little endian
    216     */
    217     bool IsBigEndian(void);
     25}
     26namespace flair {
     27namespace core {
     28class IODevice;
     29
     30/*! \class FrameworkManager
     31*
     32* \brief Main class of the Framework library
     33*
     34* This is the main class of the library. Only one instance of this class is
     35*allowed
     36* by program. Morevoer, its name must be unique if more than one program using
     37*this class
     38* is running on the same system (a control and a simlator for example). \n
     39* This class allows: \n
     40* -connexion with ground station, \n
     41* -creation of a QTabWidget on ground station, \n
     42* -handling of xml files, used for default values of Widgets, \n
     43* -logging of datas.
     44*/
     45class FrameworkManager : public Object {
     46public:
     47  /*!
     48  * \brief Constructor
     49  *
     50  * Construct a FrameworkManager. \n
     51  * Call SetupConnection method just after this constructor to setup the
     52  *conection with a ground station.
     53  *
     54  * \param name name, must be unique
     55  */
     56  FrameworkManager(std::string name);
     57
     58  /*!
     59  * \brief Destructor
     60  *
     61  * Calling it will automatically destruct all childs. \n
     62  * Destruction implies destruction of the QTabWidget on ground station.
     63  *
     64  */
     65  ~FrameworkManager();
     66
     67  /*!
     68  * \brief Setup the connection with ground station
     69  *
     70  * Call this method just after the constructor of this class. If this method is
     71  *not called, the program will run headless.
     72  *
     73  * \param address address of ground station
     74  * \param port port of ground station
     75  * \param rcv_buf_size receive buffer size
     76  */
     77  void SetupConnection(std::string address, uint16_t port,
     78                       size_t rcv_buf_size = 10000);
     79
     80  /*!
     81  * \brief Setup the user interface
     82  *
     83  * If this method is called after SetupConnection, Widgets will be displayed in
     84  *the ground station.
     85  * Otherwise, it will run headless, but default values of Widgets will be taken
     86  *from the xml file.
     87  *
     88  * \param xml_file xml file for default values of Widgets
     89  */
     90  void SetupUserInterface(std::string xml_file);
     91
     92  /*!
     93  * \brief Get TabWidget
     94  *
     95  * \return TabWidget
     96  */
     97  gui::TabWidget *GetTabWidget(void) const;
     98
     99  /*!
     100  * \brief Logger setup
     101  *
     102  * Setup path of log files. \n
     103  * No logging will be performed if this method is not called. \n
     104  *
     105  * \param log_path path to store logs
     106  */
     107  void SetupLogger(std::string log_path);
     108
     109  /*!
     110  * \brief Add log element
     111  *
     112  * The added element will be automatically logged once
     113  * logging started (see StartLog()). \n
     114  * This element must define on its side the io_data
     115  * to log, trough IODevice::SetDataToLog method.
     116  *
     117  * \param device IODevice to add
     118  */
     119  void AddDeviceToLog(IODevice *device);
     120
     121  /*!
     122  * \brief Start logging
     123  *
     124  * All IODevice added through AddDeviceToLog() method
     125  * will automatically be logged. \n
     126  * SetupLogger() must have been called before.
     127  */
     128  void StartLog(void);
     129
     130  /*!
     131  * \brief Stop logging
     132  *
     133  * Logs will automatically be sent to ground station.
     134  */
     135  void StopLog(void);
     136
     137  /*!
     138  * \brief Is logging?
     139  *
     140  * \return true if is logging
     141  */
     142  bool IsLogging(void) const;
     143
     144  /*!
     145  * \brief Notify that SendData's period has changed
     146  *
     147  * This funtion must be called when the period has changed. \n
     148  * Normally, it occurs in the Widget::XmlEvent method. \n
     149  * This method must be called with communication blocked (see BlockCom()).
     150  *
     151  * \param obj SendData which changed
     152  */
     153  void UpdateSendData(const gui::SendData *obj);
     154
     155  /*!
     156  * \brief Block communication
     157  *
     158  * This funtion blocks the communication beetween the program and ground
     159  *station. \n
     160  * It must be called before changing datas or parameters exchanged between the
     161  *program
     162  * and the ground station.
     163  *
     164  */
     165  void BlockCom(void);
     166
     167  /*!
     168 * \brief Unblock communication
     169 *
     170 * This funtion unblocks the communication beetween the program and ground
     171 *station. \n
     172 * It must be called after changing datas or parameters exchanged between the
     173 *program
     174 * and the ground station.
     175 *
     176 */
     177  void UnBlockCom(void);
     178
     179  /*!
     180  * \brief Is connection lost?
     181  *
     182  * Once this method returns true, it will never return false back. \n
     183  * Note that this method return false if no connection is defined (see
     184  *SetupConnection).
     185  *
     186  * \return true if connection with ground station is lost
     187  */
     188  bool ConnectionLost(void) const;
     189
     190  /*!
     191  * \brief Disable errors display
     192  *
     193  * Disable errors display, if you do not want to saturate console for exemple.
     194  * By defaults errors disply is enabled.
     195  *
     196  * \param value true to disable errors display
     197  */
     198  void DisableErrorsDisplay(bool value);
     199
     200  /*!
     201  * \brief Is displaying errors?
     202  *
     203  *
     204  * \return true if errors display is enabled
     205  */
     206  bool IsDisplayingErrors(void) const;
     207
     208private:
     209  class FrameworkManager_impl *pimpl_;
     210};
     211
     212/*!
     213* \brief get FrameworkManager
     214*
     215* \return the FrameworkManager
     216*/
     217FrameworkManager *getFrameworkManager(void);
     218
     219/*!
     220* \brief is big endian?
     221*
     222* \return true if big endian, false if little endian
     223*/
     224bool IsBigEndian(void);
    218225
    219226} // end namespace core

  • trunk/lib/FlairCore/src/FrameworkManager_impl.cpp

    r2 r15  
    4343using namespace flair::gui;
    4444
    45 ui_com* FrameworkManager_impl::com=NULL;
    46 FrameworkManager_impl* FrameworkManager_impl::_this=NULL;
     45ui_com *FrameworkManager_impl::com = NULL;
     46FrameworkManager_impl *FrameworkManager_impl::_this = NULL;
    4747
    4848namespace {
    … …  
    5050
    5151#ifdef SIGDEBUG
    52     static const char *reason_str[] = {
    53         "undefined",
    54         "received signal",
    55         "invoked syscall",
    56         "triggered fault",
    57         "affected by priority inversion",
    58          "missing mlockall",
    59         "runaway thread",
    60     };
    61 
    62     void warn_upon_switch(int sig, siginfo_t *si, void *context)     {
    63         unsigned int reason = si->si_value.sival_int;
    64         void *bt[32];
    65         int nentries;
     52static const char *reason_str[] = {
     53    "undefined", "received signal", "invoked syscall", "triggered fault",
     54    "affected by priority inversion", "missing mlockall", "runaway thread",
     55};
     56
     57void warn_upon_switch(int sig, siginfo_t *si, void *context) {
     58  unsigned int reason = si->si_value.sival_int;
     59  void *bt[32];
     60  int nentries;
    6661#ifdef SIGDEBUG_WATCHDOG
    67         printf("\nSIGDEBUG received, reason %d: %s\n", reason,
    68            reason <= SIGDEBUG_WATCHDOG ? reason_str[reason] : "<unknown>");
    69 #endif
    70         // Dump a backtrace of the frame which caused the switch to secondary mode:
    71         nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
    72         backtrace_symbols_fd(bt,nentries,fileno(stdout));
    73     }
    74 #else //SIGDEBUG
    75     void warn_upon_switch(int sig __attribute__((unused))) {
    76         void *bt[32];
    77         int nentries;
    78 
    79         /* Dump a backtrace of the frame which caused the switch to
    80            secondary mode: */
    81         nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
    82         backtrace_symbols_fd(bt,nentries,fileno(stdout));
    83     }
    84 #endif //SIGDEBUG
     62  printf("\nSIGDEBUG received, reason %d: %s\n", reason,
     63         reason <= SIGDEBUG_WATCHDOG ? reason_str[reason] : "<unknown>");
     64#endif
     65  // Dump a backtrace of the frame which caused the switch to secondary mode:
     66  nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
     67  backtrace_symbols_fd(bt, nentries, fileno(stdout));
     68}
     69#else  // SIGDEBUG
     70void warn_upon_switch(int sig __attribute__((unused))) {
     71  void *bt[32];
     72  int nentries;
     73
     74  /* Dump a backtrace of the frame which caused the switch to
     75     secondary mode: */
     76  nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
     77  backtrace_symbols_fd(bt, nentries, fileno(stdout));
     78}
     79#endif // SIGDEBUG
    8580#endif //__XENO__
    86     void seg_fault(int sig __attribute__((unused))) {
    87         void *bt[32];
    88         int nentries;
    89 
    90         printf("Segmentation fault:\n");
    91         /* Dump a backtrace of the frame which caused the segfault: */
    92         nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
    93         backtrace_symbols_fd(bt,nentries,fileno(stdout));
    94 
    95         exit(1);
    96     }
    97 }
    98 
    99 FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self,string name): Thread(self,"FrameworkManager",FRAMEWORK_TASK_PRIORITY) {
    100     this->self=self;
    101     is_logging=false;
    102     logger_defined=false;
    103     disable_errors=false;
    104     ui_defined=false;
    105     rcv_buf=NULL;
    106     _this=this;
    107 
    108     // Avoids memory swapping for this program
    109     mlockall(MCL_CURRENT|MCL_FUTURE);
    110     struct sigaction sa;
    111 
    112     //catch segfault
    113     signal(SIGSEGV, seg_fault);
    114 
    115     //catch primary->secondary switch
     81void seg_fault(int sig __attribute__((unused))) {
     82  void *bt[32];
     83  int nentries;
     84
     85  printf("Segmentation fault:\n");
     86  /* Dump a backtrace of the frame which caused the segfault: */
     87  nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0]));
     88  backtrace_symbols_fd(bt, nentries, fileno(stdout));
     89
     90  exit(1);
     91}
     92}
     93
     94FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self,
     95                                             string name)
     96    : Thread(self, "FrameworkManager", FRAMEWORK_TASK_PRIORITY) {
     97  this->self = self;
     98  is_logging = false;
     99  logger_defined = false;
     100  disable_errors = false;
     101  ui_defined = false;
     102  rcv_buf = NULL;
     103  _this = this;
     104
     105  // Avoids memory swapping for this program
     106  mlockall(MCL_CURRENT | MCL_FUTURE);
     107  struct sigaction sa;
     108
     109  // catch segfault
     110  signal(SIGSEGV, seg_fault);
     111
     112// catch primary->secondary switch
    116113#ifdef __XENO__
    117114#ifdef SIGDEBUG
    118     sigemptyset(&sa.sa_mask);
    119     sa.sa_sigaction = warn_upon_switch;
    120     sa.sa_flags = SA_SIGINFO;
    121     sigaction(SIGDEBUG, &sa, NULL);
    122 #else
    123     signal(SIGXCPU, warn_upon_switch);
    124 #endif
    125 
    126     string task_name="Framework_"+name;
    127     int status=rt_task_shadow(NULL,task_name.c_str(),10,0);
    128     if(status!=0) {
    129         self->Err("rt_task_shadow error (%s)\n",strerror(-status));
    130     }
     115  sigemptyset(&sa.sa_mask);
     116  sa.sa_sigaction = warn_upon_switch;
     117  sa.sa_flags = SA_SIGINFO;
     118  sigaction(SIGDEBUG, &sa, NULL);
     119#else
     120  signal(SIGXCPU, warn_upon_switch);
     121#endif
     122
     123  string task_name = "Framework_" + name;
     124  int status = rt_task_shadow(NULL, task_name.c_str(), 10, 0);
     125  if (status != 0) {
     126    self->Err("rt_task_shadow error (%s)\n", strerror(-status));
     127  }
    131128
    132129#endif //__XENO__
    … …  
    134131
    135132void FrameworkManager_impl::ConnectionLost(void) {
    136     Err("connection lost\n");
    137     gcs_watchdog->SafeStop();
    138     connection_lost=true;
     133  Err("connection lost\n");
     134  gcs_watchdog->SafeStop();
     135  connection_lost = true;
    139136}
    140137
    141138FrameworkManager_impl::~FrameworkManager_impl() {
    142     //Printf("destruction FrameworkManager_impl\n");
    143     int status;
    144 
    145     SafeStop();
    146     Join();
    147 
    148     if(rcv_buf!=NULL) free(rcv_buf);
    149 
    150     if(logger_defined==true) {
    151         continuer=false;
    152         (void)pthread_join(log_th,NULL);
    153 
    154         status=DeletePipe(&cmd_pipe);
    155         if(status!=0) {
    156             Err("Error deleting pipe (%s)\n",strerror(-status));
     139  // Printf("destruction FrameworkManager_impl\n");
     140  int status;
     141
     142  SafeStop();
     143  Join();
     144
     145  if (rcv_buf != NULL)
     146    free(rcv_buf);
     147
     148  if (logger_defined == true) {
     149    continuer = false;
     150    (void)pthread_join(log_th, NULL);
     151
     152    status = DeletePipe(&cmd_pipe);
     153    if (status != 0) {
     154      Err("Error deleting pipe (%s)\n", strerror(-status));
     155    }
     156    status = DeletePipe(&data_pipe);
     157    if (status != 0) {
     158      Err("Error deleting pipe (%s)\n", strerror(-status));
     159    }
     160
     161#ifdef __XENO__
     162    status = rt_heap_delete(&log_heap);
     163    if (status != 0) {
     164      Err("rt_heap_delete error (%s)\n", strerror(-status));
     165    }
     166#endif
     167
     168    logs.clear();
     169  }
     170
     171  if (file_doc != NULL)
     172    xmlFreeDoc(file_doc);
     173
     174  if (ui_defined)
     175    delete top_layout;
     176
     177  if (com != NULL) {
     178    delete com;
     179    status = UDT::close(com_sock);
     180    if (status != 0)
     181      printf("Error udt::close %s", UDT::getlasterror().getErrorMessage());
     182
     183    status = UDT::close(file_sock);
     184    if (status != 0)
     185      printf("Error udt::close %s", UDT::getlasterror().getErrorMessage());
     186
     187    SleepMS(200); // a revoir, sinon UDT::cleanup bloque en RT
     188    UDT::cleanup();
     189  }
     190
     191  // Printf("destruction FrameworkManager_impl ok\n");
     192}
     193
     194void FrameworkManager_impl::SetupConnection(string address, uint16_t port,
     195                                            size_t rcv_buf_size) {
     196  UDT::startup();
     197  this->rcv_buf_size = rcv_buf_size;
     198
     199  // socket file_socket, doit être créé en premier, cf station sol
     200  Printf("Connecting to %s:%i\n", address.c_str(), port);
     201  file_sock = GetSocket(address, port);
     202  com_sock = GetSocket(address, port);
     203
     204  // receive buffer allocation
     205  rcv_buf = (char *)malloc(rcv_buf_size);
     206  if (rcv_buf == NULL) {
     207    Err("receive buffer malloc error\n");
     208  }
     209
     210  com = new ui_com(this, com_sock);
     211
     212  // file managment
     213  bool blocking = true;
     214  UDT::setsockopt(file_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool));
     215  UDT::setsockopt(file_sock, 0, UDT_RCVSYN, &blocking, sizeof(bool));
     216
     217  // configure timeout of blocking receive, short timeout to have priority on
     218  // the other socket (see run method)
     219  int timeout = 1; // ms
     220  UDT::setsockopt(file_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
     221
     222  timeout = 100; // ms
     223  UDT::setsockopt(com_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
     224
     225  Start();
     226
     227  // watchdog for connection with ground station
     228  connection_lost = false;
     229  gcs_watchdog = new Watchdog(
     230      this, std::bind(&FrameworkManager_impl::ConnectionLost, this),
     231      (Time)1000000000);
     232  gcs_watchdog->Start();
     233}
     234
     235void FrameworkManager_impl::SetupUserInterface(string xml_file) {
     236  ui_defined = true;
     237  this->xml_file = xml_file;
     238
     239  // top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE);
     240  top_layout = new Layout(NULL, self->ObjectName(), XML_ROOT_TYPE);
     241
     242  // xml setup of the main widget
     243  if (xml_file != "") {
     244    xmlNodePtr *file_node = &(((Widget *)(top_layout))->pimpl_->file_node);
     245    file_doc = xmlParseFile(xml_file.c_str());
     246    if (file_doc == NULL) {
     247      self->Warn("XML document not parsed successfully. Creating a new one.\n");
     248      file_doc = xmlNewDoc((xmlChar *)"1.0");
     249      *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
     250      xmlSetProp(*file_node, (xmlChar *)"name",
     251                 (xmlChar *)ObjectName().c_str());
     252      xmlDocSetRootElement(file_doc, *file_node);
     253      // PrintXml();
     254    } else {
     255      *file_node = xmlDocGetRootElement(file_doc);
     256      if (xmlStrcmp((*file_node)->name, (xmlChar *)XML_ROOT_TYPE)) {
     257        self->Warn("%s, no match found in xml file\n", XML_ROOT_TYPE);
     258        *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
     259        xmlSetProp(*file_node, (xmlChar *)"name",
     260                   (xmlChar *)ObjectName().c_str());
     261        xmlDocSetRootElement(file_doc, *file_node);
     262      }
     263    }
     264  } else {
     265    self->Err("xml file not defined\n");
     266  }
     267
     268  // gui
     269  tabwidget =
     270      new TabWidget(top_layout->At(0, 0), XML_MAIN_TABWIDGET, TabWidget::North);
     271  save_button =
     272      new PushButton(top_layout->At(1, 0),
     273                     "save config on target (" + self->ObjectName() + ")");
     274  // load_button=new PushButton(top_layout->At(1,1),"load config on target (" +
     275  // self->ObjectName() + ")");
     276}
     277
     278// in case of RT, this thread switches to secondary mode when calling
     279// com->Receive
     280// it switches back to RT in ProcessXML when mutex are locked
     281void FrameworkManager_impl::Run(void) {
     282  while (!ToBeStopped()) {
     283    ssize_t bytesRead;
     284
     285    bytesRead = com->Receive(rcv_buf, rcv_buf_size);
     286
     287    if (bytesRead == (ssize_t)rcv_buf_size)
     288      Err("FrameworkManager max receive size, augmenter le buffer size!\n");
     289
     290    if (bytesRead > 0) {
     291      // printf("recu %ld, trame %x\n",bytesRead,(uint8_t)rcv_buf[0]);
     292      // rcv_buf[bytesRead-1]=0;//pour affichage
     293      // printf("%s\n",rcv_buf);
     294
     295      switch ((uint8_t)rcv_buf[0]) {
     296      case XML_HEADER: {
     297        xmlDoc *doc;
     298        rcv_buf[bytesRead] = 0;
     299
     300        doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml",
     301                            "ISO-8859-1", 0);
     302        xmlNode *cur_node = NULL;
     303
     304        for (cur_node = xmlDocGetRootElement(doc); cur_node;
     305             cur_node = cur_node->next) {
     306          if (cur_node->type == XML_ELEMENT_NODE) {
     307            if (!xmlStrcmp(cur_node->name, (xmlChar *)XML_ROOT_TYPE)) {
     308#ifdef __XENO__
     309              WarnUponSwitches(
     310                  true); // ProcessXML should not switch to secondary mode
     311#endif
     312              top_layout->Widget::pimpl_->ProcessXML(cur_node->children);
     313#ifdef __XENO__
     314              WarnUponSwitches(false); // other parts of this thread can switch
     315                                       // to secondary mode
     316#endif
     317              break;
     318            }
     319          }
    157320        }
    158         status=DeletePipe(&data_pipe);
    159         if(status!=0) {
    160             Err("Error deleting pipe (%s)\n",strerror(-status));
     321
     322        xmlFreeDoc(doc);
     323
     324        if (save_button->Clicked())
     325          SaveXml();
     326
     327        SaveXmlChange(rcv_buf);
     328
     329        break;
     330      }
     331      case WATCHDOG_HEADER: {
     332        gcs_watchdog->Touch();
     333        break;
     334      }
     335      default:
     336        Err("unknown id: %x\n", (uint8_t)rcv_buf[0]);
     337        break;
     338      }
     339    } else {
     340      if (com->ConnectionLost())
     341        SleepMS(10); // avoid infinite loop in this case
     342    }
     343  }
     344}
     345
     346void FrameworkManager_impl::SaveXml(void) {
     347  if (ui_defined)
     348    xmlSaveFormatFile(xml_file.c_str(), file_doc, 1);
     349}
     350
     351void FrameworkManager_impl::SaveXmlChange(char *buf) {
     352  if (is_logging == true) {
     353    FILE *xml_change;
     354    char filename[256];
     355    Time time = GetTime();
     356
     357    sprintf(filename, "%s/changes_at_%lld.xml", log_path.c_str(), time);
     358
     359    xml_change = fopen(filename, "a");
     360    fprintf(xml_change, "%s", buf);
     361    fclose(xml_change);
     362
     363    sprintf(filename, "changes_at_%lld.xml", time);
     364    xml_changes.push_back(filename);
     365  }
     366}
     367
     368void FrameworkManager_impl::SendFile(string path, string name) {
     369  char *buf, *more_buf;
     370  int size;
     371  filebuf *pbuf;
     372  ssize_t nb_write;
     373  string filename = path + "/" + name;
     374
     375  // open the file
     376  fstream ifs(filename.c_str(), ios::in | ios::binary);
     377  ifs.seekg(0, ios::end);
     378  size = ifs.tellg();
     379  ifs.seekg(0, ios::beg);
     380  pbuf = ifs.rdbuf();
     381
     382  if (size <= 0) {
     383    Err("error opening file %s\n", filename.c_str());
     384    return;
     385  }
     386
     387  buf = (char *)malloc(sizeof(uint8_t) + sizeof(int) + name.size());
     388  if (buf == NULL) {
     389    Err("malloc error, not sending file\n");
     390    return;
     391  }
     392
     393  if (IsBigEndian()) {
     394    buf[0] = FILE_INFO_BIG_ENDIAN;
     395  } else {
     396    buf[0] = FILE_INFO_LITTLE_ENDIAN;
     397  }
     398
     399  memcpy(buf + 1, &size, sizeof(int));
     400  memcpy(buf + 1 + sizeof(int), name.c_str(), name.size());
     401  Printf("sending %s, size: %i\n", filename.c_str(), size);
     402  // send file information
     403  UDT::sendmsg(file_sock, buf, sizeof(uint8_t) + sizeof(int) + name.size(), -1,
     404               true);
     405
     406  more_buf = (char *)realloc((void *)buf, size);
     407  if (more_buf == NULL) {
     408    Err("realloc error, not sending file\n");
     409    free(buf);
     410    return;
     411  } else {
     412    buf = more_buf;
     413  }
     414
     415  pbuf->sgetn(buf, size);
     416  // send the file
     417  nb_write = UDT::sendmsg(file_sock, buf, size, -1, true);
     418
     419  if (nb_write < 0) {
     420    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
     421  } else if (nb_write != size) {
     422    Err("UDT::sendmsg error, sent %ld/%i\n", nb_write, size);
     423  }
     424
     425  ifs.close();
     426  free(buf);
     427}
     428
     429void FrameworkManager_impl::FinishSending() {
     430  char rm_cmd[256];
     431
     432  // send orignal xml
     433  SendFile(log_path, "setup.xml");
     434  sprintf(rm_cmd, "rm %s/setup.xml", log_path.c_str());
     435  system(rm_cmd);
     436
     437  // send xml changes
     438  for (size_t i = 0; i < xml_changes.size(); i++) {
     439    // Printf("%s\n",xml_changes.at(i).c_str());
     440    SendFile(log_path, xml_changes.at(i).c_str());
     441    sprintf(rm_cmd, "rm %s/%s", log_path.c_str(), xml_changes.at(i).c_str());
     442    system(rm_cmd);
     443  }
     444  xml_changes.clear();
     445
     446  // end notify
     447  char buf = END;
     448  int nb_write = UDT::sendmsg(file_sock, &buf, 1, -1, true);
     449
     450  if (nb_write < 0) {
     451    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
     452  } else if (nb_write != 1) {
     453    Err("UDT::sendmsg error, sent %i/%i\n", nb_write, 1);
     454  }
     455}
     456
     457UDTSOCKET FrameworkManager_impl::GetSocket(string address, uint16_t port) {
     458  while (1) {
     459    UDTSOCKET new_fd;
     460    new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0);
     461    if (new_fd == UDT::INVALID_SOCK) {
     462      Err("socket error: %s\n", UDT::getlasterror().getErrorMessage());
     463      return 0;
     464    }
     465
     466    sockaddr_in serv_addr;
     467    serv_addr.sin_family = AF_INET;
     468    serv_addr.sin_port = htons(short(port));
     469
     470    if (inet_pton(AF_INET, address.c_str(), &serv_addr.sin_addr) <= 0) {
     471      Err("incorrect network address\n");
     472      return 0;
     473    }
     474
     475    memset(&(serv_addr.sin_zero), '\0', 8);
     476
     477    if (UDT::ERROR ==
     478        UDT::connect(new_fd, (sockaddr *)&serv_addr, sizeof(serv_addr))) {
     479      // Printf("connect error: %s
     480      // %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
     481      UDT::close(new_fd);
     482      if (UDT::getlasterror().getErrorCode() != 1001 &&
     483          UDT::getlasterror().getErrorCode() != 1002) {
     484        Err("connect error: %s\n", UDT::getlasterror().getErrorMessage());
     485        return 0;
     486      }
     487    } else {
     488      // printf("connected to
     489      // %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port);
     490      return new_fd;
     491    }
     492  }
     493}
     494
     495#ifdef __XENO__
     496int FrameworkManager_impl::CreatePipe(RT_PIPE *fd, string name) {
     497  name = self->ObjectName() + "-" + name;
     498  // xenomai limitation
     499  if (name.size() > 31)
     500    self->Err("rt_pipe_create error (%s is too long)\n", name.c_str());
     501// start log writter
     502#ifdef RT_PIPE_SIZE
     503  return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
     504#else
     505  return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, 0);
     506#endif
     507}
     508#else
     509int FrameworkManager_impl::CreatePipe(int (*fd)[2], string name) {
     510  // if(pipe2(fd[0],O_NONBLOCK) == -1)
     511  if (pipe(fd[0]) == -1) {
     512    return errno;
     513  } else {
     514    int attr = fcntl((*fd)[0], F_GETFL, 0);
     515    if (attr == -1) {
     516      return errno;
     517    }
     518    if (fcntl((*fd)[0], F_SETFL, attr | O_NONBLOCK) == -1) {
     519      return errno;
     520    }
     521    attr = fcntl((*fd)[1], F_GETFL, 0);
     522    if (attr == -1) {
     523      return errno;
     524    }
     525    if (fcntl((*fd)[1], F_SETFL, attr | O_NONBLOCK) == -1) {
     526      return errno;
     527    }
     528
     529    return 0;
     530  }
     531}
     532#endif
     533
     534#ifdef __XENO__
     535int FrameworkManager_impl::DeletePipe(RT_PIPE *fd) {
     536  return rt_pipe_delete(fd);
     537}
     538#else
     539int FrameworkManager_impl::DeletePipe(int (*fd)[2]) {
     540  int status1 = close((*fd)[0]);
     541  int status2 = close((*fd)[1]);
     542  if (status1 == 0 && status2 == 0)
     543    return 0;
     544  if (status1 != 0)
     545    return status1;
     546  if (status2 != 0)
     547    return status2;
     548}
     549#endif
     550
     551void FrameworkManager_impl::SetupLogger(string log_path) {
     552  if (logger_defined == true) {
     553    Warn("SetupLogger() was already called.\n");
     554    return;
     555  }
     556
     557  this->log_path = log_path;
     558
     559  int status = CreatePipe(&cmd_pipe, "log_cmd");
     560  if (status != 0) {
     561    Err("Error creating pipe (%s)\n", strerror(-status));
     562    return;
     563  }
     564
     565  status = CreatePipe(&data_pipe, "log_data");
     566  if (status != 0) {
     567    Err("Error creating pipe (%s)\n", strerror(-status));
     568    return;
     569  }
     570
     571#ifdef __XENO__
     572  string tmp_name;
     573  tmp_name = self->ObjectName() + "-log_heap";
     574  status = rt_heap_create(&log_heap, tmp_name.c_str(), LOG_HEAP, H_FIFO);
     575  if (status != 0) {
     576    Err("rt_heap_create error (%s)\n", strerror(-status));
     577    return;
     578  }
     579#endif //__XENO__
     580
     581  continuer = true;
     582
     583#ifdef NRT_STACK_SIZE
     584  // Initialize thread creation attributes
     585  pthread_attr_t attr;
     586  if (pthread_attr_init(&attr) != 0) {
     587    Err("pthread_attr_init error\n");
     588    return;
     589  }
     590
     591  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
     592    Err("pthread_attr_setstacksize error\n");
     593    return;
     594  }
     595
     596  if (pthread_create(&log_th, &attr, write_log_user, (void *)this) < 0)
     597#else
     598  if (pthread_create(&log_th, NULL, write_log_user, (void *)this) < 0)
     599#endif
     600  {
     601    Err("pthread_create error\n");
     602    return;
     603  }
     604#ifdef NRT_STACK_SIZE
     605  if (pthread_attr_destroy(&attr) != 0) {
     606    Err("pthread_attr_destroy error\n");
     607    return;
     608  }
     609#endif
     610
     611  logger_defined = true;
     612}
     613
     614void FrameworkManager_impl::AddDeviceToLog(IODevice *device) {
     615  if (logger_defined == false) {
     616    Err("SetupLogger() was not called, not starting log\n");
     617    return;
     618  }
     619
     620  if (is_logging == false) {
     621    if (device->pimpl_->SetToBeLogged()) {
     622      log_desc_t tmp;
     623      tmp.device = device;
     624      logs.push_back(tmp);
     625    } else {
     626      Warn("not adding it twice\n");
     627    }
     628  } else {
     629    Err("impossible while logging\n");
     630  }
     631}
     632
     633void FrameworkManager_impl::StartLog(void) {
     634  if (logger_defined == false) {
     635    Err("SetupLogger() was not called, not starting log\n");
     636    return;
     637  }
     638
     639  ssize_t written;
     640  size_t nb_err = 0;
     641
     642  if (logs.size() == 0) {
     643    Warn("Not starting log: nothing to log!\n");
     644    return;
     645  }
     646
     647  if (is_logging == false) {
     648    for (size_t i = 0; i < logs.size(); i++) {
     649
     650      logs.at(i).running = true;
     651      logs.at(i).dbtFile = NULL;
     652      logs.at(i).size = logs.at(i).device->pimpl_->LogSize();
     653#ifdef __XENO__
     654      written =
     655          rt_pipe_write(&cmd_pipe, &logs.at(i), sizeof(log_desc_t), P_NORMAL);
     656#else
     657      written = write(cmd_pipe[1], &logs.at(i), sizeof(log_desc_t));
     658#endif
     659
     660      if (written < 0) {
     661        Err("write pipe error (%s)\n", strerror(-written));
     662        nb_err++;
     663        logs.at(i).running = false;
     664      } else if (written != sizeof(log_desc_t)) {
     665        Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t));
     666        nb_err++;
     667        logs.at(i).running = false;
     668      }
     669    }
     670
     671    if (nb_err != logs.size())
     672      is_logging = true;
     673  } else {
     674    Warn("Already logging\n");
     675  }
     676}
     677
     678void FrameworkManager_impl::StopLog(void) {
     679  ssize_t written;
     680
     681  if (is_logging == true) {
     682    for (size_t i = 0; i < logs.size(); i++) {
     683      logs.at(i).running = false;
     684    }
     685// send only one running false condition, user thread will stop and send all
     686#ifdef __XENO__
     687    written =
     688        rt_pipe_write(&cmd_pipe, &logs.at(0), sizeof(log_desc_t), P_NORMAL);
     689#else
     690    written = write(cmd_pipe[1], &logs.at(0), sizeof(log_desc_t));
     691#endif
     692
     693    if (written < 0) {
     694      Err("write pipe error (%s)\n", strerror(-written));
     695      return;
     696    } else if (written != sizeof(log_desc_t)) {
     697      Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t));
     698      return;
     699    }
     700
     701    is_logging = false;
     702  } else {
     703    Warn("Not logging\n");
     704  }
     705}
     706
     707char *FrameworkManager_impl::GetBuffer(size_t sz) {
     708// Printf("alloc %i\n",sz);
     709#ifdef __XENO__
     710  void *ptr;
     711  int status = rt_heap_alloc(&log_heap, sz, TM_NONBLOCK, &ptr);
     712  if (status != 0) {
     713    Err("rt_heap_alloc error (%s)\n", strerror(-status));
     714    ptr = NULL;
     715  }
     716  return (char *)ptr;
     717#else
     718  return (char *)malloc(sz);
     719#endif
     720}
     721
     722void FrameworkManager_impl::ReleaseBuffer(char *buf) {
     723#ifdef __XENO__
     724  int status = rt_heap_free(&log_heap, buf);
     725
     726  if (status != 0) {
     727    Err("rt_heap_free error (%s)\n", strerror(-status));
     728  }
     729#else
     730  free(buf);
     731#endif
     732}
     733
     734void FrameworkManager_impl::WriteLog(const char *buf, size_t size) {
     735  ssize_t written;
     736
     737#ifdef __XENO__
     738  written = rt_pipe_write(&data_pipe, buf, size, P_NORMAL);
     739#else
     740  written = write(data_pipe[1], buf, size);
     741#endif
     742
     743  if (written < 0) {
     744    Err("erreur write pipe (%s)\n", strerror(-written));
     745  } else if (written != (ssize_t)size) {
     746    Err("erreur write pipe %ld/%ld\n", written, size);
     747  }
     748}
     749
     750void *FrameworkManager_impl::write_log_user(void *arg) {
     751  int cmd_pipe = -1;
     752  int data_pipe = -1;
     753  string filename;
     754  fd_set set;
     755  struct timeval timeout;
     756  FrameworkManager_impl *caller = (FrameworkManager_impl *)arg;
     757  int rv;
     758
     759  vector<log_desc_t> logs;
     760
     761#ifdef __XENO__
     762  filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_cmd";
     763  while (cmd_pipe < 0) {
     764    cmd_pipe = open(filename.c_str(), O_RDWR);
     765    if (cmd_pipe < 0 && errno != ENOENT)
     766      caller->self->Err("open rt_pipe error: %s %i\n", filename.c_str(), errno);
     767    usleep(1000);
     768  }
     769  filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_data";
     770  while (data_pipe < 0) {
     771    data_pipe = open(filename.c_str(), O_RDWR);
     772    if (data_pipe < 0 && errno != ENOENT)
     773      caller->self->Err("open rt_pipe error: %s %i\n", filename.c_str(), errno);
     774    usleep(1000);
     775  }
     776#else
     777  cmd_pipe = caller->cmd_pipe[0];
     778  data_pipe = caller->data_pipe[0];
     779#endif
     780
     781  while (caller->continuer == true) {
     782    FD_ZERO(&set);
     783    FD_SET(cmd_pipe, &set);
     784    FD_SET(data_pipe, &set);
     785
     786    timeout.tv_sec = 0;
     787    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
     788    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
     789
     790    if (rv == -1) {
     791      caller->Err("select error\n"); // an error accured
     792    } else if (rv == 0) {
     793      // printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); //
     794      // a timeout occured
     795    } else {
     796      if (FD_ISSET(cmd_pipe, &set)) {
     797        log_desc_t tmp;
     798        read(cmd_pipe, &tmp, sizeof(log_desc_t));
     799
     800        if (tmp.running == true) // start logging
     801        {
     802          filename =
     803              caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt";
     804          printf("Creating log file %s (log size %i)\n", filename.c_str(),
     805                 (int)tmp.size);
     806          tmp.dbtFile = inithdFile((char *)filename.c_str(), UAV, tmp.size);
     807          logs.push_back(tmp);
     808
     809          if (logs.size() == 1) {
     810            filename = caller->log_path + "/setup.xml";
     811            xmlSaveFile(filename.c_str(), caller->file_doc);
     812          }
     813        } else // stop logging
     814        {
     815          for (size_t i = 0; i < logs.size(); i++) {
     816            if (logs.at(i).dbtFile != NULL) {
     817              close_hdfile(logs.at(i).dbtFile);
     818
     819              filename = caller->FileName(logs.at(i).device) + ".dbt";
     820              caller->SendFile(caller->log_path, filename);
     821
     822              fstream txt_file;
     823              filename = caller->FileName(logs.at(i).device) + ".txt";
     824              txt_file.open((caller->log_path + "/" + filename).c_str(),
     825                            fstream::out);
     826              txt_file << "1: time (us)\n2: time (ns)\n";
     827              int index = 3;
     828              logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file, &index);
     829              txt_file.close();
     830
     831              caller->SendFile(caller->log_path, filename);
     832            }
     833          }
     834          // a revoir celui ci est le xml enregistré et pas forcement l'actuel
     835          // if(caller->xml_file!="") caller->SendFile(caller->xml_file);
     836          caller->FinishSending();
     837
     838          logs.clear();
    161839        }
    162 
    163 #ifdef __XENO__
    164         status=rt_heap_delete(&log_heap);
    165         if(status!=0) {
    166             Err("rt_heap_delete error (%s)\n",strerror(-status));
     840      }
     841
     842      if (FD_ISSET(data_pipe, &set)) {
     843        log_header_t header;
     844        read(data_pipe, &header, sizeof(log_header_t));
     845
     846        for (size_t i = 0; i < logs.size(); i++) {
     847          if (logs.at(i).device == header.device) {
     848            char *buf = (char *)malloc(header.size);
     849            read(data_pipe, buf, header.size);
     850            // printf("%s \n",header.device->ObjectName().c_str());
     851            // for(int i=0;i<header.size;i++) printf("%x ",buf[i]);
     852            // printf("\n");
     853            if (logs.at(i).size == header.size) {
     854              if (logs.at(i).dbtFile != NULL) {
     855                write_hdfile(
     856                    logs.at(i).dbtFile, buf, (road_time_t)(header.time / 1000),
     857                    (road_timerange_t)(header.time % 1000), header.size);
     858              } else {
     859                printf("err\n");
     860              }
     861            } else {
     862              caller->self->Err("%s log size is not correct %i/%i\n",
     863                                header.device->ObjectName().c_str(),
     864                                header.size, logs.at(i).size);
     865            }
     866            free(buf);
     867          }
    167868        }
    168 #endif
    169 
    170         logs.clear();
    171     }
    172 
    173     if(file_doc!=NULL) xmlFreeDoc(file_doc);
    174 
    175     if(ui_defined) delete top_layout;
    176 
    177     if(com!=NULL) {
    178         delete com;
    179         status=UDT::close(com_sock);
    180         if(status!=0) printf("Error udt::close %s",UDT::getlasterror().getErrorMessage());
    181 
    182         status=UDT::close(file_sock);
    183         if(status!=0) printf("Error udt::close %s",UDT::getlasterror().getErrorMessage());
    184 
    185         SleepMS(200);//a revoir, sinon UDT::cleanup bloque en RT
    186         UDT::cleanup();
    187     }
    188 
    189     //Printf("destruction FrameworkManager_impl ok\n");
    190 }
    191 
    192 
    193 void FrameworkManager_impl::SetupConnection(string address,uint16_t port,size_t rcv_buf_size) {
    194     UDT::startup();
    195     this->rcv_buf_size=rcv_buf_size;
    196 
    197     //socket file_socket, doit être créé en premier, cf station sol
    198     Printf("Connecting to %s:%i\n",address.c_str(),port);
    199     file_sock=GetSocket(address,port);
    200     com_sock=GetSocket(address,port);
    201 
    202     //receive buffer allocation
    203     rcv_buf=(char*)malloc(rcv_buf_size);
    204     if(rcv_buf==NULL) {
    205         Err("receive buffer malloc error\n");
    206     }
    207 
    208     com=new ui_com(this,com_sock);
    209 
    210     //file managment
    211     bool blocking = true;
    212     UDT::setsockopt(file_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool));
    213     UDT::setsockopt(file_sock, 0, UDT_RCVSYN, &blocking, sizeof(bool));
    214 
    215     //configure timeout of blocking receive, short timeout to have priority on the other socket (see run method)
    216     int timeout=1;//ms
    217     UDT::setsockopt(file_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
    218 
    219     timeout=100;//ms
    220     UDT::setsockopt(com_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int));
    221 
    222     Start();
    223 
    224     //watchdog for connection with ground station
    225     connection_lost=false;
    226     gcs_watchdog=new Watchdog(this,std::bind(&FrameworkManager_impl::ConnectionLost,this),(Time)1000000000);
    227     gcs_watchdog->Start();
    228 }
    229 
    230 void FrameworkManager_impl::SetupUserInterface(string xml_file) {
    231     ui_defined=true;
    232     this->xml_file=xml_file;
    233 
    234     //top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE);
    235     top_layout=new Layout(NULL,self->ObjectName(),XML_ROOT_TYPE);
    236 
    237     //xml setup of the main widget
    238     if(xml_file!="") {
    239         xmlNodePtr* file_node=&(((Widget*)(top_layout))->pimpl_->file_node);
    240         file_doc = xmlParseFile(xml_file.c_str());
    241         if (file_doc == NULL ) {
    242             self->Warn("XML document not parsed successfully. Creating a new one.\n");
    243             file_doc = xmlNewDoc((xmlChar*)"1.0");
    244             *file_node=xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE);
    245             xmlSetProp(*file_node,(xmlChar*)"name",(xmlChar*)ObjectName().c_str());
    246             xmlDocSetRootElement(file_doc, *file_node);
    247             //PrintXml();
    248         } else {
    249             *file_node=xmlDocGetRootElement(file_doc);
    250             if(xmlStrcmp((*file_node)->name,(xmlChar*)XML_ROOT_TYPE)) {
    251                 self->Warn("%s, no match found in xml file\n",XML_ROOT_TYPE);
    252                 *file_node=xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE);
    253                 xmlSetProp(*file_node,(xmlChar*)"name",(xmlChar*)ObjectName().c_str());
    254                 xmlDocSetRootElement(file_doc, *file_node);
    255             }
    256         }
    257     } else {
    258         self->Err("xml file not defined\n");
    259     }
    260 
    261     //gui
    262     tabwidget=new TabWidget(top_layout->At(0,0),XML_MAIN_TABWIDGET,TabWidget::North);
    263     save_button=new PushButton(top_layout->At(1,0),"save config on target (" + self->ObjectName() + ")");
    264     //load_button=new PushButton(top_layout->At(1,1),"load config on target (" + self->ObjectName() + ")");
    265 }
    266 
    267 //in case of RT, this thread switches to secondary mode when calling com->Receive
    268 //it switches back to RT in ProcessXML when mutex are locked
    269 void FrameworkManager_impl::Run(void)
    270 {
    271     while(!ToBeStopped())
    272     {
    273         ssize_t bytesRead;
    274 
    275         bytesRead=com->Receive(rcv_buf,rcv_buf_size);
    276 
    277         if(bytesRead==(ssize_t)rcv_buf_size) Err("FrameworkManager max receive size, augmenter le buffer size!\n");
    278 
    279         if(bytesRead>0)
    280         {
    281             //printf("recu %ld, trame %x\n",bytesRead,(uint8_t)rcv_buf[0]);
    282             //rcv_buf[bytesRead-1]=0;//pour affichage
    283             //printf("%s\n",rcv_buf);
    284 
    285             switch((uint8_t)rcv_buf[0])
    286             {
    287                 case XML_HEADER:
    288                 {
    289                     xmlDoc *doc;
    290                     rcv_buf[bytesRead]=0;
    291 
    292                     doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml", "ISO-8859-1", 0);
    293                     xmlNode *cur_node = NULL;
    294 
    295                     for (cur_node = xmlDocGetRootElement(doc); cur_node; cur_node = cur_node->next)
    296                     {
    297                         if (cur_node->type == XML_ELEMENT_NODE)
    298                         {
    299                             if(!xmlStrcmp(cur_node->name,(xmlChar*)XML_ROOT_TYPE) )
    300                             {
    301 #ifdef __XENO__
    302                                 WarnUponSwitches(true);//ProcessXML should not switch to secondary mode
    303 #endif
    304                                 top_layout->Widget::pimpl_->ProcessXML(cur_node->children);
    305 #ifdef __XENO__
    306                                 WarnUponSwitches(false);//other parts of this thread can switch to secondary mode
    307 #endif
    308                                 break;
    309                             }
    310                         }
    311                     }
    312 
    313                     xmlFreeDoc(doc);
    314 
    315                     if(save_button->Clicked()) SaveXml();
    316 
    317                     SaveXmlChange(rcv_buf);
    318 
    319                     break;
    320                 }
    321                 case WATCHDOG_HEADER: {
    322                     gcs_watchdog->Touch();
    323                     break;
    324                 }
    325                 default:
    326                     Err("unknown id: %x\n",(uint8_t)rcv_buf[0]);
    327                     break;
    328             }
    329         } else {
    330             if(com->ConnectionLost()) SleepMS(10);//avoid infinite loop in this case
    331         }
    332     }
    333 }
    334 
    335 void FrameworkManager_impl::SaveXml(void)
    336 {
    337     if(ui_defined) xmlSaveFormatFile(xml_file.c_str(),file_doc,1);
    338 }
    339 
    340 void FrameworkManager_impl::SaveXmlChange(char* buf)
    341 {
    342     if(is_logging==true)
    343     {
    344         FILE *xml_change;
    345         char filename[256];
    346         Time time=GetTime();
    347 
    348         sprintf(filename,"%s/changes_at_%lld.xml",log_path.c_str(),time);
    349 
    350         xml_change=fopen(filename,"a");
    351         fprintf(xml_change,"%s",buf);
    352         fclose(xml_change);
    353 
    354         sprintf(filename,"changes_at_%lld.xml",time);
    355         xml_changes.push_back(filename);
    356     }
    357 }
    358 
    359 void FrameworkManager_impl::SendFile(string path,string name)
    360 {
    361     char *buf,*more_buf;
    362     int size;
    363     filebuf *pbuf;
    364     ssize_t nb_write;
    365     string filename=path+ "/" +name;
    366 
    367     // open the file
    368     fstream ifs(filename.c_str(), ios::in | ios::binary);
    369     ifs.seekg(0, ios::end);
    370     size = ifs.tellg();
    371     ifs.seekg(0, ios::beg);
    372     pbuf=ifs.rdbuf();
    373 
    374     if(size<=0)
    375     {
    376         Err("error opening file %s\n",filename.c_str());
    377         return;
    378     }
    379 
    380     buf=(char*)malloc(sizeof(uint8_t)+sizeof(int)+name.size());
    381     if(buf==NULL)
    382     {
    383         Err("malloc error, not sending file\n");
    384         return;
    385     }
    386 
    387     if(IsBigEndian())
    388     {
    389         buf[0]=FILE_INFO_BIG_ENDIAN;
    390     }
    391     else
    392     {
    393         buf[0]=FILE_INFO_LITTLE_ENDIAN;
    394     }
    395 
    396     memcpy(buf+1,&size,sizeof(int));
    397     memcpy(buf+1+sizeof(int),name.c_str(),name.size());
    398     Printf("sending %s, size: %i\n",filename.c_str(),size);
    399     // send file information
    400     UDT::sendmsg(file_sock, buf, sizeof(uint8_t)+sizeof(int)+name.size(), -1,true);
    401 
    402     more_buf=(char*)realloc((void*)buf,size);
    403     if(more_buf==NULL)
    404     {
    405         Err("realloc error, not sending file\n");
    406         free(buf);
    407         return;
    408     }
    409     else
    410     {
    411         buf=more_buf;
    412     }
    413 
    414     pbuf->sgetn (buf,size);
    415     // send the file
    416     nb_write=UDT::sendmsg(file_sock, buf, size,-1,true);
    417 
    418     if(nb_write<0)
    419     {
    420         Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    421     }
    422     else if (nb_write != size)
    423     {
    424         Err("UDT::sendmsg error, sent %ld/%i\n",nb_write,size);
    425     }
    426 
    427     ifs.close();
    428     free(buf);
    429 }
    430 
    431 void FrameworkManager_impl::FinishSending()
    432 {
    433     char rm_cmd[256];
    434 
    435     //send orignal xml
    436     SendFile(log_path,"setup.xml");
    437     sprintf(rm_cmd,"rm %s/setup.xml",log_path.c_str());
    438     system(rm_cmd);
    439 
    440     //send xml changes
    441     for(size_t i=0;i<xml_changes.size();i++)
    442     {
    443         //Printf("%s\n",xml_changes.at(i).c_str());
    444         SendFile(log_path,xml_changes.at(i).c_str());
    445         sprintf(rm_cmd,"rm %s/%s",log_path.c_str(),xml_changes.at(i).c_str());
    446         system(rm_cmd);
    447     }
    448     xml_changes.clear();
    449 
    450     //end notify
    451     char buf=END;
    452     int nb_write=UDT::sendmsg(file_sock, &buf, 1,-1,true);
    453 
    454     if(nb_write<0)
    455     {
    456         Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    457     }
    458     else if (nb_write != 1)
    459     {
    460         Err("UDT::sendmsg error, sent %i/%i\n",nb_write,1);
    461     }
    462 }
    463 
    464 UDTSOCKET FrameworkManager_impl::GetSocket(string address,uint16_t port) {
    465     while(1) {
    466         UDTSOCKET new_fd;
    467         new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0);
    468         if(new_fd==UDT::INVALID_SOCK) {
    469             Err("socket error: %s\n",UDT::getlasterror().getErrorMessage());
    470             return 0;
    471         }
    472 
    473         sockaddr_in serv_addr;
    474         serv_addr.sin_family = AF_INET;
    475         serv_addr.sin_port = htons(short(port));
    476 
    477         if (inet_pton(AF_INET,address.c_str(), &serv_addr.sin_addr) <= 0) {
    478             Err("incorrect network address\n");
    479             return 0;
    480         }
    481 
    482         memset(&(serv_addr.sin_zero), '\0', 8);
    483 
    484         if (UDT::ERROR == UDT::connect(new_fd, (sockaddr*)&serv_addr, sizeof(serv_addr))) {
    485             //Printf("connect error: %s %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
    486             UDT::close(new_fd);
    487             if(UDT::getlasterror().getErrorCode()!=1001 && UDT::getlasterror().getErrorCode()!=1002) {
    488                 Err("connect error: %s\n",UDT::getlasterror().getErrorMessage());
    489                 return 0;
    490             }
    491         } else {
    492             //printf("connected to %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port);
    493             return new_fd;
    494         }
    495     }
    496 }
    497 
    498 #ifdef __XENO__
    499 int FrameworkManager_impl::CreatePipe(RT_PIPE* fd,string name)
    500 {
    501     name=self->ObjectName()+ "-" + name;
    502     //xenomai limitation
    503     if(name.size()>31) self->Err("rt_pipe_create error (%s is too long)\n",name.c_str());
    504     //start log writter
    505 #ifdef RT_PIPE_SIZE
    506     return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO,RT_PIPE_SIZE);
    507 #else
    508     return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO,0);
    509 #endif
    510 }
    511 #else
    512 int FrameworkManager_impl::CreatePipe(int (*fd)[2],string name)
    513 {
    514     //if(pipe2(fd[0],O_NONBLOCK) == -1)
    515     if(pipe(fd[0]) == -1)
    516     {
    517         return errno;
    518     }
    519     else
    520     {
    521         int attr=fcntl((*fd)[0], F_GETFL,0);
    522         if(attr == -1)
    523         {
    524             return errno;
    525         }
    526         if(fcntl((*fd)[0], F_SETFL, attr|O_NONBLOCK)== -1)
    527         {
    528             return errno;
    529         }
    530         attr=fcntl((*fd)[1], F_GETFL,0);
    531         if(attr == -1)
    532         {
    533             return errno;
    534         }
    535         if(fcntl((*fd)[1], F_SETFL, attr|O_NONBLOCK)== -1)
    536         {
    537             return errno;
    538         }
    539 
    540         return 0;
    541     }
    542 }
    543 #endif
    544 
    545 #ifdef __XENO__
    546 int FrameworkManager_impl::DeletePipe(RT_PIPE* fd)
    547 {
    548     return rt_pipe_delete(fd);
    549 }
    550 #else
    551 int FrameworkManager_impl::DeletePipe(int (*fd)[2])
    552 {
    553     int status1=close((*fd)[0]);
    554     int status2=close((*fd)[1]);
    555     if(status1==0 && status2==0) return 0;
    556     if(status1!=0) return status1;
    557     if(status2!=0) return status2;
    558 }
    559 #endif
    560 
    561 void FrameworkManager_impl::SetupLogger(string log_path) {
    562     if(logger_defined==true) {
    563         Warn("SetupLogger() was already called.\n");
    564         return;
    565     }
    566 
    567     this->log_path=log_path;
    568 
    569     int status=CreatePipe(&cmd_pipe,"log_cmd");
    570     if(status!=0) {
    571         Err("Error creating pipe (%s)\n",strerror(-status));
    572         return;
    573     }
    574 
    575     status=CreatePipe(&data_pipe,"log_data");
    576     if(status!=0) {
    577         Err("Error creating pipe (%s)\n",strerror(-status));
    578         return;
    579     }
    580 
    581 #ifdef __XENO__
    582     string tmp_name;
    583     tmp_name=self->ObjectName() + "-log_heap";
    584     status=rt_heap_create(&log_heap,tmp_name.c_str(),LOG_HEAP,H_FIFO);
    585     if(status!=0) {
    586         Err("rt_heap_create error (%s)\n",strerror(-status));
    587         return;
    588     }
    589 #endif //__XENO__
    590 
    591     continuer=true;
    592 
    593 #ifdef NRT_STACK_SIZE
    594     // Initialize thread creation attributes
    595     pthread_attr_t attr;
    596     if(pthread_attr_init(&attr) != 0) {
    597         Err("pthread_attr_init error\n");
    598         return;
    599     }
    600 
    601     if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
    602        Err("pthread_attr_setstacksize error\n");
    603        return;
    604     }
    605 
    606     if(pthread_create(&log_th,  &attr, write_log_user, (void*)this) < 0)
    607 #else
    608     if(pthread_create(&log_th, NULL, write_log_user, (void*)this) < 0)
    609 #endif
    610     {
    611         Err("pthread_create error\n");
    612         return;
    613     }
    614 #ifdef NRT_STACK_SIZE
    615     if(pthread_attr_destroy(&attr) != 0) {
    616         Err("pthread_attr_destroy error\n");
    617         return;
    618     }
    619 #endif
    620 
    621     logger_defined=true;
    622 }
    623 
    624 void FrameworkManager_impl::AddDeviceToLog(IODevice *device)
    625 {
    626     if(logger_defined==false)
    627     {
    628         Err("SetupLogger() was not called, not starting log\n");
    629         return;
    630     }
    631 
    632     if(is_logging==false)
    633     {
    634         if(device->pimpl_->SetToBeLogged()) {
    635             log_desc_t tmp;
    636             tmp.device=device;
    637             logs.push_back(tmp);
    638         } else {
    639             Warn("not adding it twice\n");
    640         }
    641     }
    642     else
    643     {
    644         Err("impossible while logging\n");
    645     }
    646 }
    647 
    648 void FrameworkManager_impl::StartLog(void)
    649 {
    650     if(logger_defined==false)
    651     {
    652         Err("SetupLogger() was not called, not starting log\n");
    653         return;
    654     }
    655 
    656     ssize_t written;
    657     size_t nb_err=0;
    658 
    659     if(logs.size()==0)
    660     {
    661         Warn("Not starting log: nothing to log!\n");
    662         return;
    663     }
    664 
    665     if(is_logging==false)
    666     {
    667         for(size_t i=0;i<logs.size();i++)
    668         {
    669 
    670             logs.at(i).running=true;
    671             logs.at(i).dbtFile=NULL;
    672             logs.at(i).size=logs.at(i).device->pimpl_->LogSize();
    673 #ifdef __XENO__
    674             written=rt_pipe_write(&cmd_pipe,&logs.at(i),sizeof(log_desc_t),P_NORMAL);
    675 #else
    676             written=write(cmd_pipe[1],&logs.at(i),sizeof(log_desc_t));
    677 #endif
    678 
    679             if(written<0)
    680             {
    681                 Err("write pipe error (%s)\n",strerror(-written));
    682                 nb_err++;
    683                 logs.at(i).running=false;
    684             }
    685             else if (written != sizeof(log_desc_t))
    686             {
    687                 Err("write pipe error %ld/%ld\n",written,sizeof(log_desc_t));
    688                 nb_err++;
    689                 logs.at(i).running=false;
    690             }
    691         }
    692 
    693         if(nb_err!=logs.size()) is_logging=true;
    694     }
    695     else
    696     {
    697         Warn("Already logging\n");
    698     }
    699 }
    700 
    701 void FrameworkManager_impl::StopLog(void)
    702 {
    703     ssize_t written;
    704 
    705     if(is_logging==true)
    706     {
    707         for(size_t i=0;i<logs.size();i++)
    708         {
    709             logs.at(i).running=false;
    710         }
    711         //send only one running false condition, user thread will stop and send all
    712 #ifdef __XENO__
    713         written=rt_pipe_write(&cmd_pipe,&logs.at(0),sizeof(log_desc_t),P_NORMAL);
    714 #else
    715         written=write(cmd_pipe[1],&logs.at(0),sizeof(log_desc_t));
    716 #endif
    717 
    718         if(written<0)
    719         {
    720             Err("write pipe error (%s)\n",strerror(-written));
    721             return;
    722         }
    723         else if (written != sizeof(log_desc_t))
    724         {
    725             Err("write pipe error %ld/%ld\n",written,sizeof(log_desc_t));
    726             return;
    727         }
    728 
    729         is_logging=false;
    730     }
    731     else
    732     {
    733         Warn("Not logging\n");
    734     }
    735 }
    736 
    737 char* FrameworkManager_impl::GetBuffer(size_t sz)
    738 {
    739     //Printf("alloc %i\n",sz);
    740 #ifdef __XENO__
    741     void *ptr;
    742     int status=rt_heap_alloc(&log_heap,sz,TM_NONBLOCK ,&ptr);
    743     if(status!=0)
    744     {
    745         Err("rt_heap_alloc error (%s)\n",strerror(-status));
    746         ptr=NULL;
    747     }
    748     return (char*)ptr;
    749 #else
    750     return (char*)malloc(sz);
    751 #endif
    752 }
    753 
    754 void FrameworkManager_impl::ReleaseBuffer(char* buf)
    755 {
    756 #ifdef __XENO__
    757     int status=rt_heap_free(&log_heap,buf);
    758 
    759     if(status!=0)
    760     {
    761         Err("rt_heap_free error (%s)\n",strerror(-status));
    762     }
    763 #else
    764     free(buf);
    765 #endif
    766 }
    767 
    768 void FrameworkManager_impl::WriteLog(const char* buf,size_t size)
    769 {
    770     ssize_t written;
    771 
    772 #ifdef __XENO__
    773     written=rt_pipe_write(&data_pipe,buf,size,P_NORMAL);
    774 #else
    775     written=write(data_pipe[1],buf,size);
    776 #endif
    777 
    778     if(written<0)
    779     {
    780         Err("erreur write pipe (%s)\n",strerror(-written));
    781     }
    782     else if (written != (ssize_t)size)
    783     {
    784         Err("erreur write pipe %ld/%ld\n",written,size);
    785     }
    786 }
    787 
    788 void* FrameworkManager_impl::write_log_user(void * arg)
    789 {
    790     int cmd_pipe=-1;
    791     int data_pipe=-1;
    792     string filename;
    793     fd_set set;
    794     struct timeval timeout;
    795     FrameworkManager_impl *caller = (FrameworkManager_impl*)arg;
    796     int rv;
    797 
    798     vector<log_desc_t> logs;
    799 
    800 #ifdef __XENO__
    801     filename=NRT_PIPE_PATH+ caller->self->ObjectName() + "-log_cmd";
    802     while(cmd_pipe<0)
    803     {
    804         cmd_pipe = open(filename.c_str(), O_RDWR);
    805         if (cmd_pipe < 0 && errno!=ENOENT) caller->self->Err("open rt_pipe error: %s %i\n",filename.c_str(),errno);
    806         usleep(1000);
    807     }
    808     filename=NRT_PIPE_PATH+ caller->self->ObjectName() + "-log_data";
    809     while(data_pipe<0)
    810     {
    811         data_pipe = open(filename.c_str(), O_RDWR);
    812         if (data_pipe < 0 && errno!=ENOENT) caller->self->Err("open rt_pipe error: %s %i\n",filename.c_str(),errno);
    813         usleep(1000);
    814     }
    815 #else
    816     cmd_pipe=caller->cmd_pipe[0];
    817     data_pipe=caller->data_pipe[0];
    818 #endif
    819 
    820     while(caller->continuer==true)
    821     {
    822         FD_ZERO(&set);
    823         FD_SET(cmd_pipe, &set);
    824         FD_SET(data_pipe, &set);
    825 
    826         timeout.tv_sec = 0;
    827         timeout.tv_usec = SELECT_TIMEOUT_MS*1000;
    828         rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
    829 
    830         if(rv == -1)
    831         {
    832             caller->Err("select error\n"); // an error accured
    833         }
    834         else if(rv == 0)
    835         {
    836             //printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); // a timeout occured
    837         }
    838         else
    839         {
    840             if(FD_ISSET(cmd_pipe, &set))
    841             {
    842                 log_desc_t tmp;
    843                 read(cmd_pipe,&tmp,sizeof(log_desc_t));
    844 
    845                 if(tmp.running==true)//start logging
    846                 {
    847                     filename= caller->log_path + "/" +caller->FileName(tmp.device)+ ".dbt";
    848                     printf("Creating log file %s (log size %i)\n",filename.c_str(), (int)tmp.size);
    849                     tmp.dbtFile = inithdFile((char*)filename.c_str(),UAV,tmp.size);
    850                     logs.push_back(tmp);
    851 
    852                     if(logs.size()==1)
    853                     {
    854                         filename= caller->log_path + "/setup.xml";
    855                         xmlSaveFile(filename.c_str(),caller->file_doc);
    856                     }
    857                 }
    858                 else//stop logging
    859                 {
    860                     for(size_t i=0;i<logs.size();i++)
    861                     {
    862                         if(logs.at(i).dbtFile!=NULL)
    863                         {
    864                             close_hdfile(logs.at(i).dbtFile);
    865 
    866                             filename= caller->FileName(logs.at(i).device)+ ".dbt";
    867                             caller->SendFile(caller->log_path,filename);
    868 
    869                             fstream txt_file;
    870                             filename= caller->FileName(logs.at(i).device)+ ".txt";
    871                             txt_file.open((caller->log_path+"/"+filename).c_str(),fstream::out);
    872                             txt_file << "1: time (us)\n2: time (ns)\n";
    873                             int index=3;
    874                             logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file,&index);
    875                             txt_file.close();
    876 
    877                             caller->SendFile(caller->log_path,filename);
    878                         }
    879                     }
    880                     //a revoir celui ci est le xml enregistré et pas forcement l'actuel
    881                     //if(caller->xml_file!="") caller->SendFile(caller->xml_file);
    882                     caller->FinishSending();
    883 
    884                     logs.clear();
    885                 }
    886             }
    887 
    888             if(FD_ISSET(data_pipe, &set))
    889             {
    890                 log_header_t header;
    891                 read(data_pipe,&header,sizeof(log_header_t));
    892 
    893                 for(size_t i=0;i<logs.size();i++)
    894                 {
    895                     if(logs.at(i).device==header.device)
    896                     {
    897                         char* buf=(char*)malloc(header.size);
    898                         read(data_pipe,buf,header.size);
    899 //printf("%s \n",header.device->ObjectName().c_str());
    900 //for(int i=0;i<header.size;i++) printf("%x ",buf[i]);
    901 //printf("\n");
    902                         if(logs.at(i).size==header.size)
    903                         {
    904                             if(logs.at(i).dbtFile!=NULL)
    905                             {
    906                                 write_hdfile(logs.at(i).dbtFile,buf,(road_time_t)(header.time/1000),(road_timerange_t)(header.time%1000),header.size);
    907                             }
    908                             else
    909                             {
    910                                 printf("err\n");
    911                             }
    912                         }
    913                         else
    914                         {
    915                             caller->self->Err("%s log size is not correct %i/%i\n",header.device->ObjectName().c_str(),header.size,logs.at(i).size);
    916                         }
    917                         free(buf);
    918                     }
    919                 }
    920             }
    921         }
    922     }
    923 
    924 #ifdef __XENO__
    925     close(cmd_pipe);
    926     close(data_pipe);
    927 #endif
    928 
    929     pthread_exit(0);
    930 
    931 }
    932 
    933 string FrameworkManager_impl::FileName(IODevice* device)
    934 {
    935     return getFrameworkManager()->ObjectName() + "_"+device->ObjectName();
    936 }
    937 
    938 void FrameworkManager_impl::PrintXml(void) const
    939 {
    940     xmlChar *xmlbuff;
    941     int buffersize;
    942     xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1);
    943     Printf("xml:\n%s\n",xmlbuff);
    944     xmlFree(xmlbuff);
    945 }
     869      }
     870    }
     871  }
     872
     873#ifdef __XENO__
     874  close(cmd_pipe);
     875  close(data_pipe);
     876#endif
     877
     878  pthread_exit(0);
     879}
     880
     881string FrameworkManager_impl::FileName(IODevice *device) {
     882  return getFrameworkManager()->ObjectName() + "_" + device->ObjectName();
     883}
     884
     885void FrameworkManager_impl::PrintXml(void) const {
     886  xmlChar *xmlbuff;
     887  int buffersize;
     888  xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1);
     889  Printf("xml:\n%s\n", xmlbuff);
     890  xmlFree(xmlbuff);
     891}

  • trunk/lib/FlairCore/src/GeoCoordinate.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair
    23 {
    24 namespace core
    25 {
     22namespace flair {
     23namespace core {
    2624
    27 GeoCoordinate::GeoCoordinate(const Object* parent,string name,const GeoCoordinate *point,int n): io_data(parent,name,n)
    28 {
    29     if(n>1) Warn("n>1 not supported\n");
    30     CopyFrom(point);
     25GeoCoordinate::GeoCoordinate(const Object *parent, string name,
     26                             const GeoCoordinate *point, int n)
     27    : io_data(parent, name, n) {
     28  if (n > 1)
     29    Warn("n>1 not supported\n");
     30  CopyFrom(point);
    3131}
    3232
    33 GeoCoordinate::GeoCoordinate(const Object* parent,string name,double latitude,double longitude,double altitude,int n): io_data(parent,name,n)
    34 {
    35     this->latitude=latitude;
    36     this->longitude=longitude;
    37     this->altitude=altitude;
     33GeoCoordinate::GeoCoordinate(const Object *parent, string name, double latitude,
     34                             double longitude, double altitude, int n)
     35    : io_data(parent, name, n) {
     36  this->latitude = latitude;
     37  this->longitude = longitude;
     38  this->altitude = altitude;
    3839}
    3940
    40 GeoCoordinate::~GeoCoordinate()
    41 {
     41GeoCoordinate::~GeoCoordinate() {}
    4242
     43void GeoCoordinate::CopyFrom(const GeoCoordinate *point) {
     44  double latitude, longitude, altitude;
     45  point->GetCoordinates(&latitude, &longitude, &altitude);
     46  GetMutex();
     47  this->latitude = latitude;
     48  this->longitude = longitude;
     49  this->altitude = altitude;
     50  ReleaseMutex();
    4351}
    4452
    45 void GeoCoordinate::CopyFrom(const GeoCoordinate *point)
    46 {
    47     double latitude,longitude,altitude;
    48     point->GetCoordinates(&latitude,&longitude,&altitude);
    49     GetMutex();
    50     this->latitude=latitude;
    51     this->longitude=longitude;
    52     this->altitude=altitude;
    53     ReleaseMutex();
     53void GeoCoordinate::SetCoordinates(double latitude, double longitude,
     54                                   double altitude) {
     55  GetMutex();
     56  this->latitude = latitude;
     57  this->longitude = longitude;
     58  this->altitude = altitude;
     59  ReleaseMutex();
    5460}
    5561
    56 void GeoCoordinate::SetCoordinates(double latitude,double longitude,double altitude)
    57 {
    58     GetMutex();
    59     this->latitude=latitude;
    60     this->longitude=longitude;
    61     this->altitude=altitude;
    62     ReleaseMutex();
     62void GeoCoordinate::GetCoordinates(double *latitude, double *longitude,
     63                                   double *altitude) const {
     64  GetMutex();
     65  *latitude = this->latitude;
     66  *longitude = this->longitude;
     67  *altitude = this->altitude;
     68  ReleaseMutex();
    6369}
    6470
    65 void GeoCoordinate::GetCoordinates(double *latitude,double *longitude,double *altitude) const
    66 {
    67     GetMutex();
    68     *latitude=this->latitude;
    69     *longitude=this->longitude;
    70     *altitude=this->altitude;
    71     ReleaseMutex();
    72 }
    73 
    74 void GeoCoordinate::CopyDatas(char* ptr) const
    75 {
    76     Warn("a ecrire");
    77 }
     71void GeoCoordinate::CopyDatas(char *ptr) const { Warn("a ecrire"); }
    7872
    7973} // end namespace core

  • trunk/lib/FlairCore/src/GeoCoordinate.h

    r2 r15  
    1515#include <io_data.h>
    1616
    17 namespace flair { namespace core {
     17namespace flair {
     18namespace core {
    1819
    19     /*! \class GeoCoordinate
    20     *
    21     * \brief Class defining a point by its lla coordinates
    22     */
    23     class GeoCoordinate: public io_data {
    24         public:
    25             class Type: public DataType {
    26             public:
    27                 size_t GetSize() const {
    28                     return sizeof(latitude)+sizeof(longitude)+sizeof(altitude);
    29                 }
    30                 std::string GetDescription() const {return "lla";}
    31             };
     20/*! \class GeoCoordinate
     21*
     22* \brief Class defining a point by its lla coordinates
     23*/
     24class GeoCoordinate : public io_data {
     25public:
     26  class Type : public DataType {
     27  public:
     28    size_t GetSize() const {
     29      return sizeof(latitude) + sizeof(longitude) + sizeof(altitude);
     30    }
     31    std::string GetDescription() const { return "lla"; }
     32  };
    3233
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct GeoCoordinate using values from another class.
    37             *
    38             * \param parent parent
    39             * \param name name
    40             * \param point class to copy
    41             * \param n number of samples
    42             */
    43             GeoCoordinate(const Object* parent,std::string name,const GeoCoordinate *point,int n=1);
     34  /*!
     35  * \brief Constructor
     36  *
     37  * Construct GeoCoordinate using values from another class.
     38  *
     39  * \param parent parent
     40  * \param name name
     41  * \param point class to copy
     42  * \param n number of samples
     43  */
     44  GeoCoordinate(const Object *parent, std::string name,
     45                const GeoCoordinate *point, int n = 1);
    4446
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct GeoCoordinate using specified values.
    49             *
    50             * \param parent parent
    51             * \param name name
    52             * \param latitude latitude
    53             * \param longitude longitude
    54             * \param altitude altitude
    55             * \param n number of samples
    56             */
    57             GeoCoordinate(const Object* parent,std::string name,double latitude,double longitude,double altitude,int n=1);
     47  /*!
     48  * \brief Constructor
     49  *
     50  * Construct GeoCoordinate using specified values.
     51  *
     52  * \param parent parent
     53  * \param name name
     54  * \param latitude latitude
     55  * \param longitude longitude
     56  * \param altitude altitude
     57  * \param n number of samples
     58  */
     59  GeoCoordinate(const Object *parent, std::string name, double latitude,
     60                double longitude, double altitude, int n = 1);
    5861
    59             /*!
    60             * \brief Destructor
    61             *
    62             */
    63             ~GeoCoordinate();
     62  /*!
     63  * \brief Destructor
     64  *
     65  */
     66  ~GeoCoordinate();
    6467
    65             /*!
    66             * \brief Copy
    67             *
    68             * \param point class to copy
    69             */
    70             void CopyFrom(const GeoCoordinate *point);
     68  /*!
     69  * \brief Copy
     70  *
     71  * \param point class to copy
     72  */
     73  void CopyFrom(const GeoCoordinate *point);
    7174
    72             /*!
    73             * \brief Set coordinates
    74             *
    75             * \param latitude latitude
    76             * \param longitude longitude
    77             * \param altitude altitude
    78             */
    79             void SetCoordinates(double latitude,double longitude,double altitude);
     75  /*!
     76  * \brief Set coordinates
     77  *
     78  * \param latitude latitude
     79  * \param longitude longitude
     80  * \param altitude altitude
     81  */
     82  void SetCoordinates(double latitude, double longitude, double altitude);
    8083
    81             /*!
    82             * \brief Get coordinates
    83             *
    84             * \param latitude latitude
    85             * \param longitude longitude
    86             * \param altitude altitude
    87             */
    88             void GetCoordinates(double *latitude,double *longitude,double *altitude) const;
     84  /*!
     85  * \brief Get coordinates
     86  *
     87  * \param latitude latitude
     88  * \param longitude longitude
     89  * \param altitude altitude
     90  */
     91  void GetCoordinates(double *latitude, double *longitude,
     92                      double *altitude) const;
    8993
    90             Type const &GetDataType() const {return dataType;}
    91         private:
    92             /*!
    93             * \brief Copy datas
    94             *
    95             * Reimplemented from io_data. \n
    96             * See io_data::CopyDatas.
    97             *
    98             * \param dst destination buffer
    99             */
    100             void CopyDatas(char* ptr) const;
     94  Type const &GetDataType() const { return dataType; }
    10195
    102             double latitude;
    103             double longitude;
    104             double altitude;
    105             Type dataType;
    106     };
     96private:
     97  /*!
     98  * \brief Copy datas
     99  *
     100  * Reimplemented from io_data. \n
     101  * See io_data::CopyDatas.
     102  *
     103  * \param dst destination buffer
     104  */
     105  void CopyDatas(char *ptr) const;
     106
     107  double latitude;
     108  double longitude;
     109  double altitude;
     110  Type dataType;
     111};
    107112
    108113} // end namespace core

  • trunk/lib/FlairCore/src/GridLayout.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair
    23 {
    24 namespace gui
    25 {
     22namespace flair {
     23namespace gui {
    2624
    27 GridLayout::GridLayout(const LayoutPosition* position,string name): Layout(position->getLayout(),name,"GridLayout")
    28 {
    29     SetVolatileXmlProp("row",position->Row());
    30     SetVolatileXmlProp("col",position->Col());
    31     SendXml();
     25GridLayout::GridLayout(const LayoutPosition *position, string name)
     26    : Layout(position->getLayout(), name, "GridLayout") {
     27  SetVolatileXmlProp("row", position->Row());
     28  SetVolatileXmlProp("col", position->Col());
     29  SendXml();
    3230
    33     delete position;
     31  delete position;
    3432}
    3533
    36 GridLayout::~GridLayout()
    37 {
    38 
    39 }
     34GridLayout::~GridLayout() {}
    4035
    4136} // end namespace gui

  • trunk/lib/FlairCore/src/GridLayout.h

    r2 r15  
    1616#include <Layout.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
    22     class LayoutPosition;
     18namespace flair {
     19namespace gui {
     20class LayoutPosition;
    2321
    24     /*! \class GridLayout
    25     *
    26     * \brief Class displaying a QGridLayout on the ground station
    27     *
    28     */
    29     class GridLayout: public Layout
    30     {
    31         public:
    32             /*!
    33             * \brief Constructor
    34             *
    35             * Construct a QCheckBox at given position. \n
    36             * The GridLayout will automatically be child of position->getLayout() Layout. After calling this constructor,
    37             * position will be deleted as it is no longer usefull.
    38             *
    39             * \param parent parent
    40             * \param name name
     22/*! \class GridLayout
     23*
     24* \brief Class displaying a QGridLayout on the ground station
     25*
     26*/
     27class GridLayout : public Layout {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct a QCheckBox at given position. \n
     33  * The GridLayout will automatically be child of position->getLayout() Layout.
     34  After calling this constructor,
     35  * position will be deleted as it is no longer usefull.
     36  *
     37  * \param parent parent
     38  * \param name name
    4139
    42             */
    43             GridLayout(const LayoutPosition* position,std::string name);
     40  */
     41  GridLayout(const LayoutPosition *position, std::string name);
    4442
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~GridLayout();
     43  /*!
     44  * \brief Destructor
     45  *
     46  */
     47  ~GridLayout();
    5048
    51         private:
    52     };
     49private:
     50};
    5351
    5452} // end namespace gui

  • trunk/lib/FlairCore/src/GroupBox.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair
    23 {
    24 namespace gui
    25 {
     22namespace flair {
     23namespace gui {
    2624
    27 GroupBox::GroupBox(const LayoutPosition* position,string name): Layout(position->getLayout(),name,"GroupBox")
    28 {
    29     SetVolatileXmlProp("row",position->Row());
    30     SetVolatileXmlProp("col",position->Col());
    31     SendXml();
     25GroupBox::GroupBox(const LayoutPosition *position, string name)
     26    : Layout(position->getLayout(), name, "GroupBox") {
     27  SetVolatileXmlProp("row", position->Row());
     28  SetVolatileXmlProp("col", position->Col());
     29  SendXml();
    3230
    33     delete position;
     31  delete position;
    3432}
    3533
    36 GroupBox::~GroupBox()
    37 {
    38 
    39 }
     34GroupBox::~GroupBox() {}
    4035
    4136} // end namespace gui

  • trunk/lib/FlairCore/src/GroupBox.h

    r2 r15  
    1616#include <Layout.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
    22     class LayoutPosition;
     18namespace flair {
     19namespace gui {
     20class LayoutPosition;
    2321
    24     /*! \class GroupBox
    25     *
    26     * \brief Class displaying a QGroupBox on the ground station
    27     *
    28     */
    29     class GroupBox: public Layout
    30     {
    31         public:
    32             /*!
    33             * \brief Constructor
    34             *
    35             * Construct a QGroupBox at given position. \n
    36             * The GroupBox will automatically be child of position->getLayout() Layout. After calling this constructor,
    37             * position will be deleted as it is no longer usefull.
    38             *
    39             * \param position position
    40             * \param name name
    41             */
    42             GroupBox(const LayoutPosition* position,std::string name);
     22/*! \class GroupBox
     23*
     24* \brief Class displaying a QGroupBox on the ground station
     25*
     26*/
     27class GroupBox : public Layout {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct a QGroupBox at given position. \n
     33  * The GroupBox will automatically be child of position->getLayout() Layout.
     34  *After calling this constructor,
     35  * position will be deleted as it is no longer usefull.
     36  *
     37  * \param position position
     38  * \param name name
     39  */
     40  GroupBox(const LayoutPosition *position, std::string name);
    4341
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~GroupBox();
     42  /*!
     43  * \brief Destructor
     44  *
     45  */
     46  ~GroupBox();
    4947
    50         private:
    51     };
     48private:
     49};
    5250
    5351} // end namespace gui

  • trunk/lib/FlairCore/src/I2cPort.h

    r2 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21 namespace core
    22 {
    23     /*! \class I2cPort
    24     *
    25     * \brief Base class for i2c port
    26     *
    27     * This class has a Mutex which must be used to protect access to the port in case
    28     * that more than one Thread is using it. Lock the Mutex before any communication (including SetSlave)
    29     * and release it after communication.
    30     */
    31     class I2cPort: public Mutex
    32     {
    33         public:
    34             /*!
    35             * \brief Constructor
    36             *
    37             * Construct an i2c port.
    38             *
    39             * \param parent parent
    40             * \param name name
    41             */
    42             I2cPort(const Object* parent,std::string name): Mutex(parent,name)
    43             {}
     19namespace flair {
     20namespace core {
     21/*! \class I2cPort
     22*
     23* \brief Base class for i2c port
     24*
     25* This class has a Mutex which must be used to protect access to the port in
     26*case
     27* that more than one Thread is using it. Lock the Mutex before any communication
     28*(including SetSlave)
     29* and release it after communication.
     30*/
     31class I2cPort : public Mutex {
     32public:
     33  /*!
     34  * \brief Constructor
     35  *
     36  * Construct an i2c port.
     37  *
     38  * \param parent parent
     39  * \param name name
     40  */
     41  I2cPort(const Object *parent, std::string name) : Mutex(parent, name) {}
    4442
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~I2cPort(){};
     43  /*!
     44  * \brief Destructor
     45  *
     46  */
     47  ~I2cPort(){};
    5048
    51             /*!
    52             * \brief Set slave's address
    53             *
    54             * This function need to be called before any communication.
    55             *
    56             * \param address slave's address
    57             */
    58             virtual int SetSlave(uint16_t address)=0;
     49  /*!
     50  * \brief Set slave's address
     51  *
     52  * This function need to be called before any communication.
     53  *
     54  * \param address slave's address
     55  */
     56  virtual int SetSlave(uint16_t address) = 0;
    5957
    60             /*!
    61             * \brief Write datas
    62             *
    63             * \param buf pointer to datas
    64             * \param nbyte length of datas
    65             *
    66             * \return amount of written datas
    67             */
    68             virtual ssize_t Write(const void *buf,size_t nbyte)=0;
     58  /*!
     59  * \brief Write datas
     60  *
     61  * \param buf pointer to datas
     62  * \param nbyte length of datas
     63  *
     64  * \return amount of written datas
     65  */
     66  virtual ssize_t Write(const void *buf, size_t nbyte) = 0;
    6967
    70             /*!
    71             * \brief Read datas
    72             *
    73             * \param buf pointer to datas
    74             * \param nbyte length of datas
    75             *
    76             * \return amount of read datas
    77             */
    78             virtual ssize_t Read(void *buf,size_t nbyte)=0;
     68  /*!
     69  * \brief Read datas
     70  *
     71  * \param buf pointer to datas
     72  * \param nbyte length of datas
     73  *
     74  * \return amount of read datas
     75  */
     76  virtual ssize_t Read(void *buf, size_t nbyte) = 0;
    7977
    80             /*!
    81             * \brief Set RX timeout
    82             *
    83             * Timeout for waiting an ACK from the slave.
    84             *
    85             * \param timeout_ns timeout in nano second
    86             */
    87             virtual void SetRxTimeout(Time timeout_ns)=0;
     78  /*!
     79  * \brief Set RX timeout
     80  *
     81  * Timeout for waiting an ACK from the slave.
     82  *
     83  * \param timeout_ns timeout in nano second
     84  */
     85  virtual void SetRxTimeout(Time timeout_ns) = 0;
    8886
    89             /*!
    90             * \brief Set TX timeout
    91             *
    92             * Timeout for waiting an ACK from the slave.
    93             *
    94             * \param timeout_ns timeout in nano second
    95             */
    96             virtual void SetTxTimeout(Time timeout_ns)=0;
    97 
    98     };
     87  /*!
     88  * \brief Set TX timeout
     89  *
     90  * Timeout for waiting an ACK from the slave.
     91  *
     92  * \param timeout_ns timeout in nano second
     93  */
     94  virtual void SetTxTimeout(Time timeout_ns) = 0;
     95};
    9996} // end namespace core
    10097} // end namespace framework

  • trunk/lib/FlairCore/src/IODataElement.h

    r2 r15  
    1616#include "io_data.h"
    1717
    18 namespace flair { namespace core {
     18namespace flair {
     19namespace core {
    1920
    20     class DataType;
     21class DataType;
    2122
    22     /*! \class IODataElement
    23     *
    24     * \brief Abstract class for accessing an element of an io_data.
    25     */
    26     class IODataElement: public Object
    27     {
    28         public:
    29             /*!
    30             * \brief Constructor
    31             *
    32             * Construct an IODataElement. \n
    33             *
    34             */
    35             IODataElement(const io_data* parent,std::string name): Object(parent,name)
    36             {
    37                 this->parent=parent;
    38             }
    39             size_t Size() const
    40             {
    41                 return size;
    42             }
     23/*! \class IODataElement
     24*
     25* \brief Abstract class for accessing an element of an io_data.
     26*/
     27class IODataElement : public Object {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct an IODataElement. \n
     33  *
     34  */
     35  IODataElement(const io_data *parent, std::string name)
     36      : Object(parent, name) {
     37    this->parent = parent;
     38  }
     39  size_t Size() const { return size; }
    4340
    44             virtual void CopyData(char* dst) const=0;
     41  virtual void CopyData(char *dst) const = 0;
    4542
    46             /*!
    47             * \brief DataType
    48             *
    49             * \return type of data
    50             */
    51             virtual DataType const &GetDataType(void) const=0;
     43  /*!
     44  * \brief DataType
     45  *
     46  * \return type of data
     47  */
     48  virtual DataType const &GetDataType(void) const = 0;
    5249
    53         protected:
    54             size_t size;
     50protected:
     51  size_t size;
    5552
    56         private:
    57             const io_data* parent;
    58 
    59     };
     53private:
     54  const io_data *parent;
     55};
    6056
    6157} // end namespace core

  • trunk/lib/FlairCore/src/IODevice.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair
    27 {
    28 namespace core
    29 {
     26namespace flair {
     27namespace core {
    3028
    31 IODevice::IODevice(const Object* parent,string name): Object(parent,name,"IODevice")
    32 {
    33     pimpl_=new IODevice_impl(this);
     29IODevice::IODevice(const Object *parent, string name)
     30    : Object(parent, name, "IODevice") {
     31  pimpl_ = new IODevice_impl(this);
    3432}
    3533
    36 IODevice::~IODevice()
    37 {
    38     delete pimpl_;
     34IODevice::~IODevice() { delete pimpl_; }
     35
     36void IODevice::AddDeviceToLog(const IODevice *device) {
     37  pimpl_->AddDeviceToLog(device);
    3938}
    4039
    41 void IODevice::AddDeviceToLog(const IODevice* device)
    42 {
    43     pimpl_->AddDeviceToLog(device);
     40void IODevice::OutputToShMem(bool enabled) { pimpl_->OutputToShMem(enabled); }
     41
     42void IODevice::ProcessUpdate(io_data *data) {
     43  if (data != NULL)
     44    data->pimpl_->SetConsistent(true);
     45
     46  for (size_t i = 0; i < TypeChilds()->size(); i++) {
     47    ((IODevice *)TypeChilds()->at(i))->UpdateFrom(data);
     48  }
     49
     50  if (data != NULL) {
     51    if (getFrameworkManager()->IsLogging() == true)
     52      pimpl_->WriteLog(data->DataTime());
     53
     54    data->pimpl_->Circle();
     55  }
     56
     57  pimpl_->WriteToShMem();
     58
     59  pimpl_->ResumeThread();
    4460}
    4561
    46 void IODevice::OutputToShMem(bool enabled) {
    47     pimpl_->OutputToShMem(enabled);
    48 }
     62void IODevice::AddDataToLog(const io_data *data) { pimpl_->AddDataToLog(data); }
    4963
    50 void IODevice::ProcessUpdate(io_data* data)
    51 {
    52     if(data!=NULL) data->pimpl_->SetConsistent(true);
     64DataType const &IODevice::GetInputDataType() const { return dummyType; }
    5365
    54     for(size_t i=0;i<TypeChilds()->size();i++)
    55     {
    56         ((IODevice*)TypeChilds()->at(i))->UpdateFrom(data);
    57     }
    58 
    59     if(data!=NULL)
    60     {
    61         if(getFrameworkManager()->IsLogging()==true) pimpl_->WriteLog(data->DataTime());
    62 
    63         data->pimpl_->Circle();
    64     }
    65 
    66     pimpl_->WriteToShMem();
    67 
    68     pimpl_->ResumeThread();
    69 }
    70 
    71 void IODevice::AddDataToLog(const io_data* data)
    72 {
    73     pimpl_->AddDataToLog(data);
    74 }
    75 
    76 DataType const &IODevice::GetInputDataType() const {
    77     return dummyType;
    78 }
    79 
    80 DataType const &IODevice::GetOutputDataType() const {
    81     return dummyType;
    82 }
     66DataType const &IODevice::GetOutputDataType() const { return dummyType; }
    8367
    8468} // end namespace core

  • trunk/lib/FlairCore/src/IODevice.h

    r2 r15  
    2020class FrameworkManager_impl;
    2121
    22 namespace flair { namespace core {
     22namespace flair {
     23namespace core {
    2324
    24     class io_data;
    25     class DataType;
     25class io_data;
     26class DataType;
    2627
    27     /*! \class IODevice
    28     *
    29     * \brief Abstract class for input/ouput system
    30     *
    31     * An input/output system is generally used to describe a sensor, an actuator or a filter. \n
    32     * These systems can be linked (for exemple a sensor with a filter), when an IODevice
    33     * is created with a parent of type IODevice.
    34     * In this case, an update of the parent's data will call an update
    35     * of the child's data (for exemple when a sensor gets new datas, a filter is automatically called). \n
    36     * Output of this object can also be sent to a shared memory using the OutputToShMem method; in order to use it with an
    37     * external program.
    38     */
    39     class IODevice: public Object {
    40         friend class ::IODevice_impl;
    41         friend class ::Thread_impl;
    42         friend class ::FrameworkManager_impl;
     28/*! \class IODevice
     29*
     30* \brief Abstract class for input/ouput system
     31*
     32* An input/output system is generally used to describe a sensor, an actuator or
     33*a filter. \n
     34* These systems can be linked (for exemple a sensor with a filter), when an
     35*IODevice
     36* is created with a parent of type IODevice.
     37* In this case, an update of the parent's data will call an update
     38* of the child's data (for exemple when a sensor gets new datas, a filter is
     39*automatically called). \n
     40* Output of this object can also be sent to a shared memory using the
     41*OutputToShMem method; in order to use it with an
     42* external program.
     43*/
     44class IODevice : public Object {
     45  friend class ::IODevice_impl;
     46  friend class ::Thread_impl;
     47  friend class ::FrameworkManager_impl;
    4348
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct an IODevice of Object's type "IODevice" (see Object::ObjectType). \n
    49             * If parent's Object::ObjectType is also "IODevice", this IODevice will be linked to its parent
    50             * (see ProcessUpdate()).
    51             *
    52             * \param parent parent
    53             * \param name name
    54             */
    55             IODevice(const Object* parent,std::string name);
     49public:
     50  /*!
     51  * \brief Constructor
     52  *
     53  * Construct an IODevice of Object's type "IODevice" (see Object::ObjectType).
     54  *\n
     55  * If parent's Object::ObjectType is also "IODevice", this IODevice will be
     56  *linked to its parent
     57  * (see ProcessUpdate()).
     58  *
     59  * \param parent parent
     60  * \param name name
     61  */
     62  IODevice(const Object *parent, std::string name);
    5663
    57             /*!
    58             * \brief Destructor
    59             *
    60             */
    61             virtual ~IODevice();
     64  /*!
     65  * \brief Destructor
     66  *
     67  */
     68  virtual ~IODevice();
    6269
    63             /*!
    64             * \brief Add an IODevice to the logs
    65             *
    66             * The IODevice will be automatically logged among this IODevice logs,
    67             * if logging is enabled (see SetDataToLog(), FrameworkManager::StartLog
    68             * and FrameworkManager::AddDeviceToLog). \n
    69             * Logging happens when ProcessUpdate() is called. \n
    70             * Note that when an IODevice is just added for logs (ie. no parent/child
    71             * link between the two IODevice),
    72             * UpdateFrom() is not automatically called.
    73             *
    74             * \param device IODevice to log
    75             */
    76             void AddDeviceToLog(const IODevice* device);
     70  /*!
     71  * \brief Add an IODevice to the logs
     72  *
     73  * The IODevice will be automatically logged among this IODevice logs,
     74  * if logging is enabled (see SetDataToLog(), FrameworkManager::StartLog
     75  * and FrameworkManager::AddDeviceToLog). \n
     76  * Logging happens when ProcessUpdate() is called. \n
     77  * Note that when an IODevice is just added for logs (ie. no parent/child
     78  * link between the two IODevice),
     79  * UpdateFrom() is not automatically called.
     80  *
     81  * \param device IODevice to log
     82  */
     83  void AddDeviceToLog(const IODevice *device);
    7784
    78             /*!
    79             * \brief Add an io_data to the log
    80             *
    81             * The io_data will be automatically logged if enabled (see FrameworkManager::StartLog
    82             * and FrameworkManager::AddDeviceToLog),
    83             * during call to ProcessUpdate().
    84             *
    85             * \param data data to log
    86             */
    87             void AddDataToLog(const io_data* data);
     85  /*!
     86  * \brief Add an io_data to the log
     87  *
     88  * The io_data will be automatically logged if enabled (see
     89  *FrameworkManager::StartLog
     90  * and FrameworkManager::AddDeviceToLog),
     91  * during call to ProcessUpdate().
     92  *
     93  * \param data data to log
     94  */
     95  void AddDataToLog(const io_data *data);
    8896
    89             /*!
    90             * \brief Send the output to a shared memory
    91             *
    92             * Use this method to output log datas to a shared memory.
    93             * This can be usefull to get datas from an external progam. \n
    94             * Output happens when ProcessUpdate() is called. \n
    95             * The name and the structure of the shared memory will be displayed when
    96             * this method is called with true as argument. \n
    97             * By default it is not enabled.
    98             *
    99             *
    100             * \param enabled true to enable the output, false otherwise
    101             */
    102             void OutputToShMem(bool enabled);
     97  /*!
     98  * \brief Send the output to a shared memory
     99  *
     100  * Use this method to output log datas to a shared memory.
     101  * This can be usefull to get datas from an external progam. \n
     102  * Output happens when ProcessUpdate() is called. \n
     103  * The name and the structure of the shared memory will be displayed when
     104  * this method is called with true as argument. \n
     105  * By default it is not enabled.
     106  *
     107  *
     108  * \param enabled true to enable the output, false otherwise
     109  */
     110  void OutputToShMem(bool enabled);
    103111
    104             //TODO: these 2 method should be pure virtual
    105             virtual DataType const &GetInputDataType() const;
    106             virtual DataType const &GetOutputDataType() const;
     112  // TODO: these 2 method should be pure virtual
     113  virtual DataType const &GetInputDataType() const;
     114  virtual DataType const &GetOutputDataType() const;
    107115
    108         protected:
    109             /*!
    110             * \brief Process the childs of type IODevice, and log if needed
    111             *
    112             * This method must be called after computing datas;
    113             * generally at the end of the reimplemented UpdateFrom or after acquiring datas in case of a sensor. \n
    114             * It will call UpdateFrom methods of each child of type IODevice,
    115             * and log all datas (this IODevice and its childs)
    116             * if logging is enabled (see SetDataToLog(), AddDeviceToLog(),
    117             * FrameworkManager::StartLog and FrameworkManager::AddDeviceToLog). \n
    118             * If a thread is waiting on this IODevice (see Thread::WaitUpdate), it will be resumed.
    119             *
    120             * \param data data to process
    121             */
    122             void ProcessUpdate(io_data* data);
     116protected:
     117  /*!
     118  * \brief Process the childs of type IODevice, and log if needed
     119  *
     120  * This method must be called after computing datas;
     121  * generally at the end of the reimplemented UpdateFrom or after acquiring
     122  *datas in case of a sensor. \n
     123  * It will call UpdateFrom methods of each child of type IODevice,
     124  * and log all datas (this IODevice and its childs)
     125  * if logging is enabled (see SetDataToLog(), AddDeviceToLog(),
     126  * FrameworkManager::StartLog and FrameworkManager::AddDeviceToLog). \n
     127  * If a thread is waiting on this IODevice (see Thread::WaitUpdate), it will be
     128  *resumed.
     129  *
     130  * \param data data to process
     131  */
     132  void ProcessUpdate(io_data *data);
    123133
    124         private:
    125             /*!
    126             * \brief Update using provided datas
    127             *
    128             * This method is automatically called by ProcessUpdate()
    129             * of the Object::Parent's if its Object::ObjectType is "IODevice". \n
    130             * This method must be reimplemented, in order to process the data from the parent.
    131             *
    132             * \param data data from the parent to process
    133             */
    134             virtual void UpdateFrom(const io_data *data)=0;
     134private:
     135  /*!
     136  * \brief Update using provided datas
     137  *
     138  * This method is automatically called by ProcessUpdate()
     139  * of the Object::Parent's if its Object::ObjectType is "IODevice". \n
     140  * This method must be reimplemented, in order to process the data from the
     141  *parent.
     142  *
     143  * \param data data from the parent to process
     144  */
     145  virtual void UpdateFrom(const io_data *data) = 0;
    135146
    136             class IODevice_impl* pimpl_;
    137     };
     147  class IODevice_impl *pimpl_;
     148};
    138149
    139150} // end namespace core

  • trunk/lib/FlairCore/src/IODevice_impl.cpp

    r2 r15  
    3131using namespace flair::core;
    3232
    33 IODevice_impl::IODevice_impl(const IODevice* self) {
    34     this->self=self;
    35     thread_to_wake=NULL;
    36     wake_mutex= new Mutex(self);
    37     framework_impl=::getFrameworkManagerImpl();
    38     framework=getFrameworkManager();
    39     tobelogged=false;
    40     outputtoshm=false;
     33IODevice_impl::IODevice_impl(const IODevice *self) {
     34  this->self = self;
     35  thread_to_wake = NULL;
     36  wake_mutex = new Mutex(self);
     37  framework_impl = ::getFrameworkManagerImpl();
     38  framework = getFrameworkManager();
     39  tobelogged = false;
     40  outputtoshm = false;
    4141}
    4242
    … …  
    4444
    4545void IODevice_impl::OutputToShMem(bool enabled) {
    46     if(framework->IsLogging()) {
    47         self->Err("impossible while logging\n");
     46  if (framework->IsLogging()) {
     47    self->Err("impossible while logging\n");
     48  } else {
     49    if (LogSize() == 0) {
     50      self->Warn("log size is null, not enabling output to shared memory.");
     51      return;
     52    }
     53
     54    if (enabled) {
     55      string dev_name =
     56          getFrameworkManager()->ObjectName() + "-" + self->ObjectName();
     57      shmem = new SharedMem(self, dev_name.c_str(), LogSize());
     58      outputtoshm = true;
     59
     60      Printf("Created %s shared memory for object %s; size: %i\n",
     61             dev_name.c_str(), self->ObjectName().c_str(), LogSize());
     62      PrintLogsDescriptors();
    4863    } else {
    49         if(LogSize()==0) {
    50             self->Warn("log size is null, not enabling output to shared memory.");
    51             return;
    52         }
    53 
    54         if(enabled) {
    55             string dev_name=getFrameworkManager()->ObjectName()+ "-" + self->ObjectName();
    56             shmem=new SharedMem(self,dev_name.c_str(),LogSize());
    57             outputtoshm=true;
    58 
    59             Printf("Created %s shared memory for object %s; size: %i\n",dev_name.c_str(),self->ObjectName().c_str(),LogSize());
    60             PrintLogsDescriptors();
    61         } else {
    62             outputtoshm=false;
    63             delete shmem;
    64         }
     64      outputtoshm = false;
     65      delete shmem;
    6566    }
     67  }
    6668}
    6769
    6870void IODevice_impl::PrintLogsDescriptors(void) {
    69     //own logs
    70     for(size_t i=0;i<datasToLog.size();i++) datasToLog.at(i)->pimpl_->PrintLogDescriptor();
    71     //childs logs
    72     for(size_t i=0;i<self->TypeChilds()->size();i++) ((IODevice*)self->TypeChilds()->at(i))->pimpl_->PrintLogsDescriptors();
    73     //manually added logs
    74     for(size_t i=0;i<devicesToLog.size();i++) devicesToLog.at(i)->pimpl_->PrintLogsDescriptors();
     71  // own logs
     72  for (size_t i = 0; i < datasToLog.size(); i++)
     73    datasToLog.at(i)->pimpl_->PrintLogDescriptor();
     74  // childs logs
     75  for (size_t i = 0; i < self->TypeChilds()->size(); i++)
     76    ((IODevice *)self->TypeChilds()->at(i))->pimpl_->PrintLogsDescriptors();
     77  // manually added logs
     78  for (size_t i = 0; i < devicesToLog.size(); i++)
     79    devicesToLog.at(i)->pimpl_->PrintLogsDescriptors();
    7580}
    7681
    7782void IODevice_impl::WriteToShMem(void) {
    78     if(outputtoshm) {
    79         size_t size=LogSize();
    80 
    81         char* buf=framework_impl->GetBuffer(size);
    82         char* buf_orig=buf;
    83         if(buf==NULL) {
    84             self->Err("err GetBuffer\n");
    85             return;
    86         }
    87 
    88         AppendLog(&buf);
    89 
    90         shmem->Write(buf_orig,size);
    91         framework_impl->ReleaseBuffer(buf_orig);
     83  if (outputtoshm) {
     84    size_t size = LogSize();
     85
     86    char *buf = framework_impl->GetBuffer(size);
     87    char *buf_orig = buf;
     88    if (buf == NULL) {
     89      self->Err("err GetBuffer\n");
     90      return;
    9291    }
    93 }
    94 
    95 void IODevice_impl::AddDeviceToLog(const IODevice* device) {
    96     if(framework->IsLogging()) {
    97         self->Err("impossible while logging\n");
    98     } else {
    99         devicesToLog.push_back(device);
    100     }
     92
     93    AppendLog(&buf);
     94
     95    shmem->Write(buf_orig, size);
     96    framework_impl->ReleaseBuffer(buf_orig);
     97  }
     98}
     99
     100void IODevice_impl::AddDeviceToLog(const IODevice *device) {
     101  if (framework->IsLogging()) {
     102    self->Err("impossible while logging\n");
     103  } else {
     104    devicesToLog.push_back(device);
     105  }
    101106}
    102107
    103108bool IODevice_impl::SetToBeLogged(void) {
    104     if(!tobelogged) {
    105         tobelogged=true;
    106         return true;
    107     } else {
    108         self->Warn("already added to log\n");
    109         return false;
    110     }
    111 }
    112 
    113 void IODevice_impl::WriteLogsDescriptors(fstream& desc_file,int *index) {
    114     //own descriptor
    115     for(size_t i=0;i<datasToLog.size();i++) datasToLog.at(i)->pimpl_->WriteLogDescriptor(desc_file,index);
    116     //childs descriptors
    117     for(size_t i=0;i<self->TypeChilds()->size();i++) ((IODevice*)self->TypeChilds()->at(i))->pimpl_->WriteLogsDescriptors(desc_file,index);
    118     //manually added logs
    119     for(size_t i=0;i<devicesToLog.size();i++) devicesToLog.at(i)->pimpl_->WriteLogsDescriptors(desc_file,index);
     109  if (!tobelogged) {
     110    tobelogged = true;
     111    return true;
     112  } else {
     113    self->Warn("already added to log\n");
     114    return false;
     115  }
     116}
     117
     118void IODevice_impl::WriteLogsDescriptors(fstream &desc_file, int *index) {
     119  // own descriptor
     120  for (size_t i = 0; i < datasToLog.size(); i++)
     121    datasToLog.at(i)->pimpl_->WriteLogDescriptor(desc_file, index);
     122  // childs descriptors
     123  for (size_t i = 0; i < self->TypeChilds()->size(); i++)
     124    ((IODevice *)self->TypeChilds()->at(i))
     125        ->pimpl_->WriteLogsDescriptors(desc_file, index);
     126  // manually added logs
     127  for (size_t i = 0; i < devicesToLog.size(); i++)
     128    devicesToLog.at(i)->pimpl_->WriteLogsDescriptors(desc_file, index);
    120129}
    121130
    122131void IODevice_impl::ResumeThread(void) {
    123     wake_mutex->GetMutex();
    124     if(thread_to_wake!=NULL) {
    125         thread_to_wake->Resume();
    126         thread_to_wake=NULL;
    127     }
    128     wake_mutex->ReleaseMutex();
    129 }
    130 
    131 void IODevice_impl::AddDataToLog(const io_data* data) {
    132     if(framework->IsLogging()) {
    133         self->Err("impossible while logging\n");
    134     } else {
    135         datasToLog.push_back(data);
    136     }
     132  wake_mutex->GetMutex();
     133  if (thread_to_wake != NULL) {
     134    thread_to_wake->Resume();
     135    thread_to_wake = NULL;
     136  }
     137  wake_mutex->ReleaseMutex();
     138}
     139
     140void IODevice_impl::AddDataToLog(const io_data *data) {
     141  if (framework->IsLogging()) {
     142    self->Err("impossible while logging\n");
     143  } else {
     144    datasToLog.push_back(data);
     145  }
    137146}
    138147
    139148size_t IODevice_impl::LogSize() const {
    140     size_t value=0;
    141 
    142     for(size_t i=0;i<datasToLog.size();i++) {
    143         value+=datasToLog.at(i)->GetDataType().GetSize();
    144     }
    145 
    146     //childs logs
    147     for(size_t i=0;i<self->TypeChilds()->size();i++) {
    148         value+=((IODevice*)self->TypeChilds()->at(i))->pimpl_->LogSize();
    149     }
    150     //manually added logs
    151     for(size_t i=0;i<devicesToLog.size();i++) {
    152         value+=devicesToLog.at(i)->pimpl_->LogSize();
    153     }
    154 
    155     return value;
     149  size_t value = 0;
     150
     151  for (size_t i = 0; i < datasToLog.size(); i++) {
     152    value += datasToLog.at(i)->GetDataType().GetSize();
     153  }
     154
     155  // childs logs
     156  for (size_t i = 0; i < self->TypeChilds()->size(); i++) {
     157    value += ((IODevice *)self->TypeChilds()->at(i))->pimpl_->LogSize();
     158  }
     159  // manually added logs
     160  for (size_t i = 0; i < devicesToLog.size(); i++) {
     161    value += devicesToLog.at(i)->pimpl_->LogSize();
     162  }
     163
     164  return value;
    156165}
    157166
    158167void IODevice_impl::WriteLog(Time time) {
    159     if(tobelogged==false) return;
    160 
    161     size_t size=LogSize();
    162 
    163     char* buf=framework_impl->GetBuffer(sizeof(FrameworkManager_impl::log_header_t)+size);
    164     char* buf_orig=buf;
    165     if(buf==NULL) {
    166         self->Err("err GetBuffer\n");
    167         return;
    168     }
    169 
    170     FrameworkManager_impl::log_header_t header;
    171     header.device=self;
    172     header.size=size;
    173     header.time=time;
    174 
    175     memcpy(buf,&header,sizeof(FrameworkManager_impl::log_header_t));
    176     buf+=sizeof(FrameworkManager_impl::log_header_t);
    177     AppendLog(&buf);
    178 
    179     framework_impl->WriteLog(buf_orig,sizeof(FrameworkManager_impl::log_header_t)+size);
    180     framework_impl->ReleaseBuffer(buf_orig);
    181 }
    182 
    183 void IODevice_impl::AppendLog(char** ptr)
    184 {
    185     //Printf("AppendLog %s %x\n",self->ObjectName().c_str(),*ptr);
    186 
    187     //copy state to buf
    188     for(size_t i=0;i<datasToLog.size();i++) {
    189         //printf("copy\n");
    190         datasToLog.at(i)->CopyDatas(*ptr);
    191         (*ptr)+=datasToLog.at(i)->GetDataType().GetSize();
    192     }
    193 
    194     //copy linked states to buf
    195     for(size_t i=0;i<self->TypeChilds()->size();i++) {
    196         ((IODevice*)self->TypeChilds()->at(i))->pimpl_->AppendLog(ptr);
    197     }
    198     //copy manually added logs to buf
    199     for(size_t i=0;i<devicesToLog.size();i++) {
    200         devicesToLog.at(i)->pimpl_->AppendLog(ptr);
    201         //devices.at(i)->DataToLog()->CopyDatas(*ptr);
    202         //(*ptr)+=devices.at(i)->DataToLog()->Size();
    203     }
    204     //Printf("AppendLog %s ok\n",self->ObjectName().c_str());
    205 }
    206 
    207 int IODevice_impl::SetToWake(const Thread* thread) {
    208     int status=0;
    209 
    210     wake_mutex->GetMutex();
    211     if(thread_to_wake==NULL) {
    212         thread_to_wake=(Thread*)thread;
    213     } else {
    214         status=-1;
    215     }
    216     wake_mutex->ReleaseMutex();
    217 
    218     return status;
    219 }
     168  if (tobelogged == false)
     169    return;
     170
     171  size_t size = LogSize();
     172
     173  char *buf = framework_impl->GetBuffer(
     174      sizeof(FrameworkManager_impl::log_header_t) + size);
     175  char *buf_orig = buf;
     176  if (buf == NULL) {
     177    self->Err("err GetBuffer\n");
     178    return;
     179  }
     180
     181  FrameworkManager_impl::log_header_t header;
     182  header.device = self;
     183  header.size = size;
     184  header.time = time;
     185
     186  memcpy(buf, &header, sizeof(FrameworkManager_impl::log_header_t));
     187  buf += sizeof(FrameworkManager_impl::log_header_t);
     188  AppendLog(&buf);
     189
     190  framework_impl->WriteLog(buf_orig,
     191                           sizeof(FrameworkManager_impl::log_header_t) + size);
     192  framework_impl->ReleaseBuffer(buf_orig);
     193}
     194
     195void IODevice_impl::AppendLog(char **ptr) {
     196  // Printf("AppendLog %s %x\n",self->ObjectName().c_str(),*ptr);
     197
     198  // copy state to buf
     199  for (size_t i = 0; i < datasToLog.size(); i++) {
     200    // printf("copy\n");
     201    datasToLog.at(i)->CopyDatas(*ptr);
     202    (*ptr) += datasToLog.at(i)->GetDataType().GetSize();
     203  }
     204
     205  // copy linked states to buf
     206  for (size_t i = 0; i < self->TypeChilds()->size(); i++) {
     207    ((IODevice *)self->TypeChilds()->at(i))->pimpl_->AppendLog(ptr);
     208  }
     209  // copy manually added logs to buf
     210  for (size_t i = 0; i < devicesToLog.size(); i++) {
     211    devicesToLog.at(i)->pimpl_->AppendLog(ptr);
     212    // devices.at(i)->DataToLog()->CopyDatas(*ptr);
     213    //(*ptr)+=devices.at(i)->DataToLog()->Size();
     214  }
     215  // Printf("AppendLog %s ok\n",self->ObjectName().c_str());
     216}
     217
     218int IODevice_impl::SetToWake(const Thread *thread) {
     219  int status = 0;
     220
     221  wake_mutex->GetMutex();
     222  if (thread_to_wake == NULL) {
     223    thread_to_wake = (Thread *)thread;
     224  } else {
     225    status = -1;
     226  }
     227  wake_mutex->ReleaseMutex();
     228
     229  return status;
     230}

  • trunk/lib/FlairCore/src/ImuData.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair { namespace core {
     26namespace flair {
     27namespace core {
    2728
    2829/*! \class ImuDataElement
    2930    */
    30     class ImuDataElement: public IODataElement {
    31         public:
    32 
    33             ImuDataElement(const ImuData *inImudata,string name,ImuData::PlotableData_t inPlotableData):IODataElement(inImudata,name) {
    34                 imudata=inImudata;
    35                 plotableData=inPlotableData;
    36 
    37                 size=4;
    38             }
    39 
    40             ~ImuDataElement() {}
    41 
    42             void CopyData(char* dst) const {
    43                 float data;
    44                 Vector3D rawAcc=imudata->GetRawAcc();
    45                 Vector3D rawMag=imudata->GetRawMag();
    46                 Vector3D rawGyr=imudata->GetRawGyr();
    47                 switch(plotableData) {
    48                 case ImuData::RawAx:
    49                     data=rawAcc.x;
    50                     break;
    51                 case ImuData::RawAy:
    52                     data=rawAcc.y;
    53                     break;
    54                 case ImuData::RawAz:
    55                     data=rawAcc.z;
    56                     break;
    57                 case ImuData::RawGx:
    58                     data=rawGyr.x;
    59                     break;
    60                 case ImuData::RawGy:
    61                     data=rawGyr.y;
    62                     break;
    63                 case ImuData::RawGz:
    64                     data=rawGyr.z;
    65                     break;
    66                 case ImuData::RawGxDeg:
    67                     data=Euler::ToDegree(rawGyr.x);
    68                     break;
    69                 case ImuData::RawGyDeg:
    70                     data=Euler::ToDegree(rawGyr.y);
    71                     break;
    72                 case ImuData::RawGzDeg:
    73                     data=Euler::ToDegree(rawGyr.z);
    74                     break;
    75                 case ImuData::RawMx:
    76                     data=rawMag.x;
    77                     break;
    78                 case ImuData::RawMy:
    79                     data=rawMag.y;
    80                     break;
    81                 case ImuData::RawMz:
    82                     data=rawMag.z;
    83                     break;
    84                 default:
    85                     imudata->Err("data type unavailable.\n");
    86                     data=0;
    87                     break;
    88                 }
    89 
    90                 memcpy(dst,&data,sizeof(float));
    91             }
    92 
    93             FloatType const &GetDataType(void) const {
    94                 return dataType;
    95             }
    96 
    97         private:
    98             const ImuData *imudata;
    99             ImuData::PlotableData_t plotableData;
    100             FloatType dataType;
    101 
    102     };
    103 
    104 ImuData::ImuData(const Object* parent,std::string name,int n): io_data(parent,name,n),dataType(floatType) {
    105     if(n>1) Warn("n>1 not supported\n");
    106 
    107     AppendLogDescription("raw_ax",floatType);
    108     AppendLogDescription("raw_ay",floatType);
    109     AppendLogDescription("raw_az",floatType);
    110 
    111     AppendLogDescription("raw_gx",floatType);
    112     AppendLogDescription("raw_gy",floatType);
    113     AppendLogDescription("raw_gz",floatType);
    114 
    115     AppendLogDescription("raw_mx",floatType);
    116     AppendLogDescription("raw_my",floatType);
    117     AppendLogDescription("raw_mz",floatType);
    118 
     31class ImuDataElement : public IODataElement {
     32public:
     33  ImuDataElement(const ImuData *inImudata, string name,
     34                 ImuData::PlotableData_t inPlotableData)
     35      : IODataElement(inImudata, name) {
     36    imudata = inImudata;
     37    plotableData = inPlotableData;
     38
     39    size = 4;
     40  }
     41
     42  ~ImuDataElement() {}
     43
     44  void CopyData(char *dst) const {
     45    float data;
     46    Vector3D rawAcc = imudata->GetRawAcc();
     47    Vector3D rawMag = imudata->GetRawMag();
     48    Vector3D rawGyr = imudata->GetRawGyr();
     49    switch (plotableData) {
     50    case ImuData::RawAx:
     51      data = rawAcc.x;
     52      break;
     53    case ImuData::RawAy:
     54      data = rawAcc.y;
     55      break;
     56    case ImuData::RawAz:
     57      data = rawAcc.z;
     58      break;
     59    case ImuData::RawGx:
     60      data = rawGyr.x;
     61      break;
     62    case ImuData::RawGy:
     63      data = rawGyr.y;
     64      break;
     65    case ImuData::RawGz:
     66      data = rawGyr.z;
     67      break;
     68    case ImuData::RawGxDeg:
     69      data = Euler::ToDegree(rawGyr.x);
     70      break;
     71    case ImuData::RawGyDeg:
     72      data = Euler::ToDegree(rawGyr.y);
     73      break;
     74    case ImuData::RawGzDeg:
     75      data = Euler::ToDegree(rawGyr.z);
     76      break;
     77    case ImuData::RawMx:
     78      data = rawMag.x;
     79      break;
     80    case ImuData::RawMy:
     81      data = rawMag.y;
     82      break;
     83    case ImuData::RawMz:
     84      data = rawMag.z;
     85      break;
     86    default:
     87      imudata->Err("data type unavailable.\n");
     88      data = 0;
     89      break;
     90    }
     91
     92    memcpy(dst, &data, sizeof(float));
     93  }
     94
     95  FloatType const &GetDataType(void) const { return dataType; }
     96
     97private:
     98  const ImuData *imudata;
     99  ImuData::PlotableData_t plotableData;
     100  FloatType dataType;
     101};
     102
     103ImuData::ImuData(const Object *parent, std::string name, int n)
     104    : io_data(parent, name, n), dataType(floatType) {
     105  if (n > 1)
     106    Warn("n>1 not supported\n");
     107
     108  AppendLogDescription("raw_ax", floatType);
     109  AppendLogDescription("raw_ay", floatType);
     110  AppendLogDescription("raw_az", floatType);
     111
     112  AppendLogDescription("raw_gx", floatType);
     113  AppendLogDescription("raw_gy", floatType);
     114  AppendLogDescription("raw_gz", floatType);
     115
     116  AppendLogDescription("raw_mx", floatType);
     117  AppendLogDescription("raw_my", floatType);
     118  AppendLogDescription("raw_mz", floatType);
    119119}
    120120
    121121ImuData::~ImuData() {}
    122122
    123 
    124123Vector3D ImuData::GetRawAcc(void) const {
    125     Vector3D out;
    126     GetMutex();
    127     out=rawAcc;
    128     ReleaseMutex();
    129     return out;
     124  Vector3D out;
     125  GetMutex();
     126  out = rawAcc;
     127  ReleaseMutex();
     128  return out;
    130129}
    131130
    132131Vector3D ImuData::GetRawMag(void) const {
    133     Vector3D out;
    134     GetMutex();
    135     out=rawMag;
    136     ReleaseMutex();
    137     return out;
     132  Vector3D out;
     133  GetMutex();
     134  out = rawMag;
     135  ReleaseMutex();
     136  return out;
    138137}
    139138
    140139Vector3D ImuData::GetRawGyr(void) const {
    141     Vector3D out;
    142     GetMutex();
    143     out=rawGyr;
    144     ReleaseMutex();
    145     return out;
    146 }
    147 
    148 void ImuData::GetRawAccMagAndGyr(Vector3D &inRawAcc,Vector3D &inRawMag,Vector3D &inRawGyr) const {
    149     GetMutex();
    150     inRawAcc=rawAcc;
    151     inRawMag=rawMag;
    152     inRawGyr=rawGyr;
    153     ReleaseMutex();
     140  Vector3D out;
     141  GetMutex();
     142  out = rawGyr;
     143  ReleaseMutex();
     144  return out;
     145}
     146
     147void ImuData::GetRawAccMagAndGyr(Vector3D &inRawAcc, Vector3D &inRawMag,
     148                                 Vector3D &inRawGyr) const {
     149  GetMutex();
     150  inRawAcc = rawAcc;
     151  inRawMag = rawMag;
     152  inRawGyr = rawGyr;
     153  ReleaseMutex();
    154154}
    155155
    156156void ImuData::SetRawAcc(const Vector3D &inRawAcc) {
    157     GetMutex();
    158     rawAcc=inRawAcc;
    159     ReleaseMutex();
     157  GetMutex();
     158  rawAcc = inRawAcc;
     159  ReleaseMutex();
    160160}
    161161
    162162void ImuData::SetRawMag(const Vector3D &inRawMag) {
    163     GetMutex();
    164     rawMag=inRawMag;
    165     ReleaseMutex();
     163  GetMutex();
     164  rawMag = inRawMag;
     165  ReleaseMutex();
    166166}
    167167
    168168void ImuData::SetRawGyr(const Vector3D &inRawGyr) {
    169     GetMutex();
    170     rawGyr=inRawGyr;
    171     ReleaseMutex();
    172 }
    173 
    174 void ImuData::SetRawAccMagAndGyr(const Vector3D &inRawAcc,const Vector3D &inRawMag,const Vector3D &inRawGyr) {
    175     GetMutex();
    176     rawAcc=inRawAcc;
    177     rawMag=inRawMag;
    178     rawGyr=inRawGyr;
    179     ReleaseMutex();
    180 }
    181 
    182 IODataElement* ImuData::Element(PlotableData_t data_type) const {
    183     string name;
    184     switch(data_type) {
    185     case ImuData::RawAx:
    186         name="RawAx";
    187         break;
    188     case ImuData::RawAy:
    189         name="RawAy";
    190         break;
    191     case ImuData::RawAz:
    192         name="RawAz";
    193         break;
    194     case ImuData::RawGx:
    195         name="RawGx";
    196         break;
    197     case ImuData::RawGy:
    198         name="RawGy";
    199         break;
    200     case ImuData::RawGz:
    201         name="RawGz";
    202         break;
    203     case ImuData::RawGxDeg:
    204         name="RawGx degree";
    205         break;
    206     case ImuData::RawGyDeg:
    207         name="RawGy degree";
    208         break;
    209     case ImuData::RawGzDeg:
    210         name="RawGz degree";
    211         break;
    212     case ImuData::RawMx:
    213         name="RawMx";
    214         break;
    215     case ImuData::RawMy:
    216         name="RawMy";
    217         break;
    218     case ImuData::RawMz:
    219         name="RawMz";
    220         break;
    221     }
    222 
    223     return new ImuDataElement(this,name,data_type);
    224 }
    225 
    226 void ImuData::CopyDatas(char* dst) const {
    227     GetMutex();
    228 
    229     Queue(&dst,&rawAcc.x,sizeof(rawAcc.x));
    230     Queue(&dst,&rawAcc.y,sizeof(rawAcc.y));
    231     Queue(&dst,&rawAcc.z,sizeof(rawAcc.z));
    232 
    233     Queue(&dst,&rawGyr.x,sizeof(rawGyr.x));
    234     Queue(&dst,&rawGyr.y,sizeof(rawGyr.y));
    235     Queue(&dst,&rawGyr.z,sizeof(rawGyr.z));
    236 
    237     Queue(&dst,&rawMag.x,sizeof(rawMag.x));
    238     Queue(&dst,&rawMag.y,sizeof(rawMag.y));
    239     Queue(&dst,&rawMag.z,sizeof(rawMag.z));
    240 
    241     ReleaseMutex();
    242 }
    243 
    244 void ImuData::Queue(char** dst,const void *src,size_t size) const {
    245     memcpy(*dst,src,size);
    246     *dst+=size;
     169  GetMutex();
     170  rawGyr = inRawGyr;
     171  ReleaseMutex();
     172}
     173
     174void ImuData::SetRawAccMagAndGyr(const Vector3D &inRawAcc,
     175                                 const Vector3D &inRawMag,
     176                                 const Vector3D &inRawGyr) {
     177  GetMutex();
     178  rawAcc = inRawAcc;
     179  rawMag = inRawMag;
     180  rawGyr = inRawGyr;
     181  ReleaseMutex();
     182}
     183
     184IODataElement *ImuData::Element(PlotableData_t data_type) const {
     185  string name;
     186  switch (data_type) {
     187  case ImuData::RawAx:
     188    name = "RawAx";
     189    break;
     190  case ImuData::RawAy:
     191    name = "RawAy";
     192    break;
     193  case ImuData::RawAz:
     194    name = "RawAz";
     195    break;
     196  case ImuData::RawGx:
     197    name = "RawGx";
     198    break;
     199  case ImuData::RawGy:
     200    name = "RawGy";
     201    break;
     202  case ImuData::RawGz:
     203    name = "RawGz";
     204    break;
     205  case ImuData::RawGxDeg:
     206    name = "RawGx degree";
     207    break;
     208  case ImuData::RawGyDeg:
     209    name = "RawGy degree";
     210    break;
     211  case ImuData::RawGzDeg:
     212    name = "RawGz degree";
     213    break;
     214  case ImuData::RawMx:
     215    name = "RawMx";
     216    break;
     217  case ImuData::RawMy:
     218    name = "RawMy";
     219    break;
     220  case ImuData::RawMz:
     221    name = "RawMz";
     222    break;
     223  }
     224
     225  return new ImuDataElement(this, name, data_type);
     226}
     227
     228void ImuData::CopyDatas(char *dst) const {
     229  GetMutex();
     230
     231  Queue(&dst, &rawAcc.x, sizeof(rawAcc.x));
     232  Queue(&dst, &rawAcc.y, sizeof(rawAcc.y));
     233  Queue(&dst, &rawAcc.z, sizeof(rawAcc.z));
     234
     235  Queue(&dst, &rawGyr.x, sizeof(rawGyr.x));
     236  Queue(&dst, &rawGyr.y, sizeof(rawGyr.y));
     237  Queue(&dst, &rawGyr.z, sizeof(rawGyr.z));
     238
     239  Queue(&dst, &rawMag.x, sizeof(rawMag.x));
     240  Queue(&dst, &rawMag.y, sizeof(rawMag.y));
     241  Queue(&dst, &rawMag.z, sizeof(rawMag.z));
     242
     243  ReleaseMutex();
     244}
     245
     246void ImuData::Queue(char **dst, const void *src, size_t size) const {
     247  memcpy(*dst, src, size);
     248  *dst += size;
    247249}
    248250

  • trunk/lib/FlairCore/src/ImuData.h

    r2 r15  
    1717#include <Vector3D.h>
    1818
    19 namespace flair { namespace core {
    20 
    21     /*! \class ImuData
    22     *
    23     * \brief Class defining IMU datas
    24     *
    25     * IMU (inertial measurement unit) datas consist of raw accelerometer values, raw gyrometer values
    26     * and raw magnetometer values.
    27     *
    28     */
    29     class ImuData: public io_data {
    30         public:
    31             class Type: public DataType {
    32             public:
    33                 Type(ScalarType const &_elementDataType):
    34                     elementDataType(_elementDataType){}
    35                 ScalarType const &GetElementDataType() const {return elementDataType;}
    36                 std::string GetDescription() const {return "imu data";}
    37                 size_t GetSize() const {
    38                     size_t size=0;
    39                     size+=3*elementDataType.GetSize();//RawAcc
    40                     size+=3*elementDataType.GetSize();//RawGyr
    41                     size+=3*elementDataType.GetSize();//RawMag
    42                     return size;
    43                 }
    44             private:
    45                 ScalarType const &elementDataType;
    46             };
    47 
    48             /*!
    49             \enum PlotableData_t
    50             \brief Datas wich can be plotted in a DataPlot1D
    51             */
    52             typedef enum {
    53                 RawAx/*! x raw accelerometer */, RawAy/*! y raw accelerometer */ ,RawAz/*! z raw accelerometer */,
    54                 RawGx/*! x raw gyrometer */,RawGy/*! y raw gyrometer */,RawGz/*! z raw gyrometer */,
    55                 RawGxDeg/*! x raw gyrometer degree */,RawGyDeg/*! y raw gyrometer degree */,RawGzDeg/*! z raw gyrometer degree */,
    56                 RawMx/*! x raw magnetometer */,RawMy/*! y raw magnetometer */,RawMz/*! z raw magnetometer */
    57                 } PlotableData_t;
    58 
    59             /*!
    60             * \brief Constructor
    61             *
    62             * Construct an io_data representing IMU datas. \n
    63             *
    64             * \param parent parent
    65             * \param name name
    66             * \param n number of samples
    67             */
    68             ImuData(const Object* parent,std::string name="",int n=1);
    69 
    70             /*!
    71             * \brief Destructor
    72             *
    73             */
    74             ~ImuData();
    75 
    76             /*!
    77             * \brief Element
    78             *
    79             * Get a pointer to a specific element. This pointer can be used for plotting.
    80             *
    81             * \param data_type data type
    82             *
    83             * \return pointer to the element
    84             */
    85             IODataElement* Element(PlotableData_t data_type) const;
    86 
    87             /*!
    88             * \brief Get raw accelerations
    89             *
    90             * This method is mutex protected.
    91             *
    92             * \return raw accelerations
    93             *
    94             */
    95             Vector3D GetRawAcc(void) const;
    96 
    97             /*!
    98             * \brief Get raw magnetometers
    99             *
    100             * This method is mutex protected.
    101             *
    102             * \return raw magnetometers
    103             *
    104             */
    105             Vector3D GetRawMag(void) const;
    106 
    107             /*!
    108             * \brief Get raw angular speed
    109             *
    110             * This method is mutex protected.
    111             *
    112             * \return raw angular speed
    113             *
    114             */
    115             Vector3D GetRawGyr(void) const;
    116 
    117             /*!
    118             * \brief Get raw accelerations, magnetometers and angular speeds
    119             *
    120             * This method is mutex protected.
    121             *
    122             * \param rawAcc raw accelerations
    123             * \param rawMag raw magnetometers
    124             * \param rawGyr raw angular speeds
    125             *
    126             */
    127             void GetRawAccMagAndGyr(Vector3D &rawAcc,Vector3D &rawMag,Vector3D &rawGyr) const;
    128 
    129             /*!
    130             * \brief Set raw accelerations
    131             *
    132             * This method is mutex protected.
    133             *
    134             * \param raw accelerations
    135             *
    136             */
    137             void SetRawAcc(const Vector3D &rawAcc);
    138 
    139             /*!
    140             * \brief Set raw magnetometers
    141             *
    142             * This method is mutex protected.
    143             *
    144             * \param raw magnetometers
    145             *
    146             */
    147             void SetRawMag(const Vector3D &rawMag);
    148 
    149             /*!
    150             * \brief Set raw angular speed
    151             *
    152             * This method is mutex protected.
    153             *
    154             * \param raw angular speed
    155             *
    156             */
    157             void SetRawGyr(const Vector3D &rawGyr);
    158 
    159             /*!
    160             * \brief Set raw accelerations, magnetometers and angular speeds
    161             *
    162             * This method is mutex protected.
    163             *
    164             * \param rawAcc raw accelerations
    165             * \param rawMag raw magnetometers
    166             * \param rawGyr raw angular speeds
    167             *
    168             */
    169             void SetRawAccMagAndGyr(const Vector3D &rawAcc,const Vector3D &rawMag,const Vector3D &rawGyr);
    170 
    171             Type const&GetDataType() const {return dataType;}
    172         private:
    173             /*!
    174             * \brief Copy datas
    175             *
    176             * Reimplemented from io_data. \n
    177             * See io_data::CopyDatas.
    178             *
    179             * \param dst destination buffer
    180             */
    181             void CopyDatas(char* dst) const;
    182 
    183             /*!
    184             * \brief Raw accelerometer
    185             *
    186             */
    187             Vector3D rawAcc;
    188 
    189             /*!
    190             * \brief Raw gyrometer
    191             *
    192             */
    193             Vector3D rawGyr;
    194 
    195             /*!
    196             * \brief Raw magnetometer
    197             *
    198             */
    199             Vector3D rawMag;
    200 
    201             void Queue(char** dst,const void *src,size_t size) const;
    202             Type dataType;
    203 
    204     };
     19namespace flair {
     20namespace core {
     21
     22/*! \class ImuData
     23*
     24* \brief Class defining IMU datas
     25*
     26* IMU (inertial measurement unit) datas consist of raw accelerometer values, raw
     27*gyrometer values
     28* and raw magnetometer values.
     29*
     30*/
     31class ImuData : public io_data {
     32public:
     33  class Type : public DataType {
     34  public:
     35    Type(ScalarType const &_elementDataType)
     36        : elementDataType(_elementDataType) {}
     37    ScalarType const &GetElementDataType() const { return elementDataType; }
     38    std::string GetDescription() const { return "imu data"; }
     39    size_t GetSize() const {
     40      size_t size = 0;
     41      size += 3 * elementDataType.GetSize(); // RawAcc
     42      size += 3 * elementDataType.GetSize(); // RawGyr
     43      size += 3 * elementDataType.GetSize(); // RawMag
     44      return size;
     45    }
     46
     47  private:
     48    ScalarType const &elementDataType;
     49  };
     50
     51  /*!
     52  \enum PlotableData_t
     53  \brief Datas wich can be plotted in a DataPlot1D
     54  */
     55  typedef enum {
     56    RawAx /*! x raw accelerometer */,
     57    RawAy /*! y raw accelerometer */,
     58    RawAz /*! z raw accelerometer */,
     59    RawGx /*! x raw gyrometer */,
     60    RawGy /*! y raw gyrometer */,
     61    RawGz /*! z raw gyrometer */,
     62    RawGxDeg /*! x raw gyrometer degree */,
     63    RawGyDeg /*! y raw gyrometer degree */,
     64    RawGzDeg /*! z raw gyrometer degree */,
     65    RawMx /*! x raw magnetometer */,
     66    RawMy /*! y raw magnetometer */,
     67    RawMz /*! z raw magnetometer */
     68  } PlotableData_t;
     69
     70  /*!
     71  * \brief Constructor
     72  *
     73  * Construct an io_data representing IMU datas. \n
     74  *
     75  * \param parent parent
     76  * \param name name
     77  * \param n number of samples
     78  */
     79  ImuData(const Object *parent, std::string name = "", int n = 1);
     80
     81  /*!
     82  * \brief Destructor
     83  *
     84  */
     85  ~ImuData();
     86
     87  /*!
     88  * \brief Element
     89  *
     90  * Get a pointer to a specific element. This pointer can be used for plotting.
     91  *
     92  * \param data_type data type
     93  *
     94  * \return pointer to the element
     95  */
     96  IODataElement *Element(PlotableData_t data_type) const;
     97
     98  /*!
     99  * \brief Get raw accelerations
     100  *
     101  * This method is mutex protected.
     102  *
     103  * \return raw accelerations
     104  *
     105  */
     106  Vector3D GetRawAcc(void) const;
     107
     108  /*!
     109  * \brief Get raw magnetometers
     110  *
     111  * This method is mutex protected.
     112  *
     113  * \return raw magnetometers
     114  *
     115  */
     116  Vector3D GetRawMag(void) const;
     117
     118  /*!
     119  * \brief Get raw angular speed
     120  *
     121  * This method is mutex protected.
     122  *
     123  * \return raw angular speed
     124  *
     125  */
     126  Vector3D GetRawGyr(void) const;
     127
     128  /*!
     129  * \brief Get raw accelerations, magnetometers and angular speeds
     130  *
     131  * This method is mutex protected.
     132  *
     133  * \param rawAcc raw accelerations
     134  * \param rawMag raw magnetometers
     135  * \param rawGyr raw angular speeds
     136  *
     137  */
     138  void GetRawAccMagAndGyr(Vector3D &rawAcc, Vector3D &rawMag,
     139                          Vector3D &rawGyr) const;
     140
     141  /*!
     142  * \brief Set raw accelerations
     143  *
     144  * This method is mutex protected.
     145  *
     146  * \param raw accelerations
     147  *
     148  */
     149  void SetRawAcc(const Vector3D &rawAcc);
     150
     151  /*!
     152  * \brief Set raw magnetometers
     153  *
     154  * This method is mutex protected.
     155  *
     156  * \param raw magnetometers
     157  *
     158  */
     159  void SetRawMag(const Vector3D &rawMag);
     160
     161  /*!
     162  * \brief Set raw angular speed
     163  *
     164  * This method is mutex protected.
     165  *
     166  * \param raw angular speed
     167  *
     168  */
     169  void SetRawGyr(const Vector3D &rawGyr);
     170
     171  /*!
     172  * \brief Set raw accelerations, magnetometers and angular speeds
     173  *
     174  * This method is mutex protected.
     175  *
     176  * \param rawAcc raw accelerations
     177  * \param rawMag raw magnetometers
     178  * \param rawGyr raw angular speeds
     179  *
     180  */
     181  void SetRawAccMagAndGyr(const Vector3D &rawAcc, const Vector3D &rawMag,
     182                          const Vector3D &rawGyr);
     183
     184  Type const &GetDataType() const { return dataType; }
     185
     186private:
     187  /*!
     188  * \brief Copy datas
     189  *
     190  * Reimplemented from io_data. \n
     191  * See io_data::CopyDatas.
     192  *
     193  * \param dst destination buffer
     194  */
     195  void CopyDatas(char *dst) const;
     196
     197  /*!
     198  * \brief Raw accelerometer
     199  *
     200  */
     201  Vector3D rawAcc;
     202
     203  /*!
     204  * \brief Raw gyrometer
     205  *
     206  */
     207  Vector3D rawGyr;
     208
     209  /*!
     210  * \brief Raw magnetometer
     211  *
     212  */
     213  Vector3D rawMag;
     214
     215  void Queue(char **dst, const void *src, size_t size) const;
     216  Type dataType;
     217};
    205218
    206219} // end namespace core

  • trunk/lib/FlairCore/src/Label.cpp

    r2 r15  
    2222using std::string;
    2323
    24 namespace flair
    25 {
    26 namespace gui
    27 {
     24namespace flair {
     25namespace gui {
    2826
    29 Label::Label(const LayoutPosition* position,string name,size_t buf_size): Widget(position->getLayout(),name,"Label")
    30 {
    31     SetVolatileXmlProp("row",position->Row());
    32     SetVolatileXmlProp("col",position->Col());
     27Label::Label(const LayoutPosition *position, string name, size_t buf_size)
     28    : Widget(position->getLayout(), name, "Label") {
     29  SetVolatileXmlProp("row", position->Row());
     30  SetVolatileXmlProp("col", position->Col());
    3331
    34     SendXml();
     32  SendXml();
    3533
    36     printf_buffer=(char*)malloc(buf_size);
    37     if(printf_buffer==NULL) Err("erreur malloc\n");
     34  printf_buffer = (char *)malloc(buf_size);
     35  if (printf_buffer == NULL)
     36    Err("erreur malloc\n");
    3837
    39     delete position;
     38  delete position;
    4039}
    4140
    42 Label::~Label()
    43 {
    44     if(printf_buffer!=NULL) free(printf_buffer);
    45     printf_buffer=NULL;
     41Label::~Label() {
     42  if (printf_buffer != NULL)
     43    free(printf_buffer);
     44  printf_buffer = NULL;
    4645}
    4746
     47void Label::SetText(const char *format, ...) {
     48  int n;
    4849
    49 void Label::SetText(const char * format, ...)
    50 {
    51     int n;
     50  va_list args;
     51  va_start(args, format);
     52  n = vsprintf(printf_buffer, format, args);
     53  va_end(args);
     54  if (n <= 0)
     55    return;
    5256
    53     va_list args;
    54     va_start (args, format);
    55     n = vsprintf(printf_buffer,format, args);
    56     va_end (args);
    57     if (n<=0) return;
    58 
    59     SetVolatileXmlProp("value",printf_buffer);
    60     SendXml();
    61 
     57  SetVolatileXmlProp("value", printf_buffer);
     58  SendXml();
    6259}
    6360

  • trunk/lib/FlairCore/src/Label.h

    r2 r15  
    1616#include <Widget.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class LayoutPosition;
     21class LayoutPosition;
    2422
    25     /*! \class Label
    26     *
    27     * \brief Class displaying a QLabel on the ground station
    28     *
    29     */
    30     class Label:public Widget
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a QLabel at given position. \n
    37             * The Label will automatically be child of position->getLayout() Layout. After calling this constructor,
    38             * position will be deleted as it is no longer usefull.
    39             *
    40             * \param parent parent
    41             * \param name name
    42             * \param buf_size size of the text buffer
    43             */
    44             Label(const LayoutPosition* position,std::string name,size_t buf_size=255);
     23/*! \class Label
     24*
     25* \brief Class displaying a QLabel on the ground station
     26*
     27*/
     28class Label : public Widget {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a QLabel at given position. \n
     34  * The Label will automatically be child of position->getLayout() Layout. After
     35  *calling this constructor,
     36  * position will be deleted as it is no longer usefull.
     37  *
     38  * \param parent parent
     39  * \param name name
     40  * \param buf_size size of the text buffer
     41  */
     42  Label(const LayoutPosition *position, std::string name,
     43        size_t buf_size = 255);
    4544
    46             /*!
    47             * \brief Destructor
    48             *
    49             */
    50             ~Label();
     45  /*!
     46  * \brief Destructor
     47  *
     48  */
     49  ~Label();
    5150
    52             /*!
    53             * \brief Set text
    54             *
    55             * \param format text string to display, see standard printf
    56             */
    57             void SetText(const char * format, ...);
     51  /*!
     52  * \brief Set text
     53  *
     54  * \param format text string to display, see standard printf
     55  */
     56  void SetText(const char *format, ...);
    5857
    59         private:
    60             char* printf_buffer;
    61     };
     58private:
     59  char *printf_buffer;
     60};
    6261
    6362} // end namespace gui

  • trunk/lib/FlairCore/src/Layout.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair
    24 {
    25 namespace gui
    26 {
     23namespace flair {
     24namespace gui {
    2725
    28 Layout::Layout(const Widget* parent,string name,string type): Widget(parent,name,type)
    29 {
    30     mutex=new core::Mutex(this,name);
     26Layout::Layout(const Widget *parent, string name, string type)
     27    : Widget(parent, name, type) {
     28  mutex = new core::Mutex(this, name);
    3129}
    3230
    33 Layout::~Layout()
    34 {
     31Layout::~Layout() {}
    3532
     33LayoutPosition *Layout::LastRowLastCol(void) const {
     34  return new LayoutPosition(this, -1, 0);
    3635}
    3736
    38 LayoutPosition* Layout::LastRowLastCol(void) const
    39 {
    40     return new LayoutPosition(this,-1,0);
     37LayoutPosition *Layout::NewRow(void) const {
     38  return new LayoutPosition(this, -1, -1);
    4139}
    4240
    43 LayoutPosition*  Layout::NewRow(void) const
    44 {
    45     return new LayoutPosition(this,-1,-1);
    46 }
    47 
    48 LayoutPosition*  Layout::At(uint16_t row,uint16_t col) const
    49 {
    50     return new LayoutPosition(this,row,col);
     41LayoutPosition *Layout::At(uint16_t row, uint16_t col) const {
     42  return new LayoutPosition(this, row, col);
    5143}
    5244

  • trunk/lib/FlairCore/src/Layout.h

    r2 r15  
    1717#include <Mutex.h>
    1818
    19 namespace flair
    20 {
    21 namespace gui
    22 {
    23     class LayoutPosition;
     19namespace flair {
     20namespace gui {
     21class LayoutPosition;
    2422
    25     /*! \class Layout
    26     *
    27     * \brief Abstract class to display a layout on the ground station
    28     *
    29     * This is an abstract class to display layouts (like GridLayout, GroupBox, etc). \n
    30     * A layout can contains Widgets (Box, DataPlot, Picture, etc). \n
    31     * Layout holds a Mutex, which can be used to protect access to Box's value for example.
    32     */
    33     class Layout: public Widget
    34     {
    35         friend class Box;
     23/*! \class Layout
     24*
     25* \brief Abstract class to display a layout on the ground station
     26*
     27* This is an abstract class to display layouts (like GridLayout, GroupBox, etc).
     28*\n
     29* A layout can contains Widgets (Box, DataPlot, Picture, etc). \n
     30* Layout holds a Mutex, which can be used to protect access to Box's value for
     31*example.
     32*/
     33class Layout : public Widget {
     34  friend class Box;
    3635
    37         public:
    38             /*!
    39             * \brief Constructor
    40             *
    41             * Construct a Layout. Type is a predifined one, and will be
    42             * interpreted by the ground station.
    43             *
    44             * \param parent parent
    45             * \param name name
    46             * \param type type of layout
    47             */
    48             Layout(const Widget* parent,std::string name,std::string type);
     36public:
     37  /*!
     38  * \brief Constructor
     39  *
     40  * Construct a Layout. Type is a predifined one, and will be
     41  * interpreted by the ground station.
     42  *
     43  * \param parent parent
     44  * \param name name
     45  * \param type type of layout
     46  */
     47  Layout(const Widget *parent, std::string name, std::string type);
    4948
    50             /*!
    51             * \brief Destructor
    52             *
    53             */
    54             ~Layout();
     49  /*!
     50  * \brief Destructor
     51  *
     52  */
     53  ~Layout();
    5554
    56             /*!
    57             * \brief Last Row and Col LayoutPosition
    58             *
    59             * Get a LayoutPosition at the last row and col.
    60             *
    61             * \return the LayoutPosition
    62             */
    63             LayoutPosition* LastRowLastCol(void) const;
     55  /*!
     56  * \brief Last Row and Col LayoutPosition
     57  *
     58  * Get a LayoutPosition at the last row and col.
     59  *
     60  * \return the LayoutPosition
     61  */
     62  LayoutPosition *LastRowLastCol(void) const;
    6463
    65             /*!
    66             * \brief New Row LayoutPosition
    67             *
    68             * Get a LayoutPosition at a new row and first col. This new row will be at the last position.
    69             *
    70             * \return the LayoutPosition
    71             */
    72             LayoutPosition*  NewRow(void) const;
     64  /*!
     65  * \brief New Row LayoutPosition
     66  *
     67  * Get a LayoutPosition at a new row and first col. This new row will be at the
     68  *last position.
     69  *
     70  * \return the LayoutPosition
     71  */
     72  LayoutPosition *NewRow(void) const;
    7373
    74             /*!
    75             * \brief LayoutPosition at specified position
    76             *
    77             * Get a LayoutPosition at specified row and col.
    78             *
    79             * \param row row
    80             * \param col col
    81             *
    82             * \return the LayoutPosition
    83             */
    84             LayoutPosition*  At(uint16_t row,uint16_t col) const;
     74  /*!
     75  * \brief LayoutPosition at specified position
     76  *
     77  * Get a LayoutPosition at specified row and col.
     78  *
     79  * \param row row
     80  * \param col col
     81  *
     82  * \return the LayoutPosition
     83  */
     84  LayoutPosition *At(uint16_t row, uint16_t col) const;
    8585
    86         private:
    87             /*!
    88             * \brief Mutex
    89             *
    90             * This Mutex can be used by friends class like Box to protect access
    91             * to Box's value.
    92             */
    93             core::Mutex* mutex;
    94     };
     86private:
     87  /*!
     88  * \brief Mutex
     89  *
     90  * This Mutex can be used by friends class like Box to protect access
     91  * to Box's value.
     92  */
     93  core::Mutex *mutex;
     94};
    9595
    9696} // end namespace gui

  • trunk/lib/FlairCore/src/LayoutPosition.cpp

    r2 r15  
    1717#include "LayoutPosition.h"
    1818
     19namespace flair {
     20namespace gui {
    1921
    20 namespace flair
    21 {
    22 namespace gui
    23 {
    24 
    25 LayoutPosition::LayoutPosition(const Layout* layout,int16_t row,int16_t col)
    26 {
    27     this->layout=layout;
    28     this->row=row;
    29     this->col=col;
     22LayoutPosition::LayoutPosition(const Layout *layout, int16_t row, int16_t col) {
     23  this->layout = layout;
     24  this->row = row;
     25  this->col = col;
    3026}
    3127
    32 LayoutPosition::~LayoutPosition()
    33 {
     28LayoutPosition::~LayoutPosition() {}
    3429
    35 }
     30const Layout *LayoutPosition::getLayout(void) const { return layout; }
    3631
    37 const Layout* LayoutPosition::getLayout(void) const
    38 {
    39     return layout;
    40 }
     32int16_t LayoutPosition::Row(void) const { return row; }
    4133
    42 int16_t LayoutPosition::Row(void) const
    43 {
    44     return row;
    45 }
    46 
    47 int16_t LayoutPosition::Col(void) const
    48 {
    49     return col;
    50 }
     34int16_t LayoutPosition::Col(void) const { return col; }
    5135
    5236} // end namespace gui

  • trunk/lib/FlairCore/src/LayoutPosition.h

    r2 r15  
    1616#include <stdint.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
    22     class Layout;
     18namespace flair {
     19namespace gui {
     20class Layout;
    2321
    24     /*! \class LayoutPosition
    25     *
    26     * \brief Class to define a position in a layout on the ground station.
    27     *
     22/*! \class LayoutPosition
     23*
     24* \brief Class to define a position in a layout on the ground station.
     25*
    2826
    29     */
    30     class LayoutPosition
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a LayoutPosition, in given Layout at given place.
    37             *
    38             * \param layout layout
    39             * \param row row position
    40             * \param col col position
    41             */
    42             LayoutPosition(const Layout* layout,int16_t row,int16_t col);
     27*/
     28class LayoutPosition {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a LayoutPosition, in given Layout at given place.
     34  *
     35  * \param layout layout
     36  * \param row row position
     37  * \param col col position
     38  */
     39  LayoutPosition(const Layout *layout, int16_t row, int16_t col);
    4340
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~LayoutPosition();
     41  /*!
     42  * \brief Destructor
     43  *
     44  */
     45  ~LayoutPosition();
    4946
    50             /*!
    51             * \brief get Layout
    52             *
    53             * \return the parent Layout
    54             */
    55             const Layout* getLayout(void) const;
     47  /*!
     48  * \brief get Layout
     49  *
     50  * \return the parent Layout
     51  */
     52  const Layout *getLayout(void) const;
    5653
    57             /*!
    58             * \brief get row
    59             *
    60             * \return row position
    61             */
    62             int16_t Row(void) const;
     54  /*!
     55  * \brief get row
     56  *
     57  * \return row position
     58  */
     59  int16_t Row(void) const;
    6360
    64             /*!
    65             * \brief get col
    66             *
    67             * \return col position
    68             */
    69             int16_t Col(void) const;
     61  /*!
     62  * \brief get col
     63  *
     64  * \return col position
     65  */
     66  int16_t Col(void) const;
    7067
    71 
    72         private:
    73             const Layout *layout;
    74             int16_t row,col;
    75 
    76     };
     68private:
     69  const Layout *layout;
     70  int16_t row, col;
     71};
    7772
    7873} // end namespace gui

  • trunk/lib/FlairCore/src/Map.cpp

    r2 r15  
    2727using std::ostringstream;
    2828
    29 namespace flair
    30 {
    31 namespace gui
    32 {
     29namespace flair {
     30namespace gui {
    3331
    3432using namespace core;
    3533
    36 Map::Map(const LayoutPosition* position,string name):SendData(position,name,"Map",1000) {
    37     size_t i=0;
    38     while(1) {
    39         double latitude,longitude;
    40         double altitude=0;
    41         ostringstream lat_prop,long_prop,alt_prop;
    42         lat_prop << "lat" << i;
    43         long_prop << "long" << i;
    44         alt_prop << "alt" << i;
    45         if(GetPersistentXmlProp(lat_prop.str(),latitude) && GetPersistentXmlProp(long_prop.str(),longitude)) {
    46             SetVolatileXmlProp(lat_prop.str(),latitude);
    47             SetVolatileXmlProp(long_prop.str(),longitude);
    48             if(GetPersistentXmlProp(alt_prop.str(),altitude)) SetVolatileXmlProp(alt_prop.str(),altitude);
    49             GeoCoordinate *checkpoint=new GeoCoordinate(this,"checkpoint",latitude,longitude,altitude);
    50             checkpoints.push_back(checkpoint);
    51         } else {
    52             break;
    53         }
    54         i++;
     34Map::Map(const LayoutPosition *position, string name)
     35    : SendData(position, name, "Map", 1000) {
     36  size_t i = 0;
     37  while (1) {
     38    double latitude, longitude;
     39    double altitude = 0;
     40    ostringstream lat_prop, long_prop, alt_prop;
     41    lat_prop << "lat" << i;
     42    long_prop << "long" << i;
     43    alt_prop << "alt" << i;
     44    if (GetPersistentXmlProp(lat_prop.str(), latitude) &&
     45        GetPersistentXmlProp(long_prop.str(), longitude)) {
     46      SetVolatileXmlProp(lat_prop.str(), latitude);
     47      SetVolatileXmlProp(long_prop.str(), longitude);
     48      if (GetPersistentXmlProp(alt_prop.str(), altitude))
     49        SetVolatileXmlProp(alt_prop.str(), altitude);
     50      GeoCoordinate *checkpoint =
     51          new GeoCoordinate(this, "checkpoint", latitude, longitude, altitude);
     52      checkpoints.push_back(checkpoint);
     53    } else {
     54      break;
    5555    }
    56     for(size_t i=0;i<checkpoints.size();i++) {
    57         double latitude,longitude,altitude;
    58         checkpoints.at(i)->GetCoordinates(&latitude,&longitude,&altitude);
    59         //printf("%i %f %f\n",i,latitude,longitude);
    60     }
     56    i++;
     57  }
     58  for (size_t i = 0; i < checkpoints.size(); i++) {
     59    double latitude, longitude, altitude;
     60    checkpoints.at(i)->GetCoordinates(&latitude, &longitude, &altitude);
     61    // printf("%i %f %f\n",i,latitude,longitude);
     62  }
    6163
    62     SendXml();
    63 /*
    64     //update value from xml file
    65     XmlEvent(XmlFileNode());
    66     if(checkpoints_node.size()!=0) SendXml();//pour les checkpoints
     64  SendXml();
     65  /*
     66      //update value from xml file
     67      XmlEvent(XmlFileNode());
     68      if(checkpoints_node.size()!=0) SendXml();//pour les checkpoints
    6769
    68     //on enleve les checkpoints du xml
    69     for(size_t i=0;i<checkpoints_node.size();i++)
    70     {
    71         xmlUnlinkNode(checkpoints_node.at(i));
    72         xmlFreeNode(checkpoints_node.at(i));
    73     }*/
     70      //on enleve les checkpoints du xml
     71      for(size_t i=0;i<checkpoints_node.size();i++)
     72      {
     73          xmlUnlinkNode(checkpoints_node.at(i));
     74          xmlFreeNode(checkpoints_node.at(i));
     75      }*/
    7476}
    7577
    76 Map::~Map() {
    77 
    78 }
     78Map::~Map() {}
    7979
    8080void Map::ExtraXmlEvent(void) {
    8181
    82 //attention pas rt safe (creation checkpoint)
    83     size_t i=0;
    84     while(1) {
    85         //printf("test %i\n",i);
    86         double latitude,longitude;
    87         double altitude=0;
    88         ostringstream lat_prop,long_prop,alt_prop;
    89         lat_prop << "lat" << i;
    90         long_prop << "long" << i;
    91         alt_prop << "alt" << i;
    92         if(GetPersistentXmlProp(lat_prop.str(),latitude) && GetPersistentXmlProp(long_prop.str(),longitude)) {
    93             GetPersistentXmlProp(alt_prop.str(),altitude);
    94             if(i>=checkpoints.size()) {
    95                 GeoCoordinate *checkpoint=new GeoCoordinate(this,"checkpoint",latitude,longitude,altitude);
    96                 checkpoints.push_back(checkpoint);
    97                 //printf("add %i\n",i);
    98             } else {
    99                 checkpoints.at(i)->SetCoordinates(latitude,longitude,altitude);
    100             }
    101         } else {
    102             if(i==checkpoints.size()) break;
    103         }
    104         i++;
     82  // attention pas rt safe (creation checkpoint)
     83  size_t i = 0;
     84  while (1) {
     85    // printf("test %i\n",i);
     86    double latitude, longitude;
     87    double altitude = 0;
     88    ostringstream lat_prop, long_prop, alt_prop;
     89    lat_prop << "lat" << i;
     90    long_prop << "long" << i;
     91    alt_prop << "alt" << i;
     92    if (GetPersistentXmlProp(lat_prop.str(), latitude) &&
     93        GetPersistentXmlProp(long_prop.str(), longitude)) {
     94      GetPersistentXmlProp(alt_prop.str(), altitude);
     95      if (i >= checkpoints.size()) {
     96        GeoCoordinate *checkpoint = new GeoCoordinate(
     97            this, "checkpoint", latitude, longitude, altitude);
     98        checkpoints.push_back(checkpoint);
     99        // printf("add %i\n",i);
     100      } else {
     101        checkpoints.at(i)->SetCoordinates(latitude, longitude, altitude);
     102      }
     103    } else {
     104      if (i == checkpoints.size())
     105        break;
    105106    }
     107    i++;
     108  }
    106109
    107     for(size_t i=0;i<checkpoints.size();i++) {
    108         double latitude,longitude,altitude;
    109         checkpoints.at(i)->GetCoordinates(&latitude,&longitude,&altitude);
    110         //printf("%i %f %f\n",i,latitude,longitude);
    111     }
     110  for (size_t i = 0; i < checkpoints.size(); i++) {
     111    double latitude, longitude, altitude;
     112    checkpoints.at(i)->GetCoordinates(&latitude, &longitude, &altitude);
     113    // printf("%i %f %f\n",i,latitude,longitude);
     114  }
    112115}
    113116
    114 void Map::AddPoint(const GeoCoordinate* point,string name) {
    115     SetVolatileXmlProp("point",name);
    116     SendXml();
     117void Map::AddPoint(const GeoCoordinate *point, string name) {
     118  SetVolatileXmlProp("point", name);
     119  SendXml();
    117120
    118     to_draw.push_back(point);
    119     SetSendSize(to_draw.size()*3*sizeof(double));//lat,long,alt
     121  to_draw.push_back(point);
     122  SetSendSize(to_draw.size() * 3 * sizeof(double)); // lat,long,alt
    120123}
    121124
    122 void Map::CopyDatas(char* buf) const {
    123     for(size_t i=0;i<to_draw.size();i++) {
    124         double latitude,longitude,altitude;
    125         to_draw.at(i)->GetCoordinates(&latitude,&longitude,&altitude);
    126         memcpy(buf+i*3*sizeof(double),&latitude,sizeof(double));
    127         memcpy(buf+sizeof(double)+i*3*sizeof(double),&longitude,sizeof(double));
    128         memcpy(buf+2*sizeof(double)+i*3*sizeof(double),&altitude,sizeof(double));
    129     }
     125void Map::CopyDatas(char *buf) const {
     126  for (size_t i = 0; i < to_draw.size(); i++) {
     127    double latitude, longitude, altitude;
     128    to_draw.at(i)->GetCoordinates(&latitude, &longitude, &altitude);
     129    memcpy(buf + i * 3 * sizeof(double), &latitude, sizeof(double));
     130    memcpy(buf + sizeof(double) + i * 3 * sizeof(double), &longitude,
     131           sizeof(double));
     132    memcpy(buf + 2 * sizeof(double) + i * 3 * sizeof(double), &altitude,
     133           sizeof(double));
     134  }
    130135}
    131136

  • trunk/lib/FlairCore/src/Map.h

    r2 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21 namespace core
    22 {
    23     class GeoCoordinate;
     19namespace flair {
     20namespace core {
     21class GeoCoordinate;
    2422}
    2523
    26 namespace gui
    27 {
     24namespace gui {
    2825
    29     class LayoutPosition;
     26class LayoutPosition;
    3027
    31     /*! \class Map
    32     *
    33     * \brief Class displaying a GPS map on the ground station
    34     *
    35     */
    36     class Map: public SendData
    37     {
    38         public:
    39             /*!
    40             * \brief Constructor
    41             *
    42             * Construct a map at given position. \n
    43             * The Map will automatically be child of position->getLayout() Layout. After calling this constructor,
    44             * position will be deleted as it is no longer usefull.
    45             *
    46             * \param position position to draw map
    47             * \param name name
    48             */
    49             Map(const LayoutPosition* position,std::string name);
     28/*! \class Map
     29*
     30* \brief Class displaying a GPS map on the ground station
     31*
     32*/
     33class Map : public SendData {
     34public:
     35  /*!
     36  * \brief Constructor
     37  *
     38  * Construct a map at given position. \n
     39  * The Map will automatically be child of position->getLayout() Layout. After
     40  *calling this constructor,
     41  * position will be deleted as it is no longer usefull.
     42  *
     43  * \param position position to draw map
     44  * \param name name
     45  */
     46  Map(const LayoutPosition *position, std::string name);
    5047
    51             /*!
    52             * \brief Destructor
    53             *
    54             */
    55             ~Map();
     48  /*!
     49  * \brief Destructor
     50  *
     51  */
     52  ~Map();
    5653
    57             /*!
    58             * \brief Add a point to the map
    59             *
    60             * \param point point to draw
    61             * \param name name
    62             */
    63             void AddPoint(const core::GeoCoordinate* point,std::string name="");
     54  /*!
     55  * \brief Add a point to the map
     56  *
     57  * \param point point to draw
     58  * \param name name
     59  */
     60  void AddPoint(const core::GeoCoordinate *point, std::string name = "");
    6461
    65             /*!
    66             * \brief Copy datas to specified buffer
    67             *
    68             * Reimplemented from SendData.
    69             *
    70             * \param buf output buffer
    71             */
    72             void CopyDatas(char* buf) const;
     62  /*!
     63  * \brief Copy datas to specified buffer
     64  *
     65  * Reimplemented from SendData.
     66  *
     67  * \param buf output buffer
     68  */
     69  void CopyDatas(char *buf) const;
    7370
    74         private:
    75             /*!
    76             * \brief Extra Xml event
    77             *
    78             * Reimplemented from SendData.
    79             */
    80             void ExtraXmlEvent(void);
     71private:
     72  /*!
     73  * \brief Extra Xml event
     74  *
     75  * Reimplemented from SendData.
     76  */
     77  void ExtraXmlEvent(void);
    8178
    82             std::vector<core::GeoCoordinate*> checkpoints;
    83             std::vector<const core::GeoCoordinate*> to_draw;
    84             //std::vector<xmlNodePtr> checkpoints_node;
    85             //bool init;
    86     };
     79  std::vector<core::GeoCoordinate *> checkpoints;
     80  std::vector<const core::GeoCoordinate *> to_draw;
     81  // std::vector<xmlNodePtr> checkpoints_node;
     82  // bool init;
     83};
    8784
    8885} // end namespace gui

  • trunk/lib/FlairCore/src/Mutex.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair
    24 {
    25 namespace core
    26 {
     23namespace flair {
     24namespace core {
    2725
    28 Mutex::Mutex(const Object *parent,string name): Object(parent, name,"mutex")
    29 {
    30     pimpl_=new Mutex_impl(this);
     26Mutex::Mutex(const Object *parent, string name)
     27    : Object(parent, name, "mutex") {
     28  pimpl_ = new Mutex_impl(this);
    3129}
    3230
    33 Mutex::~Mutex()
    34 {
    35     delete pimpl_;
    36 }
     31Mutex::~Mutex() { delete pimpl_; }
    3732
    38 void Mutex::GetMutex(void) const
    39 {
    40     pimpl_->GetMutex();
    41 }
     33void Mutex::GetMutex(void) const { pimpl_->GetMutex(); }
    4234
    43 void Mutex::ReleaseMutex(void) const
    44 {
    45     pimpl_->ReleaseMutex();
    46 }
     35void Mutex::ReleaseMutex(void) const { pimpl_->ReleaseMutex(); }
    4736
    4837/*

  • trunk/lib/FlairCore/src/Mutex.h

    r2 r15  
    1919class ConditionVariable_impl;
    2020
    21 namespace flair
    22 {
    23 namespace core
    24 {
     21namespace flair {
     22namespace core {
    2523
    26     /*! \class Mutex
    27     *
    28     * \brief Class defining a mutex
    29     *
    30     */
    31     class Mutex: public Object
    32     {
    33         friend class ::ConditionVariable_impl;
     24/*! \class Mutex
     25*
     26* \brief Class defining a mutex
     27*
     28*/
     29class Mutex : public Object {
     30  friend class ::ConditionVariable_impl;
    3431
    35         public:
    36             /*!
    37             * \brief Constructor
    38             *
    39             * Construct a mutex.
    40             *
    41             * \param parent parent
    42             * \param name name
    43             */
    44             Mutex(const Object *parent,std::string name="");
     32public:
     33  /*!
     34  * \brief Constructor
     35  *
     36  * Construct a mutex.
     37  *
     38  * \param parent parent
     39  * \param name name
     40  */
     41  Mutex(const Object *parent, std::string name = "");
    4542
    46             /*!
    47             * \brief Destructor
    48             *
    49             */
    50             ~Mutex();
     43  /*!
     44  * \brief Destructor
     45  *
     46  */
     47  ~Mutex();
    5148
    52             /*!
    53             * \brief GetMutex
    54             *
    55             * Lock the mutex.
    56             *
    57             */
    58             void GetMutex(void) const;
     49  /*!
     50  * \brief GetMutex
     51  *
     52  * Lock the mutex.
     53  *
     54  */
     55  void GetMutex(void) const;
    5956
    60             /*!
    61             * \brief ReleaseMutex
    62             *
    63             * Unlock the mutex.
    64             *
    65             */
    66             void ReleaseMutex(void) const;
     57  /*!
     58  * \brief ReleaseMutex
     59  *
     60  * Unlock the mutex.
     61  *
     62  */
     63  void ReleaseMutex(void) const;
    6764
    68         private:
    69             class Mutex_impl* pimpl_;
    70     };
     65private:
     66  class Mutex_impl *pimpl_;
     67};
    7168
    7269} // end namespace core

  • trunk/lib/FlairCore/src/Mutex_impl.cpp

    r2 r15  
    2323using namespace flair::core;
    2424
    25 Mutex_impl::Mutex_impl(Mutex *self)
    26 {
    27     this->self=self;
     25Mutex_impl::Mutex_impl(Mutex *self) {
     26  this->self = self;
    2827#ifdef __XENO__
    29     int status=rt_mutex_create(&mutex,NULL);
    30     if(status!=0) self->Err("rt_mutex_create error (%s)\n",strerror(-status));
     28  int status = rt_mutex_create(&mutex, NULL);
     29  if (status != 0)
     30    self->Err("rt_mutex_create error (%s)\n", strerror(-status));
    3131#else
    32     //flag_locked=false;//revoir l'implementation nrt du is_locked
    33     pthread_mutexattr_t attr;
    34     if(pthread_mutexattr_init(&attr)!=0)
    35     {
    36         self->Err("pthread_mutexattr_init error\n");
    37     }
    38     if(pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE) != 0)
    39     {
    40         self->Err("pthread_mutexattr_settype error\n");
    41     }
    42     if(pthread_mutex_init(&mutex, &attr)!=0)
    43     {
    44         self->Err("pthread_mutex_init error\n");
    45     }
    46     if(pthread_mutexattr_destroy(&attr)!=0)
    47     {
    48         self->Err("pthread_mutexattr_destroy error\n");
    49     }
     32  // flag_locked=false;//revoir l'implementation nrt du is_locked
     33  pthread_mutexattr_t attr;
     34  if (pthread_mutexattr_init(&attr) != 0) {
     35    self->Err("pthread_mutexattr_init error\n");
     36  }
     37  if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE) != 0) {
     38    self->Err("pthread_mutexattr_settype error\n");
     39  }
     40  if (pthread_mutex_init(&mutex, &attr) != 0) {
     41    self->Err("pthread_mutex_init error\n");
     42  }
     43  if (pthread_mutexattr_destroy(&attr) != 0) {
     44    self->Err("pthread_mutexattr_destroy error\n");
     45  }
    5046#endif
    5147}
    5248
    53 Mutex_impl::~Mutex_impl()
    54 {
    55     int status;
     49Mutex_impl::~Mutex_impl() {
     50  int status;
    5651#ifdef __XENO__
    57     status=rt_mutex_delete(&mutex);
     52  status = rt_mutex_delete(&mutex);
    5853#else
    59     status=pthread_mutex_destroy(&mutex);
     54  status = pthread_mutex_destroy(&mutex);
    6055#endif
    61     if(status!=0) self->Err("error destroying mutex (%s)\n",strerror(-status));
     56  if (status != 0)
     57    self->Err("error destroying mutex (%s)\n", strerror(-status));
    6258}
    6359
    64 void Mutex_impl::GetMutex(void)
    65 {
    66     int status;
     60void Mutex_impl::GetMutex(void) {
     61  int status;
    6762#ifdef __XENO__
    68     status=rt_mutex_acquire(&mutex,TM_INFINITE);
     63  status = rt_mutex_acquire(&mutex, TM_INFINITE);
    6964#else
    70     //flag_locked=true;
    71     status=pthread_mutex_lock(&mutex);
     65  // flag_locked=true;
     66  status = pthread_mutex_lock(&mutex);
    7267#endif
    73     if(status!=0) self->Err("error (%s)\n",strerror(-status));
     68  if (status != 0)
     69    self->Err("error (%s)\n", strerror(-status));
    7470}
    7571
    76 void Mutex_impl::ReleaseMutex(void)
    77 {
    78     int status;
     72void Mutex_impl::ReleaseMutex(void) {
     73  int status;
    7974#ifdef __XENO__
    80     status=rt_mutex_release(&mutex);
     75  status = rt_mutex_release(&mutex);
    8176#else
    82     status=pthread_mutex_unlock(&mutex);
    83     //flag_locked=false;
     77  status = pthread_mutex_unlock(&mutex);
     78// flag_locked=false;
    8479#endif
    85     if(status!=0) self->Err("error (%s)\n",strerror(-status));
     80  if (status != 0)
     81    self->Err("error (%s)\n", strerror(-status));
    8682}
    8783
    … …  
    9288    RT_MUTEX_INFO info;
    9389    int status=rt_mutex_inquire(&mutex_rt,&info);
    94     if(status!=0) mutex->Err("erreur rt_mutex_inquire (%s)\n",strerror(-status));
     90    if(status!=0) mutex->Err("erreur rt_mutex_inquire
     91(%s)\n",strerror(-status));
    9592    if(info.locked>0) return true;
    9693    return false;

  • trunk/lib/FlairCore/src/Object.cpp

    r2 r15  
    3333using std::vector;
    3434
    35 namespace flair
    36 {
    37 namespace core
    38 {
     35namespace flair {
     36namespace core {
    3937
    40 Time GetTime(void)
    41 {
     38Time GetTime(void) {
    4239#ifdef __XENO__
    43     return rt_timer_read();
     40  return rt_timer_read();
    4441#else
    45     struct timeval t;
    46     gettimeofday(&t, NULL);
    47     return (Time)((Time)(t.tv_sec)*1000000 + (Time)(t.tv_usec))*1000;
     42  struct timeval t;
     43  gettimeofday(&t, NULL);
     44  return (Time)((Time)(t.tv_sec) * 1000000 + (Time)(t.tv_usec)) * 1000;
    4845
    4946#endif
    5047}
    5148
    52 void Printf(const char *format, ...)
    53 {
    54     va_list args;
    55     va_start(args, format);
     49void Printf(const char *format, ...) {
     50  va_list args;
     51  va_start(args, format);
    5652#ifdef __XENO__
    57     if(rt_task_self()!=NULL)
    58     {
    59         rt_vfprintf(stderr,format, args);
    60     }
    61     else
     53  if (rt_task_self() != NULL) {
     54    rt_vfprintf(stderr, format, args);
     55  } else
    6256#endif
    63     {
    64         vfprintf(stderr,format, args);
    65     }
     57  {
     58    vfprintf(stderr, format, args);
     59  }
    6660
    67     va_end (args);
     61  va_end(args);
    6862}
    6963
    70 Object::Object(const Object* parent,string name,string type)
    71 {
    72     pimpl_=new Object_impl(this,parent,name,type);
    73     if(parent!=NULL) parent->pimpl_->AddChild(this);
     64Object::Object(const Object *parent, string name, string type) {
     65  pimpl_ = new Object_impl(this, parent, name, type);
     66  if (parent != NULL)
     67    parent->pimpl_->AddChild(this);
    7468}
    7569
    76 Object::~Object()
    77 {
    78     if(pimpl_->parent!=NULL) pimpl_->parent->pimpl_->RemoveChild(this);
    79     delete pimpl_;
     70Object::~Object() {
     71  if (pimpl_->parent != NULL)
     72    pimpl_->parent->pimpl_->RemoveChild(this);
     73  delete pimpl_;
    8074}
    8175
    82 string Object::ObjectName(void) const
    83 {
    84     return pimpl_->name;
     76string Object::ObjectName(void) const { return pimpl_->name; }
     77
     78string Object::ObjectType(void) const { return pimpl_->type; }
     79
     80const Object *Object::Parent(void) const { return pimpl_->parent; }
     81
     82vector<const Object *> *Object::TypeChilds(void) const {
     83  return &(pimpl_->type_childs);
    8584}
    8685
    87 string Object::ObjectType(void) const
    88 {
    89     return pimpl_->type;
     86vector<const Object *> *Object::Childs(void) const { return &(pimpl_->childs); }
     87
     88void Object::ColorPrintf(color_t color, const char *function, int line,
     89                         const char *format, va_list *args) const {
     90#ifdef __XENO__
     91  if (rt_task_self() != NULL) {
     92    rt_fprintf(stderr, "\033[%dm", color);
     93    if (line) {
     94      rt_fprintf(stderr, "%s - line %d, %s: ", function, line,
     95                 pimpl_->name.c_str());
     96    } else {
     97      rt_fprintf(stderr, "%s, %s: ", function, pimpl_->name.c_str());
     98    }
     99    rt_vfprintf(stderr, format, *args);
     100    rt_fprintf(stderr, "\033[%dm", color_t::Auto);
     101  } else
     102#endif
     103  {
     104    fprintf(stderr, "\033[%dm", color);
     105    if (line) {
     106      fprintf(stderr, "%s - line %d, %s: ", function, line,
     107              pimpl_->name.c_str());
     108    } else {
     109      fprintf(stderr, "%s, %s: ", function, pimpl_->name.c_str());
     110    }
     111    vfprintf(stderr, format, *args);
     112    fprintf(stderr, "\033[%dm", color_t::Auto);
     113  }
    90114}
    91115
    92 const Object* Object::Parent(void) const
    93 {
    94     return pimpl_->parent;
    95 }
    96 
    97 vector<const Object*>* Object::TypeChilds(void) const
    98 {
    99     return &(pimpl_->type_childs);
    100 }
    101 
    102 vector<const Object*>* Object::Childs(void) const
    103 {
    104     return &(pimpl_->childs);
    105 }
    106 
    107 void Object::ColorPrintf(color_t color, const char *function, int line, const char *format,va_list *args) const {
    108 #ifdef __XENO__
    109     if(rt_task_self()!=NULL)
    110     {
    111         rt_fprintf(stderr,"\033[%dm", color);
    112         if (line) {
    113             rt_fprintf(stderr,"%s - line %d, %s: ", function, line, pimpl_->name.c_str());
    114         } else {
    115             rt_fprintf(stderr,"%s, %s: ", function, pimpl_->name.c_str());
    116         }
    117         rt_vfprintf(stderr,format, *args);
    118         rt_fprintf(stderr,"\033[%dm", color_t::Auto);
    119     }
    120     else
    121 #endif
    122     {
    123         fprintf(stderr,"\033[%dm", color);
    124         if (line) {
    125             fprintf(stderr,"%s - line %d, %s: ", function, line, pimpl_->name.c_str());
    126         } else {
    127             fprintf(stderr,"%s, %s: ", function, pimpl_->name.c_str());
    128         }
    129         vfprintf(stderr,format, *args);
    130         fprintf(stderr,"\033[%dm", color_t::Auto);
    131     }
    132 }
    133 
    134 void Object::Information(const char *function, int line, const char *format, ...) const {
    135     va_list args;
    136     va_start(args, format);
    137     ColorPrintf(color_t::Green, function, line, format, &args);
    138     va_end (args);
     116void Object::Information(const char *function, int line, const char *format,
     117                         ...) const {
     118  va_list args;
     119  va_start(args, format);
     120  ColorPrintf(color_t::Green, function, line, format, &args);
     121  va_end(args);
    139122}
    140123
    141124void Object::Warning(const char *function, const char *format, ...) const {
    142     va_list args;
    143     va_start(args, format);
    144     ColorPrintf(color_t::Orange, function, 0, format, &args);
    145     va_end (args);
     125  va_list args;
     126  va_start(args, format);
     127  ColorPrintf(color_t::Orange, function, 0, format, &args);
     128  va_end(args);
    146129}
    147130
    148131void Object::Error(const char *function, const char *format, ...) const {
    149     va_list args;
    150     va_start(args, format);
    151     ColorPrintf(color_t::Red, function, 0, format, &args);
    152     va_end (args);
     132  va_list args;
     133  va_start(args, format);
     134  ColorPrintf(color_t::Red, function, 0, format, &args);
     135  va_end(args);
    153136
    154     pimpl_->error_occured=true;
     137  pimpl_->error_occured = true;
    155138}
    156139
    157 bool Object::ErrorOccured(bool recursive) const
    158 {
    159     return pimpl_->ErrorOccured(recursive);
     140bool Object::ErrorOccured(bool recursive) const {
     141  return pimpl_->ErrorOccured(recursive);
    160142}
    161143

  • trunk/lib/FlairCore/src/Object.h

    r2 r15  
    1818#include <stdarg.h>
    1919
    20 #define Warn(...) Warning(__PRETTY_FUNCTION__,__VA_ARGS__)
    21 #define Err(...) Error(__PRETTY_FUNCTION__,__VA_ARGS__)
    22 #define Info(...) Information(__PRETTY_FUNCTION__,__LINE__,__VA_ARGS__)
     20#define Warn(...) Warning(__PRETTY_FUNCTION__, __VA_ARGS__)
     21#define Err(...) Error(__PRETTY_FUNCTION__, __VA_ARGS__)
     22#define Info(...) Information(__PRETTY_FUNCTION__, __LINE__, __VA_ARGS__)
    2323
    2424#define TIME_INFINITE 0
    … …  
    2828class Widget_impl;
    2929
    30 namespace flair
    31 {
    32 namespace core
    33 {
     30namespace flair {
     31namespace core {
    3432
    35     class FrameworkManager;
     33class FrameworkManager;
    3634
    37     class Message {
    38     public:
    39         Message(unsigned int myBufferSize):bufferSize(myBufferSize) {
    40             buffer=new char[bufferSize];
    41         }
    42         ~Message() {
    43             delete buffer;
    44         }
    45         char *buffer;
    46         size_t bufferSize;
    47     };
     35class Message {
     36public:
     37  Message(unsigned int myBufferSize) : bufferSize(myBufferSize) {
     38    buffer = new char[bufferSize];
     39  }
     40  ~Message() { delete buffer; }
     41  char *buffer;
     42  size_t bufferSize;
     43};
    4844
    49     /*!
    50     * \brief Time definition, in ns
    51     *
    52     */
    53     typedef unsigned long long Time;
     45/*!
     46* \brief Time definition, in ns
     47*
     48*/
     49typedef unsigned long long Time;
    5450
    55     /*!
    56     * \brief Time
    57     *
    58     * \return actual time in ns (origin depends on whether the method is compiled in hard real time mode or not. As a conquence, only time differences should be used)
    59     */
    60     Time GetTime(void);
     51/*!
     52* \brief Time
     53*
     54* \return actual time in ns (origin depends on whether the method is compiled in
     55*hard real time mode or not. As a conquence, only time differences should be
     56*used)
     57*/
     58Time GetTime(void);
    6159
    62     /*!
    63     * \brief Formatted print
    64     *
    65     * See standard printf for syntax.
    66     *
    67     * \param format text string to display
    68     */
    69     void Printf(const char *format, ...);
     60/*!
     61* \brief Formatted print
     62*
     63* See standard printf for syntax.
     64*
     65* \param format text string to display
     66*/
     67void Printf(const char *format, ...);
    7068
    71     /*! \class Object
    72     *
    73     * \brief Base class for all Framework's classes
    74     *
    75     * This is the base class for all other classes. \n
    76     * It handles parent/child links and thus allow auto destruction of childs.
    77     *
    78     */
    79     class Object
    80     {
    81         friend class ::Widget_impl;
     69/*! \class Object
     70*
     71* \brief Base class for all Framework's classes
     72*
     73* This is the base class for all other classes. \n
     74* It handles parent/child links and thus allow auto destruction of childs.
     75*
     76*/
     77class Object {
     78  friend class ::Widget_impl;
    8279
    83         public:
    84             typedef enum {
    85                 Auto=0,
    86                 Red=31,
    87                 Green=32,
    88                 Orange=33
    89             } color_t;
    90             /*!
    91             * \brief Constructor
    92             *
    93             * Construct an Object, which is child of its parent.
    94             *
    95             * \param parent parent
    96             * \param name name
    97             * \param type type
    98             */
    99             Object(const Object* parent=NULL,std::string name="",std::string type="");
     80public:
     81  typedef enum { Auto = 0, Red = 31, Green = 32, Orange = 33 } color_t;
     82  /*!
     83  * \brief Constructor
     84  *
     85  * Construct an Object, which is child of its parent.
     86  *
     87  * \param parent parent
     88  * \param name name
     89  * \param type type
     90  */
     91  Object(const Object *parent = NULL, std::string name = "",
     92         std::string type = "");
    10093
    101             /*!
    102             * \brief Destructor
    103             *
    104             * Calling it will automatically destruct all childs.
    105             *
    106             */
    107             virtual ~Object();
     94  /*!
     95  * \brief Destructor
     96  *
     97  * Calling it will automatically destruct all childs.
     98  *
     99  */
     100  virtual ~Object();
    108101
    109             /*!
    110             * \brief Name
    111             *
    112             * \return Object's name
    113             */
    114             std::string ObjectName(void) const;
     102  /*!
     103  * \brief Name
     104  *
     105  * \return Object's name
     106  */
     107  std::string ObjectName(void) const;
    115108
    116             /*!
    117             * \brief Type
    118             *
    119             * \return Object's type
    120             */
    121             std::string ObjectType(void) const;
     109  /*!
     110  * \brief Type
     111  *
     112  * \return Object's type
     113  */
     114  std::string ObjectType(void) const;
    122115
    123             /*!
    124             * \brief Parent
    125             *
    126             * \return Object's parent
    127             */
    128             const Object* Parent(void) const;
     116  /*!
     117  * \brief Parent
     118  *
     119  * \return Object's parent
     120  */
     121  const Object *Parent(void) const;
    129122
    130             /*!
    131             * \brief Childs of the same type
    132             *
    133             * \return a vector of all childs of the same type
    134             */
    135             std::vector<const Object*>* TypeChilds(void) const;
     123  /*!
     124  * \brief Childs of the same type
     125  *
     126  * \return a vector of all childs of the same type
     127  */
     128  std::vector<const Object *> *TypeChilds(void) const;
    136129
    137             /*!
    138             * \brief Childs
    139             *
    140             * \return a vector of all childs
    141             */
    142             std::vector<const Object*>* Childs(void) const;
     130  /*!
     131  * \brief Childs
     132  *
     133  * \return a vector of all childs
     134  */
     135  std::vector<const Object *> *Childs(void) const;
    143136
    144             /*!
    145             * \brief Formatted information
    146             *
    147             * Green colored Printf(). \n
    148             * Note that it is better to call Info macro, which automatically fills function parameter.
    149             *
    150             * \param function name of calling function
    151             * \param line line number in calling function
    152             * \param format text string to display
    153             */
    154             void Information(const char *function, int line, const char *format, ...) const;
     137  /*!
     138  * \brief Formatted information
     139  *
     140  * Green colored Printf(). \n
     141  * Note that it is better to call Info macro, which automatically fills
     142  *function parameter.
     143  *
     144  * \param function name of calling function
     145  * \param line line number in calling function
     146  * \param format text string to display
     147  */
     148  void Information(const char *function, int line, const char *format,
     149                   ...) const;
    155150
    156             /*!
    157             * \brief Formatted warning
    158             *
    159             * Orange colored Printf(). \n
    160             * Note that it is better to call Warn macro, which automatically fills function parameter.
    161             *
    162             * \param function name of calling function
    163             * \param format text string to display
    164             */
    165             void Warning(const char *function,const char *format, ...) const;
     151  /*!
     152  * \brief Formatted warning
     153  *
     154  * Orange colored Printf(). \n
     155  * Note that it is better to call Warn macro, which automatically fills
     156  *function parameter.
     157  *
     158  * \param function name of calling function
     159  * \param format text string to display
     160  */
     161  void Warning(const char *function, const char *format, ...) const;
    166162
    167             /*!
    168             * \brief Formatted error
    169             *
    170             * Red colored Printf(). \n
    171             * Note that it is better to call Err macro, which automatically fills function parameter. \n
    172             * After calling this method, ErrorOccured() will always return true.
    173             *
    174             * \param function name of calling function
    175             * \param format text string to display
    176             */
    177             void Error(const char *function,const char *format, ...) const;
     163  /*!
     164  * \brief Formatted error
     165  *
     166  * Red colored Printf(). \n
     167  * Note that it is better to call Err macro, which automatically fills function
     168  *parameter. \n
     169  * After calling this method, ErrorOccured() will always return true.
     170  *
     171  * \param function name of calling function
     172  * \param format text string to display
     173  */
     174  void Error(const char *function, const char *format, ...) const;
    178175
    179             /*!
    180             * \brief Has an errror occured?
    181             *
    182             * Check if an error occured, in fact if Error() was called at least once. \n
    183             * Once Error() was called, this method will never return back false.
    184             *
    185             * \param recursive if true, recursively check among childs
    186             * \return true if an error occured
    187             */
    188             bool ErrorOccured(bool recursive=true) const;
     176  /*!
     177  * \brief Has an errror occured?
     178  *
     179  * Check if an error occured, in fact if Error() was called at least once. \n
     180  * Once Error() was called, this method will never return back false.
     181  *
     182  * \param recursive if true, recursively check among childs
     183  * \return true if an error occured
     184  */
     185  bool ErrorOccured(bool recursive = true) const;
    189186
    190         private:
    191             class Object_impl* pimpl_;
    192             void ColorPrintf(color_t, const char *function, int line, const char *format, va_list *args) const;
    193     };
     187private:
     188  class Object_impl *pimpl_;
     189  void ColorPrintf(color_t, const char *function, int line, const char *format,
     190                   va_list *args) const;
     191};
    194192
    195193} // end namespace core

  • trunk/lib/FlairCore/src/Object_impl.cpp

    r2 r15  
    2323using namespace flair::core;
    2424
    25 Object_impl::Object_impl(const Object* self,const Object* parent,string name,string type)
    26 {
    27     //Printf("Object %s\n",name.c_str());
    28     this->self=self;
    29     this->parent=parent;
    30     this->name=name;
    31     this->type=type;
    32     error_occured=false;
     25Object_impl::Object_impl(const Object *self, const Object *parent, string name,
     26                         string type) {
     27  // Printf("Object %s\n",name.c_str());
     28  this->self = self;
     29  this->parent = parent;
     30  this->name = name;
     31  this->type = type;
     32  error_occured = false;
    3333
    34     if(parent!=NULL)
    35     {
    36         if(name=="") this->name=parent->ObjectName();
    37     }
     34  if (parent != NULL) {
     35    if (name == "")
     36      this->name = parent->ObjectName();
     37  }
    3838}
    3939
    40 Object_impl::~Object_impl()
    41 {
    42     //Printf("destruction Object %s %s\n",name.c_str(),type.c_str());
     40Object_impl::~Object_impl() {
     41  // Printf("destruction Object %s %s\n",name.c_str(),type.c_str());
    4342
    44     while(childs.size()!=0)
    45     {
    46         //Printf("child %i %s %s\n",childs.size(),childs.front()->ObjectName().c_str(),childs.front()->ObjectType().c_str());
    47         //if(childs.front()!=NULL)
    48         delete childs.front();
    49     }
     43  while (childs.size() != 0) {
     44    // Printf("child %i %s
     45    // %s\n",childs.size(),childs.front()->ObjectName().c_str(),childs.front()->ObjectType().c_str());
     46    // if(childs.front()!=NULL)
     47    delete childs.front();
     48  }
    5049
    51     if(type_childs.size()!=0)
    52     {
    53         type_childs.clear();
    54         self->Warn("type_childs not cleared\n");
    55     }
     50  if (type_childs.size() != 0) {
     51    type_childs.clear();
     52    self->Warn("type_childs not cleared\n");
     53  }
    5654
    57     //Printf("destruction Object %s %s ok\n",name.c_str(),type.c_str());
     55  // Printf("destruction Object %s %s ok\n",name.c_str(),type.c_str());
    5856}
    5957
    60 void Object_impl::AddChild(const Object* child)
    61 {
    62     childs.push_back(child);
    63 //self->Printf("added Object %s %s (%s %s)\n",child->ObjectName().c_str(),child->ObjectType().c_str(),name.c_str(),type.c_str());
    64     if(child->ObjectType()==type) type_childs.push_back(child);
     58void Object_impl::AddChild(const Object *child) {
     59  childs.push_back(child);
     60  // self->Printf("added Object %s %s (%s
     61  // %s)\n",child->ObjectName().c_str(),child->ObjectType().c_str(),name.c_str(),type.c_str());
     62  if (child->ObjectType() == type)
     63    type_childs.push_back(child);
    6564}
    6665
    67 void Object_impl::RemoveChild(const Object* child)
    68 {
    69     //self->Printf("removed Object %s %s (%s %s)\n",child->ObjectName().c_str(),child->ObjectType().c_str(),name.c_str(),type.c_str());
     66void Object_impl::RemoveChild(const Object *child) {
     67  // self->Printf("removed Object %s %s (%s
     68  // %s)\n",child->ObjectName().c_str(),child->ObjectType().c_str(),name.c_str(),type.c_str());
    7069
    71     for(vector<const Object*>::iterator it=childs.begin() ; it < childs.end(); it++ )
    72     {
    73         if(*it==child)
    74         {
    75             childs.erase (it);
    76             break;
    77         }
     70  for (vector<const Object *>::iterator it = childs.begin(); it < childs.end();
     71       it++) {
     72    if (*it == child) {
     73      childs.erase(it);
     74      break;
    7875    }
     76  }
    7977
    80     for(vector<const Object*>::iterator it=type_childs.begin() ; it < type_childs.end(); it++ )
    81     {
    82         if(*it==child)
    83         {
    84             type_childs.erase (it);
    85             break;
    86         }
     78  for (vector<const Object *>::iterator it = type_childs.begin();
     79       it < type_childs.end(); it++) {
     80    if (*it == child) {
     81      type_childs.erase(it);
     82      break;
    8783    }
     84  }
    8885}
    8986
    90 bool Object_impl::ErrorOccured(bool recursive)
    91 {
    92     if(recursive==true)
    93     {
    94         for(size_t i=0;i<childs.size();i++)
    95         {
    96             if(childs[i]->ErrorOccured(true)==true)
    97             {
    98                 return true;
    99             }
    100         }
     87bool Object_impl::ErrorOccured(bool recursive) {
     88  if (recursive == true) {
     89    for (size_t i = 0; i < childs.size(); i++) {
     90      if (childs[i]->ErrorOccured(true) == true) {
     91        return true;
     92      }
    10193    }
    102     return error_occured;
     94  }
     95  return error_occured;
    10396}

  • trunk/lib/FlairCore/src/OneAxisRotation.cpp

    r2 r15  
    1616/*********************************************************************/
    1717
    18 //this "filter" is in core but it should be in filter
    19 //filter depends on sensoractuator
    20 //sensoractuators depends on oneaxisrotation
    21 //so if oneaxisrotation is in fiter we have a circular dependency
    22 //TODO: fix this!
     18// this "filter" is in core but it should be in filter
     19// filter depends on sensoractuator
     20// sensoractuators depends on oneaxisrotation
     21// so if oneaxisrotation is in fiter we have a circular dependency
     22// TODO: fix this!
    2323
    2424#include "OneAxisRotation.h"
    … …  
    2929using std::string;
    3030
    31 namespace flair
    32 {
    33 namespace core
    34 {
     31namespace flair {
     32namespace core {
    3533
    36 OneAxisRotation::OneAxisRotation(const gui::LayoutPosition* position,string name): gui::GroupBox(position,name)
    37 {
    38     pimpl_=new OneAxisRotation_impl(this);
     34OneAxisRotation::OneAxisRotation(const gui::LayoutPosition *position,
     35                                 string name)
     36    : gui::GroupBox(position, name) {
     37  pimpl_ = new OneAxisRotation_impl(this);
    3938}
    4039
    41 OneAxisRotation::~OneAxisRotation()
    42 {
    43     delete pimpl_;
     40OneAxisRotation::~OneAxisRotation() { delete pimpl_; }
     41
     42void OneAxisRotation::ComputeRotation(Vector3D &vector) const {
     43  pimpl_->ComputeRotation(vector);
    4444}
    4545
    46 void OneAxisRotation::ComputeRotation(Vector3D& vector) const
    47 {
    48     pimpl_->ComputeRotation(vector);
     46void OneAxisRotation::ComputeRotation(Euler &euler) const {
     47  pimpl_->ComputeRotation(euler);
    4948}
    5049
    51 void OneAxisRotation::ComputeRotation(Euler& euler) const
    52 {
    53     pimpl_->ComputeRotation(euler);
     50void OneAxisRotation::ComputeRotation(Quaternion &quaternion) const {
     51  pimpl_->ComputeRotation(quaternion);
    5452}
    5553
    56 void OneAxisRotation::ComputeRotation(Quaternion& quaternion) const {
    57     pimpl_->ComputeRotation(quaternion);
    58 }
    59 
    60 void OneAxisRotation::ComputeRotation(RotationMatrix& matrix) const {
    61     pimpl_->ComputeRotation(matrix);
     54void OneAxisRotation::ComputeRotation(RotationMatrix &matrix) const {
     55  pimpl_->ComputeRotation(matrix);
    6256}
    6357

  • trunk/lib/FlairCore/src/OneAxisRotation.h

    r2 r15  
    1818class OneAxisRotation_impl;
    1919
    20 
    21 namespace flair
    22 {
    23 namespace gui
    24 {
    25     class LayoutPosition;
     20namespace flair {
     21namespace gui {
     22class LayoutPosition;
    2623}
    2724
    28 namespace core
    29 {
    30     class Vector3D;
    31     class Euler;
    32     class Quaternion;
    33     class RotationMatrix;
     25namespace core {
     26class Vector3D;
     27class Euler;
     28class Quaternion;
     29class RotationMatrix;
    3430
    35     /*! \class OneAxisRotation
    36     *
    37     * \brief Class defining a rotation around one axis
    38     *
    39     * Axe and value of the rotation are placed in a GroupBox on ground station.
    40     *
    41     */
    42     class OneAxisRotation: public gui::GroupBox
    43     {
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct a OneAxisRotation at given position.
    49             *
    50             * \param position position to place the GroupBox
    51             * \param name name
    52             */
    53             OneAxisRotation(const gui::LayoutPosition* position,std::string namel);
     31/*! \class OneAxisRotation
     32*
     33* \brief Class defining a rotation around one axis
     34*
     35* Axe and value of the rotation are placed in a GroupBox on ground station.
     36*
     37*/
     38class OneAxisRotation : public gui::GroupBox {
     39public:
     40  /*!
     41  * \brief Constructor
     42  *
     43  * Construct a OneAxisRotation at given position.
     44  *
     45  * \param position position to place the GroupBox
     46  * \param name name
     47  */
     48  OneAxisRotation(const gui::LayoutPosition *position, std::string namel);
    5449
    55             /*!
    56             * \brief Destructor
    57             *
    58             */
    59             ~OneAxisRotation();
     50  /*!
     51  * \brief Destructor
     52  *
     53  */
     54  ~OneAxisRotation();
    6055
    61             /*!
    62             * \brief Compute rotation
    63             *
    64             * \param vector Vector3D to rotate
    65             */
    66             void ComputeRotation(core::Vector3D& vector) const;
     56  /*!
     57  * \brief Compute rotation
     58  *
     59  * \param vector Vector3D to rotate
     60  */
     61  void ComputeRotation(core::Vector3D &vector) const;
    6762
    68             /*!
    69             * \brief Compute rotation
    70             *
    71             * \param euler Euler angle to rotate
    72             */
    73             void ComputeRotation(core::Euler& euler) const;
     63  /*!
     64  * \brief Compute rotation
     65  *
     66  * \param euler Euler angle to rotate
     67  */
     68  void ComputeRotation(core::Euler &euler) const;
    7469
    75             /*!
    76             * \brief Compute rotation
    77             *
    78             * \param quaternion Quaternion to rotate
    79             */
    80             void ComputeRotation(core::Quaternion& quaternion) const;
     70  /*!
     71  * \brief Compute rotation
     72  *
     73  * \param quaternion Quaternion to rotate
     74  */
     75  void ComputeRotation(core::Quaternion &quaternion) const;
    8176
    82             /*!
    83             * \brief Compute rotation
    84             *
    85             * \param matrix RotationMatrix to rotate
    86             */
    87             void ComputeRotation(core::RotationMatrix& matrix) const;
     77  /*!
     78  * \brief Compute rotation
     79  *
     80  * \param matrix RotationMatrix to rotate
     81  */
     82  void ComputeRotation(core::RotationMatrix &matrix) const;
    8883
    89         private:
    90             const class OneAxisRotation_impl* pimpl_;
    91 
    92     };
     84private:
     85  const class OneAxisRotation_impl *pimpl_;
     86};
    9387
    9488} // end namespace core

  • trunk/lib/FlairCore/src/OneAxisRotation_impl.cpp

    r2 r15  
    2828using namespace flair::gui;
    2929
    30 OneAxisRotation_impl::OneAxisRotation_impl(GroupBox* box)
    31 {
    32     rot_value=new DoubleSpinBox(box->NewRow(),"value"," deg",-180.,180.,10.,1);
    33     rot_axe=new ComboBox(box->LastRowLastCol(),"axis");
    34     rot_axe->AddItem("x");
    35     rot_axe->AddItem("y");
    36     rot_axe->AddItem("z");
     30OneAxisRotation_impl::OneAxisRotation_impl(GroupBox *box) {
     31  rot_value =
     32      new DoubleSpinBox(box->NewRow(), "value", " deg", -180., 180., 10., 1);
     33  rot_axe = new ComboBox(box->LastRowLastCol(), "axis");
     34  rot_axe->AddItem("x");
     35  rot_axe->AddItem("y");
     36  rot_axe->AddItem("z");
    3737}
    3838
    39 OneAxisRotation_impl::~OneAxisRotation_impl()
    40 {
     39OneAxisRotation_impl::~OneAxisRotation_impl() {}
    4140
     41// compute rotation of each axis through ComputeRotation(Vector3D& vector)
     42void OneAxisRotation_impl::ComputeRotation(Quaternion &quat) const {
     43  Vector3D rot = Vector3D(quat.q1, quat.q2, quat.q3);
     44  ComputeRotation(rot);
     45  quat.q1 = rot.x;
     46  quat.q2 = rot.y;
     47  quat.q3 = rot.z;
    4248}
    4349
    44 //compute rotation of each axis through ComputeRotation(Vector3D& vector)
    45 void OneAxisRotation_impl::ComputeRotation(Quaternion& quat) const {
    46     Vector3D rot=Vector3D(quat.q1,quat.q2,quat.q3);
    47     ComputeRotation(rot);
    48     quat.q1=rot.x;
    49     quat.q2=rot.y;
    50     quat.q3=rot.z;
     50void OneAxisRotation_impl::ComputeRotation(RotationMatrix &matrix) const {
     51  printf("not yet implemented\n");
    5152}
    5253
    53 void OneAxisRotation_impl::ComputeRotation(RotationMatrix& matrix) const {
    54 printf("not yet implemented\n");
     54// on utilise la rotation d'un vector pour faire une rotation de repere
     55// d'ou le signe negatif
     56void OneAxisRotation_impl::ComputeRotation(Vector3D &vector) const {
     57  switch (rot_axe->CurrentIndex()) {
     58  case 0:
     59    vector.RotateXDeg(-rot_value->Value());
     60    break;
     61  case 1:
     62    vector.RotateYDeg(-rot_value->Value());
     63    break;
     64  case 2:
     65    vector.RotateZDeg(-rot_value->Value());
     66    break;
     67  }
    5568}
    5669
    57 //on utilise la rotation d'un vector pour faire une rotation de repere
    58 //d'ou le signe negatif
    59 void OneAxisRotation_impl::ComputeRotation(Vector3D& vector) const
    60 {
    61     switch(rot_axe->CurrentIndex())
    62     {
    63         case 0:
    64             vector.RotateXDeg(-rot_value->Value());
    65             break;
    66         case 1:
    67             vector.RotateYDeg(-rot_value->Value());
    68             break;
    69         case 2:
    70             vector.RotateZDeg(-rot_value->Value());
    71             break;
    72     }
     70void OneAxisRotation_impl::ComputeRotation(Euler &euler) const {
     71  Quaternion quat;
     72  euler.ToQuaternion(quat);
     73  ComputeRotation(quat);
     74  quat.ToEuler(euler);
    7375}
    74 
    75 void OneAxisRotation_impl::ComputeRotation(Euler& euler) const
    76 {
    77     Quaternion quat;
    78     euler.ToQuaternion(quat);
    79     ComputeRotation(quat);
    80     quat.ToEuler(euler);
    81 }
    82 

  • trunk/lib/FlairCore/src/Picture.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair
    27 {
    28 namespace gui
    29 {
     26namespace flair {
     27namespace gui {
    3028
    3129using namespace core;
    3230
    33 Picture::Picture(const LayoutPosition* position,string name,const cvimage *image):SendData(position,name,"Picture",200)
    34 {
    35     this->image=image;
     31Picture::Picture(const LayoutPosition *position, string name,
     32                 const cvimage *image)
     33    : SendData(position, name, "Picture", 200) {
     34  this->image = image;
    3635
    37     SetSendSize(image->GetDataType().GetSize());
     36  SetSendSize(image->GetDataType().GetSize());
    3837
    39     SetVolatileXmlProp("width",image->GetDataType().GetWidth());
    40     SetVolatileXmlProp("height",image->GetDataType().GetHeight());
    41     SendXml();
     38  SetVolatileXmlProp("width", image->GetDataType().GetWidth());
     39  SetVolatileXmlProp("height", image->GetDataType().GetHeight());
     40  SendXml();
    4241}
    4342
    44 Picture::~Picture()
    45 {
     43Picture::~Picture() {}
     44
     45void Picture::CopyDatas(char *buf) const {
     46  if (image != NULL) {
     47    image->GetMutex();
     48    memcpy(buf, image->img->imageData, image->GetDataType().GetSize());
     49    image->ReleaseMutex();
     50  }
    4651}
    47 
    48 
    49 void Picture::CopyDatas(char* buf) const
    50 {
    51     if(image!=NULL)
    52     {
    53         image->GetMutex();
    54         memcpy(buf,image->img->imageData,image->GetDataType().GetSize());
    55         image->ReleaseMutex();
    56     }
    57 }
    58 
    5952
    6053} // end namespace gui

  • trunk/lib/FlairCore/src/Picture.h

    r2 r15  
    1717#include <cxtypes.h>
    1818
    19 namespace flair
    20 {
    21 namespace core
    22 {
    23     class cvimage;
     19namespace flair {
     20namespace core {
     21class cvimage;
    2422}
    2523
    26 namespace gui
    27 {
     24namespace gui {
    2825
    29     class LayoutPosition;
     26class LayoutPosition;
    3027
    31     /*! \class Picture
    32     *
    33     * \brief Class displaying a Picture on the ground station
    34     *
    35     */
    36     class Picture:public SendData
    37     {
    38         public:
    39             /*!
    40             * \brief Constructor
    41             *
    42             * Construct a picture at given position. \n
    43             * The Picture will automatically be child of position->getLayout() Layout. After calling this constructor,
    44             * position will be deleted as it is no longer usefull.
    45             *
    46             * \param position position to draw plot
    47             * \param name name
    48             * \param image image to draw
    49             */
    50             Picture(const LayoutPosition* position,std::string name,const core::cvimage *image);
     28/*! \class Picture
     29*
     30* \brief Class displaying a Picture on the ground station
     31*
     32*/
     33class Picture : public SendData {
     34public:
     35  /*!
     36  * \brief Constructor
     37  *
     38  * Construct a picture at given position. \n
     39  * The Picture will automatically be child of position->getLayout() Layout.
     40  *After calling this constructor,
     41  * position will be deleted as it is no longer usefull.
     42  *
     43  * \param position position to draw plot
     44  * \param name name
     45  * \param image image to draw
     46  */
     47  Picture(const LayoutPosition *position, std::string name,
     48          const core::cvimage *image);
    5149
    52             /*!
    53             * \brief Destructor
    54             *
    55             */
    56             ~Picture();
     50  /*!
     51  * \brief Destructor
     52  *
     53  */
     54  ~Picture();
    5755
    58         private:
    59             /*!
    60             * \brief Copy datas to specified buffer
    61             *
    62             * Reimplemented from SendData.
    63             *
    64             * \param buf output buffer
    65             */
    66             void CopyDatas(char* buf) const;
     56private:
     57  /*!
     58  * \brief Copy datas to specified buffer
     59  *
     60  * Reimplemented from SendData.
     61  *
     62  * \param buf output buffer
     63  */
     64  void CopyDatas(char *buf) const;
    6765
    68             /*!
    69             * \brief Extra Xml event
    70             *
    71             * Reimplemented from SendData.
    72             */
    73             void ExtraXmlEvent(void){};
     66  /*!
     67  * \brief Extra Xml event
     68  *
     69  * Reimplemented from SendData.
     70  */
     71  void ExtraXmlEvent(void){};
    7472
    75             const core::cvimage *image;
    76     };
     73  const core::cvimage *image;
     74};
    7775
    7876} // end namespace gui

  • trunk/lib/FlairCore/src/PushButton.cpp

    r2 r15  
    2222using std::string;
    2323
    24 namespace flair
    25 {
    26 namespace gui
    27 {
     24namespace flair {
     25namespace gui {
    2826
    29 PushButton::PushButton(const LayoutPosition* position,string name): Widget(position->getLayout(),name,"PushButton")
    30 {
    31     SetVolatileXmlProp("row",position->Row());
    32     SetVolatileXmlProp("col",position->Col());
    33     delete position;
     27PushButton::PushButton(const LayoutPosition *position, string name)
     28    : Widget(position->getLayout(), name, "PushButton") {
     29  SetVolatileXmlProp("row", position->Row());
     30  SetVolatileXmlProp("col", position->Col());
     31  delete position;
    3432
    35     SendXml();
     33  SendXml();
    3634
    37     clicked=false;
     35  clicked = false;
    3836}
    3937
    40 PushButton::~PushButton()
    41 {
     38PushButton::~PushButton() {}
    4239
     40void PushButton::XmlEvent(void) {
     41  int clic = 0;
     42  GetPersistentXmlProp("value", clic);
     43
     44  if (clic == 1)
     45    clicked = true;
    4346}
    4447
    45 void PushButton::XmlEvent(void)
    46 {
    47     int clic=0;
    48     GetPersistentXmlProp("value",clic);
    49 
    50     if(clic==1) clicked=true;
    51 }
    52 
    53 bool PushButton::Clicked(void)
    54 {
    55     if(clicked==true)
    56     {
    57        clicked=false;
    58        return true;
    59     }
    60     else
    61     {
    62         return false;
    63     }
     48bool PushButton::Clicked(void) {
     49  if (clicked == true) {
     50    clicked = false;
     51    return true;
     52  } else {
     53    return false;
     54  }
    6455}
    6556

  • trunk/lib/FlairCore/src/PushButton.h

    r2 r15  
    1616#include <Widget.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class LayoutPosition;
     21class LayoutPosition;
    2422
    25     /*! \class PushButton
    26     *
    27     * \brief Class displaying a QPushButton on the ground station
    28     *
    29     */
    30     class PushButton:public Widget
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a QPushButton at given position. \n
    37             * The PushButton will automatically be child of position->getLayout() Layout. After calling this constructor,
    38             * position will be deleted as it is no longer usefull.
    39             *
    40             * \param parent parent
    41             * \param name name
    42             */
    43             PushButton(const LayoutPosition* position,std::string name);
     23/*! \class PushButton
     24*
     25* \brief Class displaying a QPushButton on the ground station
     26*
     27*/
     28class PushButton : public Widget {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a QPushButton at given position. \n
     34  * The PushButton will automatically be child of position->getLayout() Layout.
     35  *After calling this constructor,
     36  * position will be deleted as it is no longer usefull.
     37  *
     38  * \param parent parent
     39  * \param name name
     40  */
     41  PushButton(const LayoutPosition *position, std::string name);
    4442
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~PushButton();
     43  /*!
     44  * \brief Destructor
     45  *
     46  */
     47  ~PushButton();
    5048
    51             /*!
    52             * \brief Has been clicled?
    53             *
    54             * After calling this method, the internal flag
    55             * representing the state of the button is
    56             * automatically set to false.
    57             *
    58             * \return true if button was clicked
    59             */
    60             bool Clicked(void);
     49  /*!
     50  * \brief Has been clicled?
     51  *
     52  * After calling this method, the internal flag
     53  * representing the state of the button is
     54  * automatically set to false.
     55  *
     56  * \return true if button was clicked
     57  */
     58  bool Clicked(void);
    6159
    62         private:
    63             /*!
    64             * \brief XmlEvent from ground station
    65             *
    66             * Reimplemented from Widget.
    67             *
    68             */
    69             void XmlEvent(void);
     60private:
     61  /*!
     62  * \brief XmlEvent from ground station
     63  *
     64  * Reimplemented from Widget.
     65  *
     66  */
     67  void XmlEvent(void);
    7068
    71             bool clicked;
    72     };
     69  bool clicked;
     70};
    7371
    7472} // end namespace gui

  • trunk/lib/FlairCore/src/Quaternion.cpp

    r2 r15  
    2626namespace core {
    2727
    28 Quaternion::Quaternion(float inQ0,float inQ1,float inQ2,float inQ3):q0(inQ0),q1(inQ1),q2(inQ2),q3(inQ3) {
    29 }
     28Quaternion::Quaternion(float inQ0, float inQ1, float inQ2, float inQ3)
     29    : q0(inQ0), q1(inQ1), q2(inQ2), q3(inQ3) {}
    3030
    31 Quaternion::~Quaternion() {
    32 }
     31Quaternion::~Quaternion() {}
    3332
    34 Quaternion& Quaternion::operator=(const Quaternion &quaternion) {
    35     q0=quaternion.q0;
    36     q1=quaternion.q1;
    37     q2=quaternion.q2;
    38     q3=quaternion.q3;
    39     return (*this);
     33Quaternion &Quaternion::operator=(const Quaternion &quaternion) {
     34  q0 = quaternion.q0;
     35  q1 = quaternion.q1;
     36  q2 = quaternion.q2;
     37  q3 = quaternion.q3;
     38  return (*this);
    4039}
    4140
    4241float Quaternion::GetNorm(void) const {
    43     return sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
     42  return sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    4443}
    4544
    4645void Quaternion::Normalize(void) {
    47     float n=GetNorm();
    48     if(n!=0) {
    49         q0=q0/n;
    50         q1=q1/n;
    51         q2=q2/n;
    52         q3=q3/n;
    53     }
     46  float n = GetNorm();
     47  if (n != 0) {
     48    q0 = q0 / n;
     49    q1 = q1 / n;
     50    q2 = q2 / n;
     51    q3 = q3 / n;
     52  }
    5453}
    5554
    5655void Quaternion::Conjugate(void) {
    57     q1=-q1;
    58     q2=-q2;
    59     q3=-q3;
     56  q1 = -q1;
     57  q2 = -q2;
     58  q3 = -q3;
    6059}
    6160
    6261Quaternion Quaternion::GetConjugate(void) {
    63     return Quaternion(q0,-q1,-q2,-q3);
     62  return Quaternion(q0, -q1, -q2, -q3);
    6463}
    6564
    6665void Quaternion::GetLogarithm(Vector3D &logarithm) {
    67     Normalize();
    68     float v_norm=sqrtf(q1*q1+q2*q2+q3*q3);
     66  Normalize();
     67  float v_norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3);
    6968
    70     if(v_norm!=0) {
    71         float v_arccos = acosf(q0);
    72         logarithm.x=(q1*v_arccos)/v_norm;
    73         logarithm.y=(q2*v_arccos)/v_norm;
    74         logarithm.z=(q3*v_arccos)/v_norm;
    75     } else {
    76         logarithm.x=0;
    77         logarithm.y=0;
    78         logarithm.z=0;
    79     }
     69  if (v_norm != 0) {
     70    float v_arccos = acosf(q0);
     71    logarithm.x = (q1 * v_arccos) / v_norm;
     72    logarithm.y = (q2 * v_arccos) / v_norm;
     73    logarithm.z = (q3 * v_arccos) / v_norm;
     74  } else {
     75    logarithm.x = 0;
     76    logarithm.y = 0;
     77    logarithm.z = 0;
     78  }
    8079}
    8180
    8281Vector3D Quaternion::GetLogarithm(void) {
    83     Vector3D vector;
    84     GetLogarithm(vector);
    85     return vector;
     82  Vector3D vector;
     83  GetLogarithm(vector);
     84  return vector;
    8685}
    8786
    8887Quaternion Quaternion::GetDerivative(const Vector3D &angularSpeed) const {
    89     const Quaternion Qw(0,angularSpeed.x,angularSpeed.y,angularSpeed.z);
    90     return 0.5*(*this)*Qw;
     88  const Quaternion Qw(0, angularSpeed.x, angularSpeed.y, angularSpeed.z);
     89  return 0.5 * (*this) * Qw;
    9190}
    9291
    9392void Quaternion::Derivate(const Vector3D &angularSpeed) {
    94     Quaternion Q=GetDerivative(angularSpeed);
    95     (*this)=Q;
     93  Quaternion Q = GetDerivative(angularSpeed);
     94  (*this) = Q;
    9695}
    9796
    9897void Quaternion::ToEuler(Euler &euler) const {
    99     euler.roll=atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
    100     euler.pitch=asin(2*(q0*q2-q1*q3));
    101     euler.yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
     98  euler.roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
     99  euler.pitch = asin(2 * (q0 * q2 - q1 * q3));
     100  euler.yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
    102101}
    103102
    104103Euler Quaternion::ToEuler(void) const {
    105     Euler euler;
    106     ToEuler(euler);
    107     return euler;
     104  Euler euler;
     105  ToEuler(euler);
     106  return euler;
    108107}
    109108
    110109void Quaternion::ToRotationMatrix(RotationMatrix &matrix) const {
    111     float aSq = q0 * q0;
    112     float bSq = q1 * q1;
    113     float cSq = q2 * q2;
    114     float dSq = q3 * q3;
    115     matrix.m[0][0] = aSq + bSq - cSq - dSq;
    116     matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
    117     matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
    118     matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
    119     matrix.m[1][1] = aSq - bSq + cSq - dSq;
    120     matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
    121     matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
    122     matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
    123     matrix.m[2][2] = aSq - bSq - cSq + dSq;
     110  float aSq = q0 * q0;
     111  float bSq = q1 * q1;
     112  float cSq = q2 * q2;
     113  float dSq = q3 * q3;
     114  matrix.m[0][0] = aSq + bSq - cSq - dSq;
     115  matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
     116  matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
     117  matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
     118  matrix.m[1][1] = aSq - bSq + cSq - dSq;
     119  matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
     120  matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
     121  matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
     122  matrix.m[2][2] = aSq - bSq - cSq + dSq;
    124123}
    125124
    126125Quaternion &Quaternion::operator+=(const Quaternion &quaternion) {
    127     q0+=quaternion.q0;
    128     q1+=quaternion.q1;
    129     q2+=quaternion.q2;
    130     q3+=quaternion.q3;
    131     return (*this);
     126  q0 += quaternion.q0;
     127  q1 += quaternion.q1;
     128  q2 += quaternion.q2;
     129  q3 += quaternion.q3;
     130  return (*this);
    132131}
    133132
    134133Quaternion &Quaternion::operator-=(const Quaternion &quaternion) {
    135     q0-=quaternion.q0;
    136     q1-=quaternion.q1;
    137     q2-=quaternion.q2;
    138     q3-=quaternion.q3;
    139     return (*this);
     134  q0 -= quaternion.q0;
     135  q1 -= quaternion.q1;
     136  q2 -= quaternion.q2;
     137  q3 -= quaternion.q3;
     138  return (*this);
    140139}
    141140
    142 Quaternion operator + (const Quaternion &quaternionA,const Quaternion &quaterniontB) {
    143     return Quaternion(
    144                quaternionA.q0 + quaterniontB.q0,
    145                quaternionA.q1 + quaterniontB.q1,
    146                quaternionA.q2 + quaterniontB.q2,
    147                quaternionA.q3 + quaterniontB.q3);
     141Quaternion operator+(const Quaternion &quaternionA,
     142                     const Quaternion &quaterniontB) {
     143  return Quaternion(
     144      quaternionA.q0 + quaterniontB.q0, quaternionA.q1 + quaterniontB.q1,
     145      quaternionA.q2 + quaterniontB.q2, quaternionA.q3 + quaterniontB.q3);
    148146}
    149147
    150 Quaternion operator- (const Quaternion &quaterniontA,const Quaternion &quaterniontB) {
    151    return Quaternion(
    152                quaterniontA.q0 - quaterniontB.q0,
    153                quaterniontA.q1 - quaterniontB.q1,
    154                quaterniontA.q2 - quaterniontB.q2,
    155                quaterniontA.q3 - quaterniontB.q3);
     148Quaternion operator-(const Quaternion &quaterniontA,
     149                     const Quaternion &quaterniontB) {
     150  return Quaternion(
     151      quaterniontA.q0 - quaterniontB.q0, quaterniontA.q1 - quaterniontB.q1,
     152      quaterniontA.q2 - quaterniontB.q2, quaterniontA.q3 - quaterniontB.q3);
    156153}
    157154
    158155Quaternion operator-(const Quaternion &quaternion) {
    159     return Quaternion(-quaternion.q0,-quaternion.q1,-quaternion.q2,-quaternion.q3);
     156  return Quaternion(-quaternion.q0, -quaternion.q1, -quaternion.q2,
     157                    -quaternion.q3);
    160158}
    161159
    162 Quaternion operator * (const Quaternion &quaterniontA,const Quaternion &quaterniontB) {
    163     return Quaternion(
    164                quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 - quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
    165                quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 + quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
    166                quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 + quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
    167                quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 - quaterniontA.q2 * quaterniontB.q1 + quaterniontA.q3 * quaterniontB.q0);
     160Quaternion operator*(const Quaternion &quaterniontA,
     161                     const Quaternion &quaterniontB) {
     162  return Quaternion(
     163      quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 -
     164          quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
     165      quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 +
     166          quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
     167      quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 +
     168          quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
     169      quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 -
     170          quaterniontA.q2 * quaterniontB.q1 +
     171          quaterniontA.q3 * quaterniontB.q0);
    168172}
    169173
    170 Quaternion operator * (float coeff,const Quaternion &quaternion) {
    171     return Quaternion(
    172                coeff*quaternion.q0,
    173                coeff*quaternion.q1,
    174                coeff*quaternion.q2,
    175                coeff*quaternion.q3);
     174Quaternion operator*(float coeff, const Quaternion &quaternion) {
     175  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
     176                    coeff * quaternion.q2, coeff * quaternion.q3);
    176177}
    177178
    178 
    179 Quaternion operator * (const Quaternion &quaternion,float coeff) {
    180     return Quaternion(
    181                coeff*quaternion.q0,
    182                coeff*quaternion.q1,
    183                coeff*quaternion.q2,
    184                coeff*quaternion.q3);
     179Quaternion operator*(const Quaternion &quaternion, float coeff) {
     180  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
     181                    coeff * quaternion.q2, coeff * quaternion.q3);
    185182}
    186183

  • trunk/lib/FlairCore/src/Quaternion.h

    r2 r15  
    1313#define QUATERNION_H
    1414
    15 namespace flair { namespace core {
    16     class Euler;
    17     class Vector3D;
    18     class RotationMatrix;
    19 
    20     /*! \class Quaternion
    21     *
    22     * \brief Class defining a quaternion
    23     */
    24     class Quaternion {
    25         public:
    26             /*!
    27             * \brief Constructor
    28             *
    29             * Construct a quaternion using specified values.
    30             *
    31             * \param q0, scalar part
    32             * \param q1
    33             * \param q2
    34             * \param q3
    35             */
    36             Quaternion(float q0=1,float q1=0,float q2=0,float q3=0);
    37 
    38             /*!
    39             * \brief Destructor
    40             *
    41             */
    42             ~Quaternion();
    43 
    44             /*!
    45             * \brief Norm
    46             *
    47             * \return norm
    48             */
    49             float GetNorm(void) const;
    50 
    51             /*!
    52             * \brief Normalize
    53             */
    54             void Normalize(void);
    55 
    56             /*!
    57             * \brief Logarithm
    58             *
    59             * This method also Normalize the quaternion.
    60             *
    61             * \param logarithm output logarithm
    62             */
    63             void GetLogarithm(Vector3D &logarithm);
    64 
    65             /*!
    66             * \brief Logarithm
    67             *
    68             * This method also Normalize the quaternion.
    69             *
    70             * \return output logarithm
    71             */
    72             Vector3D GetLogarithm(void);
    73 
    74             /*!
    75             * \brief Conjugate
    76             */
    77             void Conjugate(void);
    78 
    79             /*!
    80             * \brief Conjugate
    81             *
    82             * \return Conjugate
    83             */
    84             Quaternion GetConjugate(void);
    85 
    86             /*!
    87             * \brief Derivative
    88             *
    89             * \param w angular speed
    90             *
    91             * \return derivative
    92             */
    93             Quaternion GetDerivative(const Vector3D &angularSpeed) const;
    94 
    95             /*!
    96             * \brief Derivate
    97             *
    98             * \param w rotationonal speed
    99             */
    100             void Derivate(const Vector3D &angularSpeed);
    101 
    102             /*!
    103             * \brief Convert to euler angles
    104             *
    105             * \param euler output euler angles
    106             */
    107             void ToEuler(Euler &euler) const;
    108 
    109             /*!
    110             * \brief Convert to euler angles
    111             *
    112             * \return euler angles
    113             */
    114             Euler ToEuler(void) const;
    115 
    116             /*!
    117             * \brief Convert to rotation matrix
    118             *
    119             * \param m output matrix
    120             */
    121             void ToRotationMatrix(RotationMatrix &matrix) const;
    122 
    123             /*!
    124             * \brief q0
    125             */
    126             float q0;
    127 
    128             /*!
    129             * \brief q1
    130             */
    131             float q1;
    132 
    133             /*!
    134             * \brief q2
    135             */
    136             float q2;
    137 
    138             /*!
    139             * \brief q3
    140             */
    141             float q3;
    142 
    143             Quaternion& operator+=(const Quaternion& quaternion);
    144             Quaternion& operator-=(const Quaternion& quaternion);
    145             Quaternion& operator=(const Quaternion& quaternion);
    146     };
    147 
    148     /*! Add
    149     *
    150     * \brief Add
    151     *
    152     * \param quaterniontA quaternion
    153     * \param quaterniontB quaternion
    154     *
    155     * \return quaterniontA+quaterniontB
    156     */
    157     Quaternion operator + (Quaternion const &quaterniontA,Quaternion const &quaterniontB);
    158 
    159     /*! Substract
    160     *
    161     * \brief Substract
    162     *
    163     * \param quaterniontA quaternion
    164     * \param quaterniontB quaternion
    165     *
    166     * \return quaterniontA-quaterniontB
    167     */
    168     Quaternion operator - (Quaternion const &quaternionA,Quaternion const &quaterniontB);
    169 
    170     /*! Minus
    171     *
    172     * \brief Minus
    173     *
    174     * \param quaternion quaternion
    175     *
    176     * \return -quaternion
    177     */
    178     Quaternion operator-(const Quaternion &quaternion);
    179 
    180     /*! Multiply
    181     *
    182     * \brief Multiply
    183     *
    184     * \param quaterniontA quaternion
    185     * \param quaterniontB quaternion
    186     *
    187     * \return quaterniontA*quaterniontB
    188     */
    189     Quaternion operator * (Quaternion const &quaternionA,Quaternion const &quaterniontB);
    190 
    191     /*! Multiply
    192     *
    193     * \brief Multiply
    194     *
    195     * \param coeff coefficient
    196     * \param quat quaternion
    197     *
    198     * \return coeff*quat
    199     */
    200     Quaternion operator * (float coeff,Quaternion const &quaternion);
    201 
    202     /*! Multiply
    203     *
    204     * \brief Multiply
    205     *
    206     * \param quat quaternion
    207     * \param coeff coefficient
    208     *
    209     * \return coeff*quat
    210     */
    211     Quaternion operator * (Quaternion const &quaternion,float coeff);
     15namespace flair {
     16namespace core {
     17class Euler;
     18class Vector3D;
     19class RotationMatrix;
     20
     21/*! \class Quaternion
     22*
     23* \brief Class defining a quaternion
     24*/
     25class Quaternion {
     26public:
     27  /*!
     28  * \brief Constructor
     29  *
     30  * Construct a quaternion using specified values.
     31  *
     32  * \param q0, scalar part
     33  * \param q1
     34  * \param q2
     35  * \param q3
     36  */
     37  Quaternion(float q0 = 1, float q1 = 0, float q2 = 0, float q3 = 0);
     38
     39  /*!
     40  * \brief Destructor
     41  *
     42  */
     43  ~Quaternion();
     44
     45  /*!
     46  * \brief Norm
     47  *
     48  * \return norm
     49  */
     50  float GetNorm(void) const;
     51
     52  /*!
     53  * \brief Normalize
     54  */
     55  void Normalize(void);
     56
     57  /*!
     58  * \brief Logarithm
     59  *
     60  * This method also Normalize the quaternion.
     61  *
     62  * \param logarithm output logarithm
     63  */
     64  void GetLogarithm(Vector3D &logarithm);
     65
     66  /*!
     67  * \brief Logarithm
     68  *
     69  * This method also Normalize the quaternion.
     70  *
     71  * \return output logarithm
     72  */
     73  Vector3D GetLogarithm(void);
     74
     75  /*!
     76  * \brief Conjugate
     77  */
     78  void Conjugate(void);
     79
     80  /*!
     81  * \brief Conjugate
     82  *
     83  * \return Conjugate
     84  */
     85  Quaternion GetConjugate(void);
     86
     87  /*!
     88  * \brief Derivative
     89  *
     90  * \param w angular speed
     91  *
     92  * \return derivative
     93  */
     94  Quaternion GetDerivative(const Vector3D &angularSpeed) const;
     95
     96  /*!
     97  * \brief Derivate
     98  *
     99  * \param w rotationonal speed
     100  */
     101  void Derivate(const Vector3D &angularSpeed);
     102
     103  /*!
     104  * \brief Convert to euler angles
     105  *
     106  * \param euler output euler angles
     107  */
     108  void ToEuler(Euler &euler) const;
     109
     110  /*!
     111  * \brief Convert to euler angles
     112  *
     113  * \return euler angles
     114  */
     115  Euler ToEuler(void) const;
     116
     117  /*!
     118  * \brief Convert to rotation matrix
     119  *
     120  * \param m output matrix
     121  */
     122  void ToRotationMatrix(RotationMatrix &matrix) const;
     123
     124  /*!
     125  * \brief q0
     126  */
     127  float q0;
     128
     129  /*!
     130  * \brief q1
     131  */
     132  float q1;
     133
     134  /*!
     135  * \brief q2
     136  */
     137  float q2;
     138
     139  /*!
     140  * \brief q3
     141  */
     142  float q3;
     143
     144  Quaternion &operator+=(const Quaternion &quaternion);
     145  Quaternion &operator-=(const Quaternion &quaternion);
     146  Quaternion &operator=(const Quaternion &quaternion);
     147};
     148
     149/*! Add
     150*
     151* \brief Add
     152*
     153* \param quaterniontA quaternion
     154* \param quaterniontB quaternion
     155*
     156* \return quaterniontA+quaterniontB
     157*/
     158Quaternion operator+(Quaternion const &quaterniontA,
     159                     Quaternion const &quaterniontB);
     160
     161/*! Substract
     162*
     163* \brief Substract
     164*
     165* \param quaterniontA quaternion
     166* \param quaterniontB quaternion
     167*
     168* \return quaterniontA-quaterniontB
     169*/
     170Quaternion operator-(Quaternion const &quaternionA,
     171                     Quaternion const &quaterniontB);
     172
     173/*! Minus
     174*
     175* \brief Minus
     176*
     177* \param quaternion quaternion
     178*
     179* \return -quaternion
     180*/
     181Quaternion operator-(const Quaternion &quaternion);
     182
     183/*! Multiply
     184*
     185* \brief Multiply
     186*
     187* \param quaterniontA quaternion
     188* \param quaterniontB quaternion
     189*
     190* \return quaterniontA*quaterniontB
     191*/
     192Quaternion operator*(Quaternion const &quaternionA,
     193                     Quaternion const &quaterniontB);
     194
     195/*! Multiply
     196*
     197* \brief Multiply
     198*
     199* \param coeff coefficient
     200* \param quat quaternion
     201*
     202* \return coeff*quat
     203*/
     204Quaternion operator*(float coeff, Quaternion const &quaternion);
     205
     206/*! Multiply
     207*
     208* \brief Multiply
     209*
     210* \param quat quaternion
     211* \param coeff coefficient
     212*
     213* \return coeff*quat
     214*/
     215Quaternion operator*(Quaternion const &quaternion, float coeff);
    212216
    213217} // end namespace core

  • trunk/lib/FlairCore/src/RTDM_I2cPort.cpp

    r2 r15  
    2626using std::string;
    2727
    28 namespace flair
    29 {
    30 namespace core
    31 {
     28namespace flair {
     29namespace core {
    3230
    33 RTDM_I2cPort::RTDM_I2cPort(const Object* parent,string name,string device) : I2cPort(parent,name)
    34 {
    35     int err=0;
    36     struct rti2c_config write_config;
     31RTDM_I2cPort::RTDM_I2cPort(const Object *parent, string name, string device)
     32    : I2cPort(parent, name) {
     33  int err = 0;
     34  struct rti2c_config write_config;
    3735
    38     //printf("i2c integer le mutex dans le driver, avec acces ioctl\n");
    39     fd = rt_dev_open(device.c_str(), 0);
    40     if (fd < 0)
    41         {
    42                 Err("rt_dev_open (%s)\n",ObjectName().c_str(),strerror(-fd));
    43         }
     36  // printf("i2c integer le mutex dans le driver, avec acces ioctl\n");
     37  fd = rt_dev_open(device.c_str(), 0);
     38  if (fd < 0) {
     39    Err("rt_dev_open (%s)\n", ObjectName().c_str(), strerror(-fd));
     40  }
    4441
    45         write_config.config_mask       = RTI2C_SET_BAUD|RTI2C_SET_TIMEOUT_RX|RTI2C_SET_TIMEOUT_TX;
    46         write_config.baud_rate         = 400000;
    47         write_config.rx_timeout        = 500000;
    48         write_config.tx_timeout        = 1000000;//5ms pour les xbldc, normalement 1ms
    49         // the rest implicitely remains default
     42  write_config.config_mask =
     43      RTI2C_SET_BAUD | RTI2C_SET_TIMEOUT_RX | RTI2C_SET_TIMEOUT_TX;
     44  write_config.baud_rate = 400000;
     45  write_config.rx_timeout = 500000;
     46  write_config.tx_timeout = 1000000; // 5ms pour les xbldc, normalement 1ms
     47  // the rest implicitely remains default
    5048
    51         // config
    52         err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_CONFIG, &write_config );
    53         if (err)
    54         {
    55             Err("rt_dev_ioctl RTI2C_RTIOC_SET_CONFIG error (%s)\n",ObjectName().c_str(),strerror(-err));
    56         }
     49  // config
     50  err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_CONFIG, &write_config);
     51  if (err) {
     52    Err("rt_dev_ioctl RTI2C_RTIOC_SET_CONFIG error (%s)\n",
     53        ObjectName().c_str(), strerror(-err));
     54  }
    5755}
    5856
    59 RTDM_I2cPort::~RTDM_I2cPort()
    60 {
    61     rt_dev_close(fd);
     57RTDM_I2cPort::~RTDM_I2cPort() { rt_dev_close(fd); }
     58
     59int RTDM_I2cPort::SetSlave(uint16_t address) {
     60  int err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_SLAVE, &address);
     61  if (err) {
     62    Err("rt_dev_ioctl RTI2C_RTIOC_SET_SLAVE error (%s)\n", strerror(-err));
     63  }
     64
     65  return err;
    6266}
    6367
    64 int RTDM_I2cPort::SetSlave(uint16_t address)
    65 {
    66         int err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_SLAVE, &address);
    67     if (err)
    68         {
    69             Err("rt_dev_ioctl RTI2C_RTIOC_SET_SLAVE error (%s)\n",strerror(-err));
    70         }
     68void RTDM_I2cPort::SetRxTimeout(Time timeout_ns) {
     69  struct rti2c_config write_config;
    7170
    72         return err;
     71  write_config.config_mask = RTI2C_SET_TIMEOUT_RX;
     72  write_config.rx_timeout = timeout_ns;
     73
     74  int err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_CONFIG, &write_config);
     75  if (err) {
     76    Err("rt_dev_ioctl RTI2C_RTIOC_SET_CONFIG (%s)\n", ObjectName().c_str(),
     77        strerror(-err));
     78  }
    7379}
    7480
    75 void RTDM_I2cPort::SetRxTimeout(Time timeout_ns)
    76 {
    77     struct rti2c_config write_config;
     81void RTDM_I2cPort::SetTxTimeout(Time timeout_ns) {
     82  struct rti2c_config write_config;
    7883
    79         write_config.config_mask       = RTI2C_SET_TIMEOUT_RX;
    80         write_config.rx_timeout        = timeout_ns;
     84  write_config.config_mask = RTI2C_SET_TIMEOUT_TX;
     85  write_config.tx_timeout = timeout_ns;
    8186
    82         int err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_CONFIG, &write_config );
    83         if (err)
    84         {
    85             Err("rt_dev_ioctl RTI2C_RTIOC_SET_CONFIG (%s)\n",ObjectName().c_str(),strerror(-err));
    86         }
     87  int err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_CONFIG, &write_config);
     88  if (err) {
     89    Err("rt_dev_ioctl RTI2C_RTIOC_SET_CONFIG (%s)\n", ObjectName().c_str(),
     90        strerror(-err));
     91  }
    8792}
    8893
    89 void RTDM_I2cPort::SetTxTimeout(Time timeout_ns)
    90 {
    91     struct rti2c_config write_config;
    92 
    93         write_config.config_mask       = RTI2C_SET_TIMEOUT_TX;
    94         write_config.tx_timeout        = timeout_ns;
    95 
    96         int err = rt_dev_ioctl(fd, RTI2C_RTIOC_SET_CONFIG, &write_config );
    97         if (err)
    98         {
    99             Err("rt_dev_ioctl RTI2C_RTIOC_SET_CONFIG (%s)\n",ObjectName().c_str(),strerror(-err));
    100         }
     94ssize_t RTDM_I2cPort::Write(const void *buf, size_t nbyte) {
     95  return rt_dev_write(fd, buf, nbyte);
    10196}
    10297
    103 ssize_t RTDM_I2cPort::Write(const void *buf,size_t nbyte)
    104 {
    105     return rt_dev_write(fd, buf, nbyte);
    106 }
    107 
    108 ssize_t RTDM_I2cPort::Read(void *buf,size_t nbyte)
    109 {
    110     return rt_dev_read(fd, buf, nbyte);
     98ssize_t RTDM_I2cPort::Read(void *buf, size_t nbyte) {
     99  return rt_dev_read(fd, buf, nbyte);
    111100}
    112101
    … …  
    119108using namespace flair::core;
    120109
     110namespace flair {
     111namespace core {
    121112
    122 namespace flair
    123 {
    124 namespace core
    125 {
    126 
    127 RTDM_I2cPort::RTDM_I2cPort(const Object* parent,string name,string device) : I2cPort(parent,name)
    128 {
    129     Err("not available in non real time\n");
     113RTDM_I2cPort::RTDM_I2cPort(const Object *parent, string name, string device)
     114    : I2cPort(parent, name) {
     115  Err("not available in non real time\n");
    130116}
    131117
    132 RTDM_I2cPort::~RTDM_I2cPort()
    133 {
     118RTDM_I2cPort::~RTDM_I2cPort() {}
    134119
    135 }
     120int RTDM_I2cPort::SetSlave(uint16_t address) { return -1; }
    136121
    137 int RTDM_I2cPort::SetSlave(uint16_t address)
    138 {
    139         return -1;
    140 }
     122void RTDM_I2cPort::SetRxTimeout(Time timeout_ns) {}
    141123
    142 void RTDM_I2cPort::SetRxTimeout(Time timeout_ns)
    143 {
    144 }
     124void RTDM_I2cPort::SetTxTimeout(Time timeout_ns) {}
    145125
    146 void RTDM_I2cPort::SetTxTimeout(Time timeout_ns)
    147 {
    148 }
     126ssize_t RTDM_I2cPort::Write(const void *buf, size_t nbyte) { return -1; }
    149127
    150 ssize_t RTDM_I2cPort::Write(const void *buf,size_t nbyte)
    151 {
    152     return -1;
    153 }
    154 
    155 ssize_t RTDM_I2cPort::Read(void *buf,size_t nbyte)
    156 {
    157     return -1;
    158 }
     128ssize_t RTDM_I2cPort::Read(void *buf, size_t nbyte) { return -1; }
    159129
    160130} // end namespace core

  • trunk/lib/FlairCore/src/RTDM_I2cPort.h

    r2 r15  
    1616#include <I2cPort.h>
    1717
    18 namespace flair
    19 {
    20 namespace core
    21 {
    22     /*! \class RTDM_I2cPort
    23     *
    24     * \brief Class for real time i2c port using RTDM
    25     *
    26     * This class can only be used with the real time version of Framework library.
    27     *
    28     */
    29     class RTDM_I2cPort: public I2cPort
    30     {
    31         public:
    32             /*!
    33             * \brief Constructor
    34             *
    35             * Construct an RTDM i2c port, with the following default values: \n
    36             * - 400kbits baudrate \n
    37             * - 500000ns RX timeout \n
    38             * - 1000000ns TX timeout
    39             *
    40             * \param parent parent
    41             * \param name name
    42             * \param device i2c device (ex rti2c1)
    43             */
    44             RTDM_I2cPort(const Object* parent,std::string name,std::string device);
     18namespace flair {
     19namespace core {
     20/*! \class RTDM_I2cPort
     21*
     22* \brief Class for real time i2c port using RTDM
     23*
     24* This class can only be used with the real time version of Framework library.
     25*
     26*/
     27class RTDM_I2cPort : public I2cPort {
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct an RTDM i2c port, with the following default values: \n
     33  * - 400kbits baudrate \n
     34  * - 500000ns RX timeout \n
     35  * - 1000000ns TX timeout
     36  *
     37  * \param parent parent
     38  * \param name name
     39  * \param device i2c device (ex rti2c1)
     40  */
     41  RTDM_I2cPort(const Object *parent, std::string name, std::string device);
    4542
    46             /*!
    47             * \brief Destructor
    48             *
    49             */
    50             ~RTDM_I2cPort();
     43  /*!
     44  * \brief Destructor
     45  *
     46  */
     47  ~RTDM_I2cPort();
    5148
    52             /*!
    53             * \brief Set slave's address
    54             *
    55             * This method need to be called before any communication.
    56             *
    57             * \param address slave's address
    58             */
    59             int SetSlave(uint16_t address);
     49  /*!
     50  * \brief Set slave's address
     51  *
     52  * This method need to be called before any communication.
     53  *
     54  * \param address slave's address
     55  */
     56  int SetSlave(uint16_t address);
    6057
    61             /*!
    62             * \brief Write datas
    63             *
    64             * \param buf pointer to datas
    65             * \param nbyte length of datas
    66             *
    67             * \return amount of written datas
    68             */
    69             ssize_t Write(const void *buf,size_t nbyte);
     58  /*!
     59  * \brief Write datas
     60  *
     61  * \param buf pointer to datas
     62  * \param nbyte length of datas
     63  *
     64  * \return amount of written datas
     65  */
     66  ssize_t Write(const void *buf, size_t nbyte);
    7067
    71             /*!
    72             * \brief Read datas
    73             *
    74             * \param buf pointer to datas
    75             * \param nbyte length of datas
    76             *
    77             * \return amount of read datas
    78             */
    79             ssize_t Read(void *buf,size_t nbyte);
     68  /*!
     69  * \brief Read datas
     70  *
     71  * \param buf pointer to datas
     72  * \param nbyte length of datas
     73  *
     74  * \return amount of read datas
     75  */
     76  ssize_t Read(void *buf, size_t nbyte);
    8077
    81             /*!
    82             * \brief Set RX timeout
    83             *
    84             * Timeout for waiting an ACK from the slave.
    85             *
    86             * \param timeout_ns timeout in nano second
    87             */
    88             void SetRxTimeout(Time timeout_ns);
     78  /*!
     79  * \brief Set RX timeout
     80  *
     81  * Timeout for waiting an ACK from the slave.
     82  *
     83  * \param timeout_ns timeout in nano second
     84  */
     85  void SetRxTimeout(Time timeout_ns);
    8986
    90             /*!
    91             * \brief Set TX timeout
    92             *
    93             * Timeout for waiting an ACK from the slave.
    94             *
    95             * \param timeout_ns timeout in nano second
    96             */
    97             void SetTxTimeout(Time timeout_ns);
     87  /*!
     88  * \brief Set TX timeout
     89  *
     90  * Timeout for waiting an ACK from the slave.
     91  *
     92  * \param timeout_ns timeout in nano second
     93  */
     94  void SetTxTimeout(Time timeout_ns);
    9895
    99         private:
    100             int fd;
    101     };
     96private:
     97  int fd;
     98};
    10299} // end namespace core
    103100} // end namespace flair

  • trunk/lib/FlairCore/src/RTDM_SerialPort.cpp

    r2 r15  
    2626using std::string;
    2727
    28 namespace flair
    29 {
    30 namespace core
    31 {
     28namespace flair {
     29namespace core {
    3230
    33 RTDM_SerialPort::RTDM_SerialPort(const Object* parent,string name,string device) : SerialPort(parent,name)
    34 {
    35     struct rtser_config write_config;
     31RTDM_SerialPort::RTDM_SerialPort(const Object *parent, string name,
     32                                 string device)
     33    : SerialPort(parent, name) {
     34  struct rtser_config write_config;
    3635
    37         write_config.config_mask      = RTSER_SET_BAUD | RTSER_SET_TIMESTAMP_HISTORY,
    38         write_config.baud_rate        = 115200,
    39         write_config.timestamp_history = RTSER_DEF_TIMESTAMP_HISTORY,
    40         // the rest implicitely remains default
     36  write_config.config_mask = RTSER_SET_BAUD | RTSER_SET_TIMESTAMP_HISTORY,
     37  write_config.baud_rate = 115200,
     38  write_config.timestamp_history = RTSER_DEF_TIMESTAMP_HISTORY,
     39  // the rest implicitely remains default
    4140
    42         fd = rt_dev_open(device.c_str(), 0);
    43         if (fd < 0)
    44         {
    45                 Err("erreur rt_dev_open (%s)\n",strerror(-fd));
    46         }
     41      fd = rt_dev_open(device.c_str(), 0);
     42  if (fd < 0) {
     43    Err("erreur rt_dev_open (%s)\n", strerror(-fd));
     44  }
    4745
    48         // config
    49         int err = rt_dev_ioctl(fd, RTSER_RTIOC_SET_CONFIG, &write_config );
    50         if (err)
    51         {
    52             Err("erreur rt_dev_ioctl RTSER_RTIOC_SET_CONFIG (%s)\n",strerror(-err));
    53         }
     46  // config
     47  int err = rt_dev_ioctl(fd, RTSER_RTIOC_SET_CONFIG, &write_config);
     48  if (err) {
     49    Err("erreur rt_dev_ioctl RTSER_RTIOC_SET_CONFIG (%s)\n", strerror(-err));
     50  }
    5451}
    5552
    56 RTDM_SerialPort::~RTDM_SerialPort()
    57 {
    58     rt_dev_close(fd);
     53RTDM_SerialPort::~RTDM_SerialPort() { rt_dev_close(fd); }
     54
     55void RTDM_SerialPort::SetBaudrate(int baudrate) {
     56  struct rtser_config write_config;
     57
     58  write_config.config_mask = RTSER_SET_BAUD;
     59  write_config.baud_rate = baudrate;
     60  // the rest implicitely remains default
     61
     62  // config
     63  int err = rt_dev_ioctl(fd, RTSER_RTIOC_SET_CONFIG, &write_config);
     64  if (err) {
     65    Err("erreur rt_dev_ioctl RTSER_RTIOC_SET_CONFIG (%s)\n", strerror(-err));
     66  }
    5967}
    6068
    61 void RTDM_SerialPort::SetBaudrate(int baudrate)
    62 {
    63     struct rtser_config write_config;
     69void RTDM_SerialPort::SetRxTimeout(core::Time timeout_ns) {
     70  struct rtser_config write_config;
    6471
    65         write_config.config_mask       = RTSER_SET_BAUD;
    66         write_config.baud_rate         = baudrate;
    67         // the rest implicitely remains default
     72  write_config.config_mask = RTSER_SET_TIMEOUT_RX;
     73  write_config.rx_timeout = timeout_ns;
     74  // the rest implicitely remains default
    6875
    69         // config
    70         int err = rt_dev_ioctl(fd, RTSER_RTIOC_SET_CONFIG, &write_config );
    71         if (err)
    72         {
    73             Err("erreur rt_dev_ioctl RTSER_RTIOC_SET_CONFIG (%s)\n",strerror(-err));
    74         }
     76  // config
     77  int err = rt_dev_ioctl(fd, RTSER_RTIOC_SET_CONFIG, &write_config);
     78  if (err) {
     79    Err("erreur rt_dev_ioctl RTSER_RTIOC_SET_CONFIG (%s)\n", strerror(-err));
     80  }
    7581}
    7682
    77 void RTDM_SerialPort::SetRxTimeout(core::Time timeout_ns)
    78 {
    79     struct rtser_config write_config;
     83void RTDM_SerialPort::FlushInput(void) {
     84  char tmp;
    8085
    81         write_config.config_mask       = RTSER_SET_TIMEOUT_RX;
    82         write_config.rx_timeout         = timeout_ns;
    83         // the rest implicitely remains default
    84 
    85         // config
    86         int err = rt_dev_ioctl(fd, RTSER_RTIOC_SET_CONFIG, &write_config );
    87         if (err)
    88         {
    89             Err("erreur rt_dev_ioctl RTSER_RTIOC_SET_CONFIG (%s)\n",strerror(-err));
    90         }
     86  SetRxTimeout(1000000);
     87  while (rt_dev_read(fd, &tmp, 1) == 1)
     88    ;
     89  SetRxTimeout(TIME_INFINITE);
    9190}
    9291
    93 void RTDM_SerialPort::FlushInput(void)
    94 {
    95     char tmp;
    96 
    97     SetRxTimeout(1000000);
    98     while(rt_dev_read(fd,&tmp,1)==1);
    99     SetRxTimeout(TIME_INFINITE);
     92ssize_t RTDM_SerialPort::Write(const void *buf, size_t nbyte) {
     93  return rt_dev_write(fd, buf, nbyte);
    10094}
    10195
    102 ssize_t RTDM_SerialPort::Write(const void *buf,size_t nbyte)
    103 {
    104     return rt_dev_write(fd, buf, nbyte);
    105 }
    106 
    107 ssize_t RTDM_SerialPort::Read(void *buf,size_t nbyte)
    108 {
    109     return rt_dev_read(fd, buf, nbyte);
     96ssize_t RTDM_SerialPort::Read(void *buf, size_t nbyte) {
     97  return rt_dev_read(fd, buf, nbyte);
    11098}
    11199
    … …  
    118106using namespace flair::core;
    119107
     108namespace flair {
     109namespace core {
    120110
    121 namespace flair
    122 {
    123 namespace core
    124 {
    125 
    126 RTDM_SerialPort::RTDM_SerialPort(const Object* parent,string name,string device) : SerialPort(parent,name)
    127 {
    128     Err("not available in non real time\n");
     111RTDM_SerialPort::RTDM_SerialPort(const Object *parent, string name,
     112                                 string device)
     113    : SerialPort(parent, name) {
     114  Err("not available in non real time\n");
    129115}
    130116
    131 RTDM_SerialPort::~RTDM_SerialPort()
    132 {
     117RTDM_SerialPort::~RTDM_SerialPort() {}
    133118
    134 }
     119ssize_t RTDM_SerialPort::Write(const void *buf, size_t nbyte) { return -1; }
    135120
    136 ssize_t RTDM_SerialPort::Write(const void *buf,size_t nbyte)
    137 {
    138     return -1;
    139 }
     121ssize_t RTDM_SerialPort::Read(void *buf, size_t nbyte) { return -1; }
    140122
    141 ssize_t RTDM_SerialPort::Read(void *buf,size_t nbyte)
    142 {
    143     return -1;
    144 }
     123void RTDM_SerialPort::SetBaudrate(int baudrate) {}
    145124
    146 void RTDM_SerialPort::SetBaudrate(int baudrate)
    147 {
    148 }
     125void RTDM_SerialPort::SetRxTimeout(core::Time timeout_ns) {}
    149126
    150 void RTDM_SerialPort::SetRxTimeout(core::Time timeout_ns)
    151 {
    152 }
    153 
    154 void RTDM_SerialPort::FlushInput(void)
    155 {
    156 }
     127void RTDM_SerialPort::FlushInput(void) {}
    157128
    158129} // end namespace core

  • trunk/lib/FlairCore/src/RTDM_SerialPort.h

    r2 r15  
    1616#include <SerialPort.h>
    1717
    18 namespace flair
    19 {
    20 namespace core
    21 {
    22     /*! \class RTDM_SerialPort
    23     *
    24     * \brief Class for real time serial port using RTDM
    25     *
    26     * This class can only be used with the real time version of Framework library.
    27     *
    28     */
    29     class RTDM_SerialPort: public SerialPort
    30     {
     18namespace flair {
     19namespace core {
     20/*! \class RTDM_SerialPort
     21*
     22* \brief Class for real time serial port using RTDM
     23*
     24* This class can only be used with the real time version of Framework library.
     25*
     26*/
     27class RTDM_SerialPort : public SerialPort {
    3128
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct an RTDM serial port, with the following default values: \n
    37             * - 115200bps baudrate
    38             *
    39             * \param parent parent
    40             * \param name name
    41             * \param device serial device (ex rtser1)
    42             */
    43             RTDM_SerialPort(const Object* parent,std::string port_name,std::string device);
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct an RTDM serial port, with the following default values: \n
     34  * - 115200bps baudrate
     35  *
     36  * \param parent parent
     37  * \param name name
     38  * \param device serial device (ex rtser1)
     39  */
     40  RTDM_SerialPort(const Object *parent, std::string port_name,
     41                  std::string device);
    4442
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~RTDM_SerialPort();
     43  /*!
     44  * \brief Destructor
     45  *
     46  */
     47  ~RTDM_SerialPort();
    5048
    51             /*!
    52             * \brief Set baudrate
    53             *
    54             * \param baudrate baudrate
    55             *
    56             */
    57             void SetBaudrate(int baudrate);
     49  /*!
     50  * \brief Set baudrate
     51  *
     52  * \param baudrate baudrate
     53  *
     54  */
     55  void SetBaudrate(int baudrate);
    5856
    59             /*!
    60             * \brief Set RX timeout
    61             *
    62             * Timeout for waiting datas.
    63             *
    64             * \param timeout_ns timeout in nano second
    65             */
    66             void SetRxTimeout(Time timeout_ns);
     57  /*!
     58  * \brief Set RX timeout
     59  *
     60  * Timeout for waiting datas.
     61  *
     62  * \param timeout_ns timeout in nano second
     63  */
     64  void SetRxTimeout(Time timeout_ns);
    6765
    68             /*!
    69             * \brief Write datas
    70             *
    71             * \param buf pointer to datas
    72             * \param nbyte length of datas
    73             *
    74             * \return amount of written datas
    75             */
    76             ssize_t Write(const void *buf,size_t nbyte);
     66  /*!
     67  * \brief Write datas
     68  *
     69  * \param buf pointer to datas
     70  * \param nbyte length of datas
     71  *
     72  * \return amount of written datas
     73  */
     74  ssize_t Write(const void *buf, size_t nbyte);
    7775
    78             /*!
    79             * \brief Read datas
    80             *
    81             * \param buf pointer to datas
    82             * \param nbyte length of datas
    83             *
    84             * \return amount of read datas
    85             */
    86             ssize_t Read(void *buf,size_t nbyte);
     76  /*!
     77  * \brief Read datas
     78  *
     79  * \param buf pointer to datas
     80  * \param nbyte length of datas
     81  *
     82  * \return amount of read datas
     83  */
     84  ssize_t Read(void *buf, size_t nbyte);
    8785
    88             /*!
    89             * \brief Flush input datas
    90             *
    91             */
    92             void FlushInput(void);
     86  /*!
     87  * \brief Flush input datas
     88  *
     89  */
     90  void FlushInput(void);
    9391
    94         private:
    95             int fd;
    96     };
     92private:
     93  int fd;
     94};
    9795} // end namespace core
    9896} // end namespace flair

  • trunk/lib/FlairCore/src/RangeFinderPlot.cpp

    r2 r15  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    Class displaying a 2D plot on the ground station for laser range finder like Hokuyo
     13//  purpose:    Class displaying a 2D plot on the ground station for laser range
     14//  finder like Hokuyo
    1415//
    1516//
    … …  
    2627using namespace flair::core;
    2728
    28 namespace flair { namespace gui {
     29namespace flair {
     30namespace gui {
    2931
    30 RangeFinderPlot::RangeFinderPlot(const LayoutPosition* position,string name,
    31                                  string x_name,float xmin,float xmax,
    32                                  string y_name,float ymin,float ymax,
    33                                  const cvmatrix* datas,float start_angle,float end_angle,uint32_t nb_samples):
    34                                  SendData(position,name,"RangeFinderPlot",200) {
    35     this->datas=datas;
     32RangeFinderPlot::RangeFinderPlot(const LayoutPosition *position, string name,
     33                                 string x_name, float xmin, float xmax,
     34                                 string y_name, float ymin, float ymax,
     35                                 const cvmatrix *datas, float start_angle,
     36                                 float end_angle, uint32_t nb_samples)
     37    : SendData(position, name, "RangeFinderPlot", 200) {
     38  this->datas = datas;
    3639
    37     SetSendSize(datas->GetDataType().GetSize());
     40  SetSendSize(datas->GetDataType().GetSize());
    3841
    39     SetVolatileXmlProp("xmin",xmin);
    40     SetVolatileXmlProp("xmax",xmax);
    41     SetVolatileXmlProp("ymin",ymin);
    42     SetVolatileXmlProp("ymax",ymax);
    43     SetVolatileXmlProp("x_name",x_name);
    44     SetVolatileXmlProp("y_name",y_name);
    45     SetVolatileXmlProp("start_angle",start_angle);
    46     SetVolatileXmlProp("end_angle",end_angle);
    47     SetVolatileXmlProp("nb_samples",nb_samples);
    48     SetVolatileXmlProp("type",datas->GetDataType().GetElementDataType().GetDescription());
    49     SendXml();
     42  SetVolatileXmlProp("xmin", xmin);
     43  SetVolatileXmlProp("xmax", xmax);
     44  SetVolatileXmlProp("ymin", ymin);
     45  SetVolatileXmlProp("ymax", ymax);
     46  SetVolatileXmlProp("x_name", x_name);
     47  SetVolatileXmlProp("y_name", y_name);
     48  SetVolatileXmlProp("start_angle", start_angle);
     49  SetVolatileXmlProp("end_angle", end_angle);
     50  SetVolatileXmlProp("nb_samples", nb_samples);
     51  SetVolatileXmlProp(
     52      "type", datas->GetDataType().GetElementDataType().GetDescription());
     53  SendXml();
    5054
    51     if(datas->Cols()!=1 || datas->Rows()!=nb_samples) Err("Wrong input matrix size\n");
     55  if (datas->Cols() != 1 || datas->Rows() != nb_samples)
     56    Err("Wrong input matrix size\n");
    5257}
    5358
    5459RangeFinderPlot::~RangeFinderPlot() {}
    5560
    56 void RangeFinderPlot::CopyDatas(char* buf) const {
    57     datas->GetMutex();
    58     memcpy(buf,datas->getCvMat()->data.ptr,datas->GetDataType().GetSize());
    59     datas->ReleaseMutex();
     61void RangeFinderPlot::CopyDatas(char *buf) const {
     62  datas->GetMutex();
     63  memcpy(buf, datas->getCvMat()->data.ptr, datas->GetDataType().GetSize());
     64  datas->ReleaseMutex();
    6065}
    6166

  • trunk/lib/FlairCore/src/RangeFinderPlot.h

    r2 r15  
    55/*!
    66 * \file RangeFinderPlot.h
    7  * \brief Class displaying a 2D plot on the ground station for laser range finder like Hokuyo
     7 * \brief Class displaying a 2D plot on the ground station for laser range
     8 * finder like Hokuyo
    89 * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
    910 * \date 2014/07/23
    … …  
    1718#include <stdint.h>
    1819
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class cvmatrix;
    24     }
     20namespace flair {
     21namespace core {
     22class cvmatrix;
     23}
    2524}
    2625
    27 namespace flair
    28 {
    29 namespace gui
    30 {
     26namespace flair {
     27namespace gui {
    3128
    32     class LayoutPosition;
     29class LayoutPosition;
    3330
    34     /*! \class RangeFinderPlot
    35     *
    36     * \brief Class displaying a 2D plot on the ground station for laser range finder like Hokuyo
    37     *
    38     */
    39     class RangeFinderPlot: public SendData
    40     {
    41         public:
    42             /*!
    43             * \brief Constructor
    44             *
    45             * Construct a 2D plot at given position. \n
    46             * The RangeFinderPlot will automatically be child of position->getLayout() Layout. After calling this constructor,
    47             * position will be deleted as it is no longer usefull.
    48             *
    49             * \param position position to display the plot
    50             * \param name name
    51             * \param x_name name of x axis
    52             * \param xmin default xmin of the plot
    53             * \param xmax default xmax of the plot
    54             * \param y_name name of y axis
    55             * \param ymin default ymin of the plot
    56             * \param ymax default ymax of the plot
    57             * \param datas laser datas
    58             * \param start_angle start angle of the laser
    59             * \param end_angle end angle of the laser
    60             * \param nb_samples number of samples
    61             */
    62             RangeFinderPlot(const LayoutPosition* position,std::string name,
    63                             std::string x_name,float xmin,float xmax,
    64                             std::string y_name,float ymin,float ymax,
    65                             const core::cvmatrix* datas,float start_angle,float end_angle,uint32_t nb_samples);
     31/*! \class RangeFinderPlot
     32*
     33* \brief Class displaying a 2D plot on the ground station for laser range finder
     34*like Hokuyo
     35*
     36*/
     37class RangeFinderPlot : public SendData {
     38public:
     39  /*!
     40  * \brief Constructor
     41  *
     42  * Construct a 2D plot at given position. \n
     43  * The RangeFinderPlot will automatically be child of position->getLayout()
     44  *Layout. After calling this constructor,
     45  * position will be deleted as it is no longer usefull.
     46  *
     47  * \param position position to display the plot
     48  * \param name name
     49  * \param x_name name of x axis
     50  * \param xmin default xmin of the plot
     51  * \param xmax default xmax of the plot
     52  * \param y_name name of y axis
     53  * \param ymin default ymin of the plot
     54  * \param ymax default ymax of the plot
     55  * \param datas laser datas
     56  * \param start_angle start angle of the laser
     57  * \param end_angle end angle of the laser
     58  * \param nb_samples number of samples
     59  */
     60  RangeFinderPlot(const LayoutPosition *position, std::string name,
     61                  std::string x_name, float xmin, float xmax,
     62                  std::string y_name, float ymin, float ymax,
     63                  const core::cvmatrix *datas, float start_angle,
     64                  float end_angle, uint32_t nb_samples);
    6665
    67             /*!
    68             * \brief Destructor
    69             *
    70             */
    71             ~RangeFinderPlot();
     66  /*!
     67  * \brief Destructor
     68  *
     69  */
     70  ~RangeFinderPlot();
    7271
    73         private:
    74             /*!
    75             * \brief Copy datas to specified buffer
    76             *
    77             * Reimplemented from SendData.
    78             *
    79             * \param buf output buffer
    80             */
    81             void CopyDatas(char* buf) const;
     72private:
     73  /*!
     74  * \brief Copy datas to specified buffer
     75  *
     76  * Reimplemented from SendData.
     77  *
     78  * \param buf output buffer
     79  */
     80  void CopyDatas(char *buf) const;
    8281
    83             /*!
    84             * \brief Extra Xml event
    85             *
    86             * Reimplemented from SendData.
    87             */
    88             void ExtraXmlEvent(void){};
     82  /*!
     83  * \brief Extra Xml event
     84  *
     85  * Reimplemented from SendData.
     86  */
     87  void ExtraXmlEvent(void){};
    8988
    90             const core::cvmatrix* datas;
    91     };
     89  const core::cvmatrix *datas;
     90};
    9291
    9392} // end namespace gui

  • trunk/lib/FlairCore/src/RotationMatrix.cpp

    r2 r15  
    2121#include "math.h"
    2222
    23 namespace flair
    24 {
    25 namespace core
    26 {
     23namespace flair {
     24namespace core {
    2725
    2826RotationMatrix::RotationMatrix() {
    29     for(int i=0;i<3;i++) {
    30         for(int j=0;j<3;j++) {
    31             if(i==j) {
    32                 m[i][j]=1;
    33             }else {
    34                 m[i][j]=0;
    35             }
    36         }
     27  for (int i = 0; i < 3; i++) {
     28    for (int j = 0; j < 3; j++) {
     29      if (i == j) {
     30        m[i][j] = 1;
     31      } else {
     32        m[i][j] = 0;
     33      }
    3734    }
     35  }
    3836}
    3937
    40 RotationMatrix::~RotationMatrix() {
    41 
    42 }
     38RotationMatrix::~RotationMatrix() {}
    4339
    4440void RotationMatrix::ToEuler(Euler &euler) const {
    45     euler.roll=atanf(m[1][2]/m[2][2]);
    46     euler.pitch=asinf(-m[0][2]);
    47     euler.yaw=atan2f(m[0][1],m[0][0]);
     41  euler.roll = atanf(m[1][2] / m[2][2]);
     42  euler.pitch = asinf(-m[0][2]);
     43  euler.yaw = atan2f(m[0][1], m[0][0]);
    4844}
    4945
    5046Euler RotationMatrix::ToEuler(void) const {
    51     Euler euler;
    52     ToEuler(euler);
    53     return euler;
     47  Euler euler;
     48  ToEuler(euler);
     49  return euler;
    5450}
    5551
    56 float& RotationMatrix::operator()(size_t row,size_t col) {
    57     if(row<3 && col<3) {
    58         return m[row][col];
    59     } else {
    60         Printf("RotationMatrix: index (%i,%i) out of bound\n",row,col);
    61         return m[2][2];
    62     }
     52float &RotationMatrix::operator()(size_t row, size_t col) {
     53  if (row < 3 && col < 3) {
     54    return m[row][col];
     55  } else {
     56    Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
     57    return m[2][2];
     58  }
    6359}
    6460
    65 const float& RotationMatrix::operator()(size_t row,size_t col) const {
    66     if(row<3 && col<3) {
    67         return m[row][col];
    68     } else {
    69         Printf("RotationMatrix: index (%i,%i) out of bound\n",row,col);
    70         return m[2][2];
    71     }
     61const float &RotationMatrix::operator()(size_t row, size_t col) const {
     62  if (row < 3 && col < 3) {
     63    return m[row][col];
     64  } else {
     65    Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
     66    return m[2][2];
     67  }
    7268}
    7369

  • trunk/lib/FlairCore/src/RotationMatrix.h

    r2 r15  
    1717namespace flair {
    1818namespace core {
    19     class Euler;
     19class Euler;
    2020
    21     /*! \class RotationMatrix
    22     *
    23     * \brief Class defining a rotation matrix
    24     */
    25     class RotationMatrix {
    26         public:
    27             /*!
    28             * \brief Constructor
    29             *
    30             * Construct an identity rotation matrix
    31             *
    32             */
    33             RotationMatrix();
     21/*! \class RotationMatrix
     22*
     23* \brief Class defining a rotation matrix
     24*/
     25class RotationMatrix {
     26public:
     27  /*!
     28  * \brief Constructor
     29  *
     30  * Construct an identity rotation matrix
     31  *
     32  */
     33  RotationMatrix();
    3434
    35             /*!
    36             * \brief Destructor
    37             *
    38             */
    39             ~RotationMatrix();
     35  /*!
     36  * \brief Destructor
     37  *
     38  */
     39  ~RotationMatrix();
    4040
    41             /*!
    42             * \brief Convert to euler angles
    43             *
    44             * \param euler output euler angles
    45             */
    46             void ToEuler(Euler &euler) const;
     41  /*!
     42  * \brief Convert to euler angles
     43  *
     44  * \param euler output euler angles
     45  */
     46  void ToEuler(Euler &euler) const;
    4747
    48             /*!
    49             * \brief Convert to euler angles
    50             *
    51             * \return euler angles
    52             */
    53             Euler ToEuler(void) const;
    54             /*!
    55             * \brief matrix
    56             *
    57             */
    58             float m[3][3];
     48  /*!
     49  * \brief Convert to euler angles
     50  *
     51  * \return euler angles
     52  */
     53  Euler ToEuler(void) const;
     54  /*!
     55  * \brief matrix
     56  *
     57  */
     58  float m[3][3];
    5959
    60             float& operator()(size_t row,size_t col);
    61             const float& operator()(size_t row,size_t col) const;
    62     };
     60  float &operator()(size_t row, size_t col);
     61  const float &operator()(size_t row, size_t col) const;
     62};
    6363
    6464} // end namespace core

  • trunk/lib/FlairCore/src/SendData.cpp

    r2 r15  
    2626using namespace flair::core;
    2727
    28 namespace flair
    29 {
    30 namespace gui
    31 {
     28namespace flair {
     29namespace gui {
    3230
    33 SendData::SendData(const LayoutPosition* position,string name,string type,uint16_t default_periodms,bool default_enabled) :Widget(position->getLayout(),name,type) {
    34     pimpl_=new SendData_impl();
     31SendData::SendData(const LayoutPosition *position, string name, string type,
     32                   uint16_t default_periodms, bool default_enabled)
     33    : Widget(position->getLayout(), name, type) {
     34  pimpl_ = new SendData_impl();
    3535
    36     pimpl_->send_size=0;
     36  pimpl_->send_size = 0;
    3737
    38     //default refesh rate: (ms)
    39     pimpl_->send_period=default_periodms;
    40     pimpl_->is_enabled=default_enabled;
     38  // default refesh rate: (ms)
     39  pimpl_->send_period = default_periodms;
     40  pimpl_->is_enabled = default_enabled;
    4141
    42     SetVolatileXmlProp("row",position->Row());
    43     SetVolatileXmlProp("col",position->Col());
    44     GetPersistentXmlProp("period",pimpl_->send_period);
    45     SetPersistentXmlProp("period",pimpl_->send_period);
    46     GetPersistentXmlProp("enabled",pimpl_->is_enabled);
    47     SetPersistentXmlProp("enabled",pimpl_->is_enabled);
     42  SetVolatileXmlProp("row", position->Row());
     43  SetVolatileXmlProp("col", position->Col());
     44  GetPersistentXmlProp("period", pimpl_->send_period);
     45  SetPersistentXmlProp("period", pimpl_->send_period);
     46  GetPersistentXmlProp("enabled", pimpl_->is_enabled);
     47  SetPersistentXmlProp("enabled", pimpl_->is_enabled);
    4848
    49     delete position;
     49  delete position;
    5050
    51     if(getUiCom()!=NULL) {
    52         //register SendData for sending to ground station
    53         getUiCom()->AddSendData(this);
    54         //resume if necessary
    55         getUiCom()->UpdateSendData(this);
    56     }
     51  if (getUiCom() != NULL) {
     52    // register SendData for sending to ground station
     53    getUiCom()->AddSendData(this);
     54    // resume if necessary
     55    getUiCom()->UpdateSendData(this);
     56  }
    5757}
    5858
    5959SendData::~SendData() {
    60     if(getUiCom()!=NULL) {
    61         //unregister SendData for sending to ground station
    62         getUiCom()->RemoveSendData(this);
    63     }
     60  if (getUiCom() != NULL) {
     61    // unregister SendData for sending to ground station
     62    getUiCom()->RemoveSendData(this);
     63  }
    6464
    65     delete pimpl_;
     65  delete pimpl_;
    6666}
    6767
    6868void SendData::XmlEvent(void) {
    69     uint16_t send_period;
    70     bool is_enabled;
    71     bool something_changed=false;
     69  uint16_t send_period;
     70  bool is_enabled;
     71  bool something_changed = false;
    7272
    73     if(GetPersistentXmlProp("period",send_period) && GetPersistentXmlProp("enabled",is_enabled)) {
    74         if(send_period!=SendPeriod()) something_changed=true;
    75         if(is_enabled!=IsEnabled()) something_changed=true;
    76     }
     73  if (GetPersistentXmlProp("period", send_period) &&
     74      GetPersistentXmlProp("enabled", is_enabled)) {
     75    if (send_period != SendPeriod())
     76      something_changed = true;
     77    if (is_enabled != IsEnabled())
     78      something_changed = true;
     79  }
    7780
    78     if(something_changed) {
    79         getFrameworkManager()->BlockCom();
     81  if (something_changed) {
     82    getFrameworkManager()->BlockCom();
    8083
    81         SetSendPeriod(send_period);
    82         SetEnabled(is_enabled);
     84    SetSendPeriod(send_period);
     85    SetEnabled(is_enabled);
    8386
    84         getFrameworkManager()->UpdateSendData(this);
     87    getFrameworkManager()->UpdateSendData(this);
    8588
    86         //ack pour la station sol
    87         //period and enabled are already in persistent prop
    88         SetVolatileXmlProp("period",send_period);
    89         SetVolatileXmlProp("enabled",is_enabled);
    90         SendXml();
     89    // ack pour la station sol
     90    // period and enabled are already in persistent prop
     91    SetVolatileXmlProp("period", send_period);
     92    SetVolatileXmlProp("enabled", is_enabled);
     93    SendXml();
    9194
    92         getFrameworkManager()->UnBlockCom();
    93     }
     95    getFrameworkManager()->UnBlockCom();
     96  }
    9497
    95     ExtraXmlEvent();
     98  ExtraXmlEvent();
    9699}
    97100
    98 size_t SendData::SendSize(void) const {
    99     return pimpl_->send_size;
     101size_t SendData::SendSize(void) const { return pimpl_->send_size; }
     102
     103uint16_t SendData::SendPeriod(void) const { return pimpl_->send_period; }
     104
     105bool SendData::IsEnabled(void) const { return pimpl_->is_enabled; }
     106
     107void SendData::SetEnabled(bool value) { pimpl_->is_enabled = value; }
     108
     109void SendData::SetSendSize(size_t value) {
     110  pimpl_->send_size = value;
     111
     112  if (getUiCom() != NULL)
     113    getUiCom()->UpdateDataToSendSize();
    100114}
    101115
    102 uint16_t SendData::SendPeriod(void) const {
    103     return pimpl_->send_period;
    104 }
    105 
    106 bool SendData::IsEnabled(void) const {
    107     return pimpl_->is_enabled;
    108 }
    109 
    110 void SendData::SetEnabled(bool value) {
    111     pimpl_->is_enabled=value;
    112 }
    113 
    114 void SendData::SetSendSize(size_t value) {
    115     pimpl_->send_size=value;
    116 
    117     if(getUiCom()!=NULL) getUiCom()->UpdateDataToSendSize();
    118 }
    119 
    120 void SendData::SetSendPeriod(uint16_t value) {
    121     pimpl_->send_period=value;
    122 }
     116void SendData::SetSendPeriod(uint16_t value) { pimpl_->send_period = value; }
    123117
    124118} // end namespace core

  • trunk/lib/FlairCore/src/SendData.h

    r2 r15  
    1818class SendData_impl;
    1919
    20 namespace flair
    21 {
    22 namespace gui
    23 {
    24     class LayoutPosition;
     20namespace flair {
     21namespace gui {
     22class LayoutPosition;
    2523
    26     /*! \class SendData
    27     *
    28     * \brief Abstract class for sending datas to ground station
    29     *
    30     */
    31     class SendData: public Widget
    32     {
    33         public:
    34             /*!
    35             * \brief Constructor
    36             *
    37             */
    38             SendData(const LayoutPosition* position,std::string name,std::string type,uint16_t default_periodms=100,bool default_enabled=false);
     24/*! \class SendData
     25*
     26* \brief Abstract class for sending datas to ground station
     27*
     28*/
     29class SendData : public Widget {
     30public:
     31  /*!
     32  * \brief Constructor
     33  *
     34  */
     35  SendData(const LayoutPosition *position, std::string name, std::string type,
     36           uint16_t default_periodms = 100, bool default_enabled = false);
    3937
    40             /*!
    41             * \brief Destructor
    42             *
    43             */
    44             virtual ~SendData();
     38  /*!
     39  * \brief Destructor
     40  *
     41  */
     42  virtual ~SendData();
    4543
    46             /*!
    47             * \brief Copy datas to specified buffer
    48             *
    49             * This method must be reimplemented, in order to send datas to ground station.
    50             *
    51             * \param buf output buffer
    52             */
    53             virtual void CopyDatas(char* buf) const =0;
     44  /*!
     45  * \brief Copy datas to specified buffer
     46  *
     47  * This method must be reimplemented, in order to send datas to ground station.
     48  *
     49  * \param buf output buffer
     50  */
     51  virtual void CopyDatas(char *buf) const = 0;
    5452
    55             size_t SendSize(void) const;
    56             uint16_t SendPeriod(void) const; // in ms
    57             bool IsEnabled(void) const;
     53  size_t SendSize(void) const;
     54  uint16_t SendPeriod(void) const; // in ms
     55  bool IsEnabled(void) const;
    5856
    59         protected:
     57protected:
     58  /*!
     59  * \brief Notify that SenData's datas have changed
     60  *
     61  * This method must be called when the datas have changed. \n
     62  * Normally, it occurs when a curve is added to a plot for example. \n
     63  * This method automatically blocks and unblocks the communication.
     64  *
     65  */
     66  void SetSendSize(size_t value);
    6067
    61             /*!
    62             * \brief Notify that SenData's datas have changed
    63             *
    64             * This method must be called when the datas have changed. \n
    65             * Normally, it occurs when a curve is added to a plot for example. \n
    66             * This method automatically blocks and unblocks the communication.
    67             *
    68             */
    69             void SetSendSize(size_t value);
     68  /*!
     69  * \brief Extra Xml event
     70  *
     71  * This method must be reimplemented to handle extra xml event. \n
     72  * It is automatically called when something changed from
     73  * ground station, through XmlEvent method. \n
     74  */
     75  virtual void ExtraXmlEvent(void) = 0;
    7076
    71             /*!
    72             * \brief Extra Xml event
    73             *
    74             * This method must be reimplemented to handle extra xml event. \n
    75             * It is automatically called when something changed from
    76             * ground station, through XmlEvent method. \n
    77             */
    78             virtual void ExtraXmlEvent(void)=0;
     77private:
     78  /*!
     79  * \brief XmlEvent from ground station
     80  *
     81  * Reimplemented from Widget. \n
     82  * This method handles period and enabled properties of the SendData. \n
     83  * Then it calls ExtraXmlEvent to handle specific xml events of reimplemented
     84  *class.
     85  *
     86  */
     87  void XmlEvent(void);
    7988
    80         private:
    81             /*!
    82             * \brief XmlEvent from ground station
    83             *
    84             * Reimplemented from Widget. \n
    85             * This method handles period and enabled properties of the SendData. \n
    86             * Then it calls ExtraXmlEvent to handle specific xml events of reimplemented class.
    87             *
    88             */
    89             void XmlEvent(void);
     89  void SetSendPeriod(uint16_t value);
     90  void SetEnabled(bool value);
    9091
    91             void SetSendPeriod(uint16_t value);
    92             void SetEnabled(bool value);
    93 
    94             class SendData_impl* pimpl_;
    95     };
     92  class SendData_impl *pimpl_;
     93};
    9694
    9795} // end namespace core

  • trunk/lib/FlairCore/src/SendData_impl.cpp

    r2 r15  
    1818#include "SendData_impl.h"
    1919
    20 SendData_impl::SendData_impl()
    21 {
    22 }
     20SendData_impl::SendData_impl() {}
    2321
    24 SendData_impl::~SendData_impl()
    25 {
    26 }
     22SendData_impl::~SendData_impl() {}

  • trunk/lib/FlairCore/src/SerialPort.h

    r2 r15  
    1717#include <stdint.h>
    1818
    19 namespace flair
    20 {
    21 namespace core
    22 {
    23     /*! \class SerialPort
    24     *
    25     * \brief Base class for serial port
    26     */
    27     class SerialPort: public Object
    28     {
    29         public:
    30             /*!
    31             * \brief Constructor
    32             *
    33             * Construct a serial port.
    34             *
    35             * \param parent parent
    36             * \param name name
    37             */
    38             SerialPort(const Object* parent,std::string name): Object(parent,name)
    39             {}
     19namespace flair {
     20namespace core {
     21/*! \class SerialPort
     22*
     23* \brief Base class for serial port
     24*/
     25class SerialPort : public Object {
     26public:
     27  /*!
     28  * \brief Constructor
     29  *
     30  * Construct a serial port.
     31  *
     32  * \param parent parent
     33  * \param name name
     34  */
     35  SerialPort(const Object *parent, std::string name) : Object(parent, name) {}
    4036
    41             /*!
    42             * \brief Destructor
    43             *
    44             */
    45             ~SerialPort(){};
     37  /*!
     38  * \brief Destructor
     39  *
     40  */
     41  ~SerialPort(){};
    4642
    47             /*!
    48             * \brief Set baudrate
    49             *
    50             * \param baudrate baudrate
    51             *
    52             */
    53             virtual void SetBaudrate(int baudrate)=0;
     43  /*!
     44  * \brief Set baudrate
     45  *
     46  * \param baudrate baudrate
     47  *
     48  */
     49  virtual void SetBaudrate(int baudrate) = 0;
    5450
    55             /*!
    56             * \brief Set RX timeout
    57             *
    58             * Timeout for waiting datas.
    59             *
    60             * \param timeout_ns timeout in nano second
    61             */
    62             virtual void SetRxTimeout(Time timeout_ns)=0;
     51  /*!
     52  * \brief Set RX timeout
     53  *
     54  * Timeout for waiting datas.
     55  *
     56  * \param timeout_ns timeout in nano second
     57  */
     58  virtual void SetRxTimeout(Time timeout_ns) = 0;
    6359
    64             /*!
    65             * \brief Write datas
    66             *
    67             * \param buf pointer to datas
    68             * \param nbyte length of datas
    69             *
    70             * \return amount of written datas
    71             */
    72             virtual ssize_t Write(const void *buf,size_t nbyte)=0;
     60  /*!
     61  * \brief Write datas
     62  *
     63  * \param buf pointer to datas
     64  * \param nbyte length of datas
     65  *
     66  * \return amount of written datas
     67  */
     68  virtual ssize_t Write(const void *buf, size_t nbyte) = 0;
    7369
    74             /*!
    75             * \brief Read datas
    76             *
    77             * \param buf pointer to datas
    78             * \param nbyte length of datas
    79             *
    80             * \return amount of read datas
    81             */
    82             virtual ssize_t Read(void *buf,size_t nbyte)=0;
     70  /*!
     71  * \brief Read datas
     72  *
     73  * \param buf pointer to datas
     74  * \param nbyte length of datas
     75  *
     76  * \return amount of read datas
     77  */
     78  virtual ssize_t Read(void *buf, size_t nbyte) = 0;
    8379
    84             /*!
    85             * \brief Flush input datas
    86             *
    87             */
    88             virtual void FlushInput(void)=0;
    89 
    90     };
     80  /*!
     81  * \brief Flush input datas
     82  *
     83  */
     84  virtual void FlushInput(void) = 0;
     85};
    9186} // end namespace core
    9287} // end namespace framework

  • trunk/lib/FlairCore/src/SharedMem.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair
    24 {
    25 namespace core
    26 {
     23namespace flair {
     24namespace core {
    2725
    28 SharedMem::SharedMem(const Object* parent,string name,size_t size): Object(parent,name,"shmem")
    29 {
    30     pimpl_=new SharedMem_impl(this,name,size);
     26SharedMem::SharedMem(const Object *parent, string name, size_t size)
     27    : Object(parent, name, "shmem") {
     28  pimpl_ = new SharedMem_impl(this, name, size);
    3129}
    3230
    33 SharedMem::~SharedMem()
    34 {
    35     delete pimpl_;
     31SharedMem::~SharedMem() { delete pimpl_; }
     32
     33void SharedMem::Write(const char *buf, size_t size) {
     34  pimpl_->Write(buf, size);
    3635}
    3736
    38 void SharedMem::Write(const char* buf,size_t size)
    39 {
    40     pimpl_->Write(buf,size);
    41 }
    42 
    43 void SharedMem::Read(char* buf,size_t size) const
    44 {
    45     pimpl_->Read(buf,size);
    46 }
     37void SharedMem::Read(char *buf, size_t size) const { pimpl_->Read(buf, size); }
    4738
    4839} // end namespace core

  • trunk/lib/FlairCore/src/SharedMem.h

    r2 r15  
    1919class SharedMem_impl;
    2020
    21 namespace flair
    22 {
    23 namespace core
    24 {
     21namespace flair {
     22namespace core {
    2523
    26     /*! \class SharedMem
    27     *
    28     * \brief Class defining a shared memory
    29     *
    30     * Shared memory is identified by its name so it can be accessed
    31     * by another processus using its name.
    32     */
     24/*! \class SharedMem
     25*
     26* \brief Class defining a shared memory
     27*
     28* Shared memory is identified by its name so it can be accessed
     29* by another processus using its name.
     30*/
    3331
    34     class SharedMem: public Object
    35     {
    36         public:
    37             /*!
    38             * \brief Constructor
    39             *
    40             * Construct a shared memory object
    41             *
    42             * \param parent parent
    43             * \param name name
    44             * \param size size of the shared memory
    45             */
    46             SharedMem(const Object* parent,std::string name,size_t size);
     32class SharedMem : public Object {
     33public:
     34  /*!
     35  * \brief Constructor
     36  *
     37  * Construct a shared memory object
     38  *
     39  * \param parent parent
     40  * \param name name
     41  * \param size size of the shared memory
     42  */
     43  SharedMem(const Object *parent, std::string name, size_t size);
    4744
    48             /*!
    49             * \brief Destructor
    50             *
    51             */
    52             ~SharedMem();
     45  /*!
     46  * \brief Destructor
     47  *
     48  */
     49  ~SharedMem();
    5350
    54             /*!
    55             * \brief Write
    56             *
    57             * \param buf input buffer to write to memory
    58             * \param size buffer size
    59             */
    60             void Write(const char* buf,size_t size);
     51  /*!
     52  * \brief Write
     53  *
     54  * \param buf input buffer to write to memory
     55  * \param size buffer size
     56  */
     57  void Write(const char *buf, size_t size);
    6158
    62             /*!
    63             * \brief Read
    64             *
    65             * \param buf output buffer to write from memory
    66             * \param size buffer size
    67             */
    68             void Read(char* buf,size_t size) const;
     59  /*!
     60  * \brief Read
     61  *
     62  * \param buf output buffer to write from memory
     63  * \param size buffer size
     64  */
     65  void Read(char *buf, size_t size) const;
    6966
    70         private:
    71             SharedMem_impl* pimpl_;
    72     };
     67private:
     68  SharedMem_impl *pimpl_;
     69};
    7370
    7471} // end namespace core

  • trunk/lib/FlairCore/src/SharedMem_impl.cpp

    r2 r15  
    2626using namespace flair::core;
    2727
    28 SharedMem_impl::SharedMem_impl(const SharedMem* self,string name,size_t size)
    29 {
    30     this->size=size;
    31     this->self=self;
     28SharedMem_impl::SharedMem_impl(const SharedMem *self, string name,
     29                               size_t size) {
     30  this->size = size;
     31  this->self = self;
    3232
    3333#ifdef __XENO__
    34     heap_binded=false;
    35     int status=rt_heap_create(&heap,name.c_str(),size,H_SHARED|H_FIFO|H_NONCACHED);
    36     if(status==-EEXIST)
    37     {
    38         heap_binded=true;
    39         status=rt_heap_bind(&heap,name.c_str(),TM_INFINITE);
    40     }
    41     if(status!=0)
    42     {
    43         self->Err("rt_heap_create error (%s)\n",strerror(-status));
    44         return;
    45     }
     34  heap_binded = false;
     35  int status = rt_heap_create(&heap, name.c_str(), size,
     36                              H_SHARED | H_FIFO | H_NONCACHED);
     37  if (status == -EEXIST) {
     38    heap_binded = true;
     39    status = rt_heap_bind(&heap, name.c_str(), TM_INFINITE);
     40  }
     41  if (status != 0) {
     42    self->Err("rt_heap_create error (%s)\n", strerror(-status));
     43    return;
     44  }
    4645
    47     void *ptr;
    48     status=rt_heap_alloc(&heap,0,TM_NONBLOCK ,&ptr);
    49     if(status!=0)
    50     {
    51         self->Err("rt_heap_alloc error (%s)\n",strerror(-status));
    52     }
    53     mem_segment =(char*)ptr;
     46  void *ptr;
     47  status = rt_heap_alloc(&heap, 0, TM_NONBLOCK, &ptr);
     48  if (status != 0) {
     49    self->Err("rt_heap_alloc error (%s)\n", strerror(-status));
     50  }
     51  mem_segment = (char *)ptr;
    5452
    55     mutex_binded=false;
    56     string mutex_name="mutex_"+ name;
    57     status=rt_mutex_create(&mutex,mutex_name.c_str());
    58     if(status==-EEXIST)
    59     {
    60         mutex_binded=true;
    61         status=rt_mutex_bind(&mutex,mutex_name.c_str(),TM_INFINITE);
    62     }
    63     if(status!=0)
    64     {
    65         self->Err("rt_mutex_create error (%s)\n",strerror(-status));
    66         return;
    67     }
     53  mutex_binded = false;
     54  string mutex_name = "mutex_" + name;
     55  status = rt_mutex_create(&mutex, mutex_name.c_str());
     56  if (status == -EEXIST) {
     57    mutex_binded = true;
     58    status = rt_mutex_bind(&mutex, mutex_name.c_str(), TM_INFINITE);
     59  }
     60  if (status != 0) {
     61    self->Err("rt_mutex_create error (%s)\n", strerror(-status));
     62    return;
     63  }
    6864
    6965#else
    70     shm_name="/" + name;
    71     fd = shm_open(shm_name.c_str(), O_RDWR | O_CREAT, 0666);
    72     if (fd == -1)
    73     {
    74         self->Err("Error creating shared memory\n");
    75     }
    76     ftruncate(fd, size);
     66  shm_name = "/" + name;
     67  fd = shm_open(shm_name.c_str(), O_RDWR | O_CREAT, 0666);
     68  if (fd == -1) {
     69    self->Err("Error creating shared memory\n");
     70  }
     71  ftruncate(fd, size);
    7772
    78     sem_name="/"  +name;
    79     sem = sem_open(sem_name.c_str(), O_CREAT, 0666, 1);
    80     if (sem == SEM_FAILED)
    81     {
    82         self->Err("Error creating semaphore\n");
    83     }
     73  sem_name = "/" + name;
     74  sem = sem_open(sem_name.c_str(), O_CREAT, 0666, 1);
     75  if (sem == SEM_FAILED) {
     76    self->Err("Error creating semaphore\n");
     77  }
    8478
    85     mem_segment =(char*)mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
    86     if (mem_segment == MAP_FAILED)
    87     {
    88         self->Err("Failed to map memory\n");
    89     }
     79  mem_segment =
     80      (char *)mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
     81  if (mem_segment == MAP_FAILED) {
     82    self->Err("Failed to map memory\n");
     83  }
    9084#endif
    9185}
    9286
    93 SharedMem_impl::~SharedMem_impl()
    94 {
    95     int status;
     87SharedMem_impl::~SharedMem_impl() {
     88  int status;
    9689#ifdef __XENO__
    97     /* unnecessary because heap is opened in H_SINGLE mode
    98     status=rt_heap_free(&heap,mem_segment);
    99     if(status!=0)
    100     {
    101         self->Err("rt_heap_free error (%s)\n",strerror(-status));
     90  /* unnecessary because heap is opened in H_SINGLE mode
     91  status=rt_heap_free(&heap,mem_segment);
     92  if(status!=0)
     93  {
     94      self->Err("rt_heap_free error (%s)\n",strerror(-status));
     95  }
     96  */
     97
     98  if (heap_binded == false) {
     99    status = rt_heap_delete(&heap);
     100    if (status != 0) {
     101      self->Err("rt_heap_delete error (%s)\n", strerror(-status));
    102102    }
    103     */
     103  }
    104104
    105     if(heap_binded==false)
    106     {
    107         status=rt_heap_delete(&heap);
    108         if(status!=0)
    109         {
    110             self->Err("rt_heap_delete error (%s)\n",strerror(-status));
    111         }
     105  if (mutex_binded == false) {
     106    status = rt_mutex_delete(&mutex);
     107    if (status != 0) {
     108      self->Err("error destroying mutex (%s)\n", strerror(-status));
    112109    }
     110  }
     111#else
     112  status = munmap(mem_segment, size);
     113  if (status != 0) {
     114    self->Err("Failed to unmap memory (%s)\n", strerror(-status));
     115  }
    113116
    114     if(mutex_binded==false)
    115     {
    116         status=rt_mutex_delete(&mutex);
    117         if(status!=0)
    118         {
    119             self->Err("error destroying mutex (%s)\n",strerror(-status));
    120         }
    121     }
    122 #else
    123     status = munmap(mem_segment, size);
    124     if(status!=0)
    125     {
    126         self->Err("Failed to unmap memory (%s)\n",strerror(-status));
    127     }
     117  status = close(fd);
     118  if (status != 0) {
     119    self->Err("Failed to close file (%s)\n", strerror(-status));
     120  }
    128121
    129     status = close(fd);
    130     if(status!=0)
    131     {
    132         self->Err("Failed to close file (%s)\n",strerror(-status));
    133     }
     122  // do not check erros as it can be done by another process
     123  status = shm_unlink(shm_name.c_str()); /*
     124   if(status!=0)
     125   {
     126       self->Err("Failed to unlink memory (%s)\n",strerror(-status));
     127   }
     128*/
     129  // do not check erros as it can be done by another process
     130  status = sem_unlink(sem_name.c_str()); /*
     131   if(status!=0)
     132   {
     133       self->Err("Failed to unlink semaphore (%s)\n",strerror(-status));
     134   }*/
    134135
    135     //do not check erros as it can be done by another process
    136     status = shm_unlink(shm_name.c_str());/*
    137     if(status!=0)
    138     {
    139         self->Err("Failed to unlink memory (%s)\n",strerror(-status));
    140     }
    141 */
    142     //do not check erros as it can be done by another process
    143     status = sem_unlink(sem_name.c_str());/*
    144     if(status!=0)
    145     {
    146         self->Err("Failed to unlink semaphore (%s)\n",strerror(-status));
    147     }*/
    148 
    149     status = sem_close(sem);
    150     if(status!=0)
    151     {
    152         self->Err("Failed to close semaphore (%s)\n",strerror(-status));
    153     }
     136  status = sem_close(sem);
     137  if (status != 0) {
     138    self->Err("Failed to close semaphore (%s)\n", strerror(-status));
     139  }
    154140#endif
    155141}
    156142
    157 
    158 void SharedMem_impl::Write(const char* buf,size_t size)
    159 {
     143void SharedMem_impl::Write(const char *buf, size_t size) {
    160144#ifdef __XENO__
    161     int status=rt_mutex_acquire(&mutex,TM_INFINITE);
    162     if(status!=0) self->Err("error (%s)\n",strerror(-status));
    163     memcpy(mem_segment,buf,size);
    164     status=rt_mutex_release(&mutex);
    165     if(status!=0) self->Err("error (%s)\n",strerror(-status));
     145  int status = rt_mutex_acquire(&mutex, TM_INFINITE);
     146  if (status != 0)
     147    self->Err("error (%s)\n", strerror(-status));
     148  memcpy(mem_segment, buf, size);
     149  status = rt_mutex_release(&mutex);
     150  if (status != 0)
     151    self->Err("error (%s)\n", strerror(-status));
    166152#else
    167     sem_wait(sem);
    168     memcpy(mem_segment,buf,size);
    169     sem_post(sem);
     153  sem_wait(sem);
     154  memcpy(mem_segment, buf, size);
     155  sem_post(sem);
    170156#endif
    171157}
    172158
    173 void SharedMem_impl::Read(char* buf,size_t size)
    174 {
     159void SharedMem_impl::Read(char *buf, size_t size) {
    175160#ifdef __XENO__
    176     int status=rt_mutex_acquire(&mutex,TM_INFINITE);
    177     if(status!=0) self->Err("error (%s)\n",strerror(-status));
    178     memcpy(buf,mem_segment,size);
    179     status=rt_mutex_release(&mutex);
    180     if(status!=0) self->Err("error (%s)\n",strerror(-status));
     161  int status = rt_mutex_acquire(&mutex, TM_INFINITE);
     162  if (status != 0)
     163    self->Err("error (%s)\n", strerror(-status));
     164  memcpy(buf, mem_segment, size);
     165  status = rt_mutex_release(&mutex);
     166  if (status != 0)
     167    self->Err("error (%s)\n", strerror(-status));
    181168#else
    182     sem_wait(sem);
    183     memcpy(buf,mem_segment,size);
    184     sem_post(sem);
     169  sem_wait(sem);
     170  memcpy(buf, mem_segment, size);
     171  sem_post(sem);
    185172#endif
    186173}

  • trunk/lib/FlairCore/src/Socket.cpp

    r2 r15  
    2323using std::string;
    2424
    25 namespace flair
    26 {
    27 namespace core
    28 {
     25namespace flair {
     26namespace core {
    2927
    30 Socket::Socket(const Object* parent, string name,uint16_t port): Object(parent,name) {
    31     pimpl_=new Socket_impl(this,name,port);
     28Socket::Socket(const Object *parent, string name, uint16_t port)
     29    : Object(parent, name) {
     30  pimpl_ = new Socket_impl(this, name, port);
    3231}
    3332
    34 Socket::Socket(const Object* parent, string name,string address,bool broadcast): Object(parent,name) {
    35     pimpl_=new Socket_impl(this,name,address,broadcast);
     33Socket::Socket(const Object *parent, string name, string address,
     34               bool broadcast)
     35    : Object(parent, name) {
     36  pimpl_ = new Socket_impl(this, name, address, broadcast);
    3637}
    3738
    38 Socket::~Socket() {
    39     delete pimpl_;
     39Socket::~Socket() { delete pimpl_; }
     40
     41void Socket::SendMessage(const char *message, size_t message_len) {
     42  pimpl_->SendMessage(message, message_len);
    4043}
    4144
    42 void Socket::SendMessage(const char* message,size_t message_len) {
    43     pimpl_->SendMessage(message,message_len);
     45void Socket::SendMessage(string message) { pimpl_->SendMessage(message); }
     46
     47ssize_t Socket::RecvMessage(char *buf, size_t buf_len, Time timeout, char *src,
     48                            size_t *src_len) {
     49  return pimpl_->RecvMessage(buf, buf_len, timeout, src, src_len);
    4450}
    4551
    46 void Socket::SendMessage(string message) {
    47     pimpl_->SendMessage(message);
     52void Socket::NetworkToHost(char *data, size_t dataSize) {
     53  if (core::IsBigEndian())
     54    return;
     55  if (dataSize == 1)
     56    return;
     57  if ((dataSize == 2) || (dataSize == 4) || (dataSize == 8) ||
     58      (dataSize == 16)) {
     59    char dataInHostEndianness[dataSize];
     60    for (unsigned int i = 0; i < dataSize; i++) {
     61      dataInHostEndianness[i] = data[dataSize - i - 1];
     62    }
     63    memcpy(data, dataInHostEndianness, dataSize);
     64    return;
     65  }
     66  throw std::runtime_error(
     67      string("Unsupported data size (") + std::to_string(dataSize) +
     68      string(") in host to network endianness conversion"));
    4869}
    4970
    50 ssize_t Socket::RecvMessage(char* buf,size_t buf_len,Time timeout,char* src,size_t* src_len) {
    51     return pimpl_->RecvMessage(buf,buf_len,timeout,src,src_len);
    52 }
    53 
    54 void Socket::NetworkToHost(char *data,size_t dataSize) {
    55     if (core::IsBigEndian()) return;
    56     if (dataSize==1) return;
    57     if ((dataSize==2)||(dataSize==4)||(dataSize==8)||(dataSize==16)) {
    58         char dataInHostEndianness[dataSize];
    59         for (unsigned int i=0;i<dataSize;i++) {
    60             dataInHostEndianness[i]=data[dataSize-i-1];
    61         }
    62         memcpy(data,dataInHostEndianness,dataSize);
    63         return;
     71void Socket::HostToNetwork(char *data, size_t dataSize) {
     72  if (IsBigEndian())
     73    return;
     74  if (dataSize == 1)
     75    return;
     76  if ((dataSize == 2) || (dataSize == 4) || (dataSize == 8) ||
     77      (dataSize == 16)) {
     78    char dataInNetworkEndianness[dataSize];
     79    for (unsigned int i = 0; i < dataSize; i++) {
     80      dataInNetworkEndianness[i] = data[dataSize - i - 1];
    6481    }
    65     throw std::runtime_error(string("Unsupported data size (")+std::to_string(dataSize)+string(") in host to network endianness conversion"));
    66 }
    67 
    68 void Socket::HostToNetwork(char *data,size_t dataSize) {
    69     if (IsBigEndian()) return;
    70     if (dataSize==1) return;
    71     if ((dataSize==2)||(dataSize==4)||(dataSize==8)||(dataSize==16)) {
    72         char dataInNetworkEndianness[dataSize];
    73         for (unsigned int i=0;i<dataSize;i++) {
    74             dataInNetworkEndianness[i]=data[dataSize-i-1];
    75         }
    76         memcpy(data,dataInNetworkEndianness,dataSize);
    77         return;
    78     }
    79     throw std::runtime_error(string("Unsupported data size (")+std::to_string(dataSize)+string(") in host to network endianness conversion"));
     82    memcpy(data, dataInNetworkEndianness, dataSize);
     83    return;
     84  }
     85  throw std::runtime_error(
     86      string("Unsupported data size (") + std::to_string(dataSize) +
     87      string(") in host to network endianness conversion"));
    8088}
    8189

  • trunk/lib/FlairCore/src/Socket.h

    r2 r15  
    2020class Socket_impl;
    2121
    22 namespace flair
    23 {
    24 namespace core
    25 {
     22namespace flair {
     23namespace core {
    2624
    27     /*! \class Socket
    28     *
    29     * \brief Class encapsulating a UDP socket. It assumes packets are coming from only one distant host on a given port.
    30     *
    31     */
    32     class Socket: public Object {
    33     public:
    34         /*!
    35         * \brief Constructor
    36         *
    37         * Construct the client side of the socket
    38         *
    39         * \param parent parent
    40         * \param name name
    41         * \param address server address (ex 192.168.1.1:9000)
    42         * \param broadcast true if address is a broadcast address
    43         */
    44         Socket(const Object* parent, std::string name,std::string address,bool broadcast=false);
     25/*! \class Socket
     26*
     27* \brief Class encapsulating a UDP socket. It assumes packets are coming from
     28*only one distant host on a given port.
     29*
     30*/
     31class Socket : public Object {
     32public:
     33  /*!
     34  * \brief Constructor
     35  *
     36  * Construct the client side of the socket
     37  *
     38  * \param parent parent
     39  * \param name name
     40  * \param address server address (ex 192.168.1.1:9000)
     41  * \param broadcast true if address is a broadcast address
     42  */
     43  Socket(const Object *parent, std::string name, std::string address,
     44         bool broadcast = false);
    4545
    46         /*!
    47         * \brief Constructor
    48         *
    49         * Construct the server side of the socket
    50         *
    51         * \param parent parent
    52         * \param name name
    53         * \param port listening port
    54         */
    55         Socket(const Object* parent, std::string name,uint16_t port);
     46  /*!
     47  * \brief Constructor
     48  *
     49  * Construct the server side of the socket
     50  *
     51  * \param parent parent
     52  * \param name name
     53  * \param port listening port
     54  */
     55  Socket(const Object *parent, std::string name, uint16_t port);
    5656
    57         /*!
    58         * \brief Destructor
    59         *
    60         */
    61         ~Socket();
     57  /*!
     58  * \brief Destructor
     59  *
     60  */
     61  ~Socket();
    6262
    63         /*!
    64         * \brief Send a message
    65         *
    66         * In case of a broadcast Socket, Parent()->ObjectName() is used as source of the message, this name should be unique.
    67         *
    68         * \param message message
    69         */
    70         void SendMessage(std::string message);
     63  /*!
     64  * \brief Send a message
     65  *
     66  * In case of a broadcast Socket, Parent()->ObjectName() is used as source of
     67  *the message, this name should be unique.
     68  *
     69  * \param message message
     70  */
     71  void SendMessage(std::string message);
    7172
    72         /*!
    73         * \brief Send a message
    74         *
    75         * \param message message
    76         * \param message_len message length
    77         */
    78         void SendMessage(const char* message,size_t message_len);
     73  /*!
     74  * \brief Send a message
     75  *
     76  * \param message message
     77  * \param message_len message length
     78  */
     79  void SendMessage(const char *message, size_t message_len);
    7980
    80         /*!
    81         * \brief Receive a message
    82         *
    83         * Receive a message and wait up to timeout. \n
    84         * If src and src_len are specified, the source of the message will be
    85         * copied in the src buffer. \n
    86         * Note that in case of a broadcast socket, own messages are filtered and
    87         * are not received.
    88         *
    89         * \param buf buffer to put the message
    90         * \param buf_len buffer length
    91         * \param timeout timeout
    92         * \param src buffer to put source name
    93         * \param src_len buffer length
    94         *
    95         * \return size of the received message
    96         */
    97         ssize_t RecvMessage(char* buf,size_t buf_len,Time timeout,char* src=NULL,size_t* src_len=NULL);
     81  /*!
     82  * \brief Receive a message
     83  *
     84  * Receive a message and wait up to timeout. \n
     85  * If src and src_len are specified, the source of the message will be
     86  * copied in the src buffer. \n
     87  * Note that in case of a broadcast socket, own messages are filtered and
     88  * are not received.
     89  *
     90  * \param buf buffer to put the message
     91  * \param buf_len buffer length
     92  * \param timeout timeout
     93  * \param src buffer to put source name
     94  * \param src_len buffer length
     95  *
     96  * \return size of the received message
     97  */
     98  ssize_t RecvMessage(char *buf, size_t buf_len, Time timeout, char *src = NULL,
     99                      size_t *src_len = NULL);
    98100
    99         void NetworkToHost(char *data,size_t dataSize);
    100         void HostToNetwork(char *data,size_t dataSize);
     101  void NetworkToHost(char *data, size_t dataSize);
     102  void HostToNetwork(char *data, size_t dataSize);
    101103
    102     private:
    103        class Socket_impl* pimpl_;
    104     };
     104private:
     105  class Socket_impl *pimpl_;
     106};
    105107
    106108} // end namespace core

  • trunk/lib/FlairCore/src/Socket_impl.cpp

    r2 r15  
    2929using namespace flair::core;
    3030
    31 Socket_impl::Socket_impl(const Socket* self,string name,uint16_t port)
    32 {
    33     this->self=self;
    34     this->port=port;
    35     this->address="";
    36     this->broadcast=false;
    37     Init();
    38 }
    39 
    40 Socket_impl::Socket_impl(const Socket* self,string name,string address,bool broadcast)
    41 {
    42     this->self=self;
    43     int pos = address.find(":");
    44     this->address=address.substr (0,pos);
    45     port=atoi(address.substr(pos+1).c_str());
    46     this->broadcast=broadcast;
    47 
    48     if(pos==0 || address=="")
    49     {
    50         self->Err("address %s is not correct\n",address.c_str());
    51     }
    52     Init();
    53 
    54 }
    55 
    56 void Socket_impl::Init(void)
    57 {
    58         int yes=1;
    59 
    60     fd = socket(AF_INET, SOCK_DGRAM, 0); //UDP
    61 
    62     if(broadcast==true)
    63     {
    64         if(setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &yes, sizeof(int) )!=0)
    65             self->Err("Setsockopt error\n");
    66     }
    67 
    68     if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int) )!=0)
    69         self->Err("Setsockopt error\n");
    70 
    71     if(address=="" || broadcast==true)
    72     {
    73         sockaddr_in sin = { 0 };
    74 
    75         if(broadcast==true)
    76         {
    77             struct hostent *hostinfo;
    78 
    79             hostinfo = gethostbyname(this->address.c_str());
    80             if (hostinfo == NULL)
    81             {
    82                 self->Err("hostinfo error\n");
    83             }
    84             sin.sin_addr = *(in_addr *) hostinfo->h_addr;
    85         }
    86         else
    87         {
    88             sin.sin_addr.s_addr=INADDR_ANY;
    89         }
    90 
    91         sin.sin_port = htons(port);
    92         sin.sin_family = AF_INET;
    93         if(bind(fd,(sockaddr *) &sin, sizeof sin) == -1)
    94         {
    95             self->Err("bind error\n");
    96         }
    97     }
    98 
    99 
    100 #ifdef __XENO__
    101     string tmp_name;
    102     int status;
    103     is_running=true;
    104 
    105     //pipe
    106     tmp_name= getFrameworkManager()->ObjectName() + "-" + self->ObjectName()+ "-pipe";
    107     //xenomai limitation
    108     if(tmp_name.size()>31) self->Err("rt_pipe_create error (%s is too long)\n",tmp_name.c_str());
     31Socket_impl::Socket_impl(const Socket *self, string name, uint16_t port) {
     32  this->self = self;
     33  this->port = port;
     34  this->address = "";
     35  this->broadcast = false;
     36  Init();
     37}
     38
     39Socket_impl::Socket_impl(const Socket *self, string name, string address,
     40                         bool broadcast) {
     41  this->self = self;
     42  int pos = address.find(":");
     43  this->address = address.substr(0, pos);
     44  port = atoi(address.substr(pos + 1).c_str());
     45  this->broadcast = broadcast;
     46
     47  if (pos == 0 || address == "") {
     48    self->Err("address %s is not correct\n", address.c_str());
     49  }
     50  Init();
     51}
     52
     53void Socket_impl::Init(void) {
     54  int yes = 1;
     55
     56  fd = socket(AF_INET, SOCK_DGRAM, 0); // UDP
     57
     58  if (broadcast == true) {
     59    if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &yes, sizeof(int)) != 0)
     60      self->Err("Setsockopt error\n");
     61  }
     62
     63  if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int)) != 0)
     64    self->Err("Setsockopt error\n");
     65
     66  if (address == "" || broadcast == true) {
     67    sockaddr_in sin = {0};
     68
     69    if (broadcast == true) {
     70      struct hostent *hostinfo;
     71
     72      hostinfo = gethostbyname(this->address.c_str());
     73      if (hostinfo == NULL) {
     74        self->Err("hostinfo error\n");
     75      }
     76      sin.sin_addr = *(in_addr *)hostinfo->h_addr;
     77    } else {
     78      sin.sin_addr.s_addr = INADDR_ANY;
     79    }
     80
     81    sin.sin_port = htons(port);
     82    sin.sin_family = AF_INET;
     83    if (bind(fd, (sockaddr *)&sin, sizeof sin) == -1) {
     84      self->Err("bind error\n");
     85    }
     86  }
     87
     88#ifdef __XENO__
     89  string tmp_name;
     90  int status;
     91  is_running = true;
     92
     93  // pipe
     94  tmp_name =
     95      getFrameworkManager()->ObjectName() + "-" + self->ObjectName() + "-pipe";
     96  // xenomai limitation
     97  if (tmp_name.size() > 31)
     98    self->Err("rt_pipe_create error (%s is too long)\n", tmp_name.c_str());
    10999#ifdef RT_PIPE_SIZE
    110     status=rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO,RT_PIPE_SIZE);
     100  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
    111101#else
    112     status=rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO,0);
    113 #endif
    114     if(status!=0)
    115     {
    116         self->Err("rt_pipe_create error (%s)\n",strerror(-status));
    117     }
    118 
    119     //start user side thread
     102  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, 0);
     103#endif
     104  if (status != 0) {
     105    self->Err("rt_pipe_create error (%s)\n", strerror(-status));
     106  }
     107
     108// start user side thread
    120109#ifdef NRT_STACK_SIZE
    121     // Initialize thread creation attributes
    122     pthread_attr_t attr;
    123     if(pthread_attr_init(&attr) != 0)
    124     {
    125         self->Err("pthread_attr_init error\n");
    126     }
    127     if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0)
    128     {
    129         self->Err("pthread_attr_setstacksize error\n");
    130     }
    131     if(pthread_create(&user_thread,  &attr, user, (void*)this) < 0)
    132     {
    133         self->Err("pthread_create error\n");
    134     }
    135     if(pthread_attr_destroy(&attr) != 0)
    136     {
    137         self->Err("pthread_attr_destroy error\n");
    138     }
    139 #else //NRT_STACK_SIZE
    140     if(pthread_create(&user_thread, NULL, user, (void*)this) < 0)
    141     {
    142         self->Err("pthread_create error\n");
    143     }
    144 #endif //NRT_STACK_SIZE
     110  // Initialize thread creation attributes
     111  pthread_attr_t attr;
     112  if (pthread_attr_init(&attr) != 0) {
     113    self->Err("pthread_attr_init error\n");
     114  }
     115  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
     116    self->Err("pthread_attr_setstacksize error\n");
     117  }
     118  if (pthread_create(&user_thread, &attr, user, (void *)this) < 0) {
     119    self->Err("pthread_create error\n");
     120  }
     121  if (pthread_attr_destroy(&attr) != 0) {
     122    self->Err("pthread_attr_destroy error\n");
     123  }
     124#else  // NRT_STACK_SIZE
     125  if (pthread_create(&user_thread, NULL, user, (void *)this) < 0) {
     126    self->Err("pthread_create error\n");
     127  }
     128#endif // NRT_STACK_SIZE
    145129#endif //__XENO__
    146130
    147     if(address!="")
    148     {
    149         struct hostent *hostinfo;
    150         hostinfo = gethostbyname(address.c_str());
    151         if (hostinfo == NULL)
    152         {
    153             self->Err("gethostbyname\n");
    154         }
    155 
    156         sock_in.sin_addr=*(in_addr *) hostinfo->h_addr;
    157         sock_in.sin_port = htons(port);
    158         sock_in.sin_family = AF_INET;
    159     }
    160 }
    161 
    162 Socket_impl::~Socket_impl()
    163 {
    164 #ifdef __XENO__
    165     is_running=false;
    166 
    167     pthread_join(user_thread,NULL);
    168     int status=rt_pipe_delete(&pipe);
    169     if(status!=0) self->Err("rt_pipe_delete error (%s)\n",strerror(-status));
    170 
    171 #endif
    172     close(fd);
    173 }
    174 
    175 void Socket_impl::SendMessage(const char* src,size_t src_len)
    176 {
    177     ssize_t written;
    178     string to_send;
    179 
    180     if(broadcast==true)
    181     {
    182         to_send=getFrameworkManager()->ObjectName()+ ":" + string(src, src_len);
    183         src_len=to_send.size();
    184         src=(char*)to_send.c_str();
    185     }
    186 
    187 #ifdef __XENO__
    188 //Printf("send pipe %s\n",src);
    189     written=rt_pipe_write(&pipe,src,src_len,P_NORMAL);
    190 
    191     if(written<0)
    192     {
    193         self->Err("rt_pipe_write error (%s)\n",strerror(-written));
    194     }
    195     else if (written != (ssize_t)src_len)
    196     {
    197         self->Err("rt_pipe_write error %i/%i\n",written,to_send.size());
    198     }
     131  if (address != "") {
     132    struct hostent *hostinfo;
     133    hostinfo = gethostbyname(address.c_str());
     134    if (hostinfo == NULL) {
     135      self->Err("gethostbyname\n");
     136    }
     137
     138    sock_in.sin_addr = *(in_addr *)hostinfo->h_addr;
     139    sock_in.sin_port = htons(port);
     140    sock_in.sin_family = AF_INET;
     141  }
     142}
     143
     144Socket_impl::~Socket_impl() {
     145#ifdef __XENO__
     146  is_running = false;
     147
     148  pthread_join(user_thread, NULL);
     149  int status = rt_pipe_delete(&pipe);
     150  if (status != 0)
     151    self->Err("rt_pipe_delete error (%s)\n", strerror(-status));
     152
     153#endif
     154  close(fd);
     155}
     156
     157void Socket_impl::SendMessage(const char *src, size_t src_len) {
     158  ssize_t written;
     159  string to_send;
     160
     161  if (broadcast == true) {
     162    to_send = getFrameworkManager()->ObjectName() + ":" + string(src, src_len);
     163    src_len = to_send.size();
     164    src = (char *)to_send.c_str();
     165  }
     166
     167#ifdef __XENO__
     168  // Printf("send pipe %s\n",src);
     169  written = rt_pipe_write(&pipe, src, src_len, P_NORMAL);
     170
     171  if (written < 0) {
     172    self->Err("rt_pipe_write error (%s)\n", strerror(-written));
     173  } else if (written != (ssize_t)src_len) {
     174    self->Err("rt_pipe_write error %i/%i\n", written, to_send.size());
     175  }
    199176#else
    200     written=sendto(fd,src,src_len, 0, (struct sockaddr *) &sock_in, sizeof(sock_in));
    201 
    202     if(written!=(ssize_t)src_len)
    203     {
    204         self->Err("sendto error\n");
    205     }
    206 #endif
    207 }
    208 
    209 void Socket_impl::SendMessage(string message)
    210 {
    211     ssize_t written;
    212 
    213     if(broadcast==true) message=self->Parent()->ObjectName()+ ":" + message;
    214     //Printf("SendMessage %s\n",message.c_str());
    215 #ifdef __XENO__
    216     written=rt_pipe_write(&pipe,message.c_str(),message.size(),P_NORMAL);
    217 
    218     if(written<0)
    219     {
    220         self->Err("rt_pipe_write error (%s)\n",strerror(-written));
    221     }
    222     else if (written != (ssize_t)message.size())
    223     {
    224         self->Err("rt_pipe_write error %i/%i\n",written,message.size());
    225     }
     177  written =
     178      sendto(fd, src, src_len, 0, (struct sockaddr *)&sock_in, sizeof(sock_in));
     179
     180  if (written != (ssize_t)src_len) {
     181    self->Err("sendto error\n");
     182  }
     183#endif
     184}
     185
     186void Socket_impl::SendMessage(string message) {
     187  ssize_t written;
     188
     189  if (broadcast == true)
     190    message = self->Parent()->ObjectName() + ":" + message;
     191// Printf("SendMessage %s\n",message.c_str());
     192#ifdef __XENO__
     193  written = rt_pipe_write(&pipe, message.c_str(), message.size(), P_NORMAL);
     194
     195  if (written < 0) {
     196    self->Err("rt_pipe_write error (%s)\n", strerror(-written));
     197  } else if (written != (ssize_t)message.size()) {
     198    self->Err("rt_pipe_write error %i/%i\n", written, message.size());
     199  }
    226200#else
    227     written=sendto(fd,message.c_str(),message.size(), 0, (struct sockaddr *) &sock_in, sizeof(sock_in));
    228     if(written!=(ssize_t)message.size())
    229     {
    230         self->Err("sendto error\n");
    231     }
    232 
    233 #endif
    234 }
    235 
    236 ssize_t Socket_impl::RecvMessage(char* msg,size_t msg_len,Time timeout,char* src,size_t* src_len)
    237 {
    238     ssize_t nb_read;
    239     char buffer[128];
    240 #ifdef __XENO__
    241     nb_read=rt_pipe_read(&pipe,&buffer,sizeof(buffer),timeout);
     201  written = sendto(fd, message.c_str(), message.size(), 0,
     202                   (struct sockaddr *)&sock_in, sizeof(sock_in));
     203  if (written != (ssize_t)message.size()) {
     204    self->Err("sendto error\n");
     205  }
     206
     207#endif
     208}
     209
     210ssize_t Socket_impl::RecvMessage(char *msg, size_t msg_len, Time timeout,
     211                                 char *src, size_t *src_len) {
     212  ssize_t nb_read;
     213  char buffer[128];
     214#ifdef __XENO__
     215  nb_read = rt_pipe_read(&pipe, &buffer, sizeof(buffer), timeout);
    242216#else
    243     socklen_t sinsize = sizeof(sock_in);
    244     struct timeval tv;
    245 
    246     if(timeout!=TIME_NONBLOCK) {
    247         int attr=fcntl(fd, F_GETFL,0);
    248         fcntl(fd, F_SETFL, attr & (~O_NONBLOCK));
    249 
    250         tv.tv_sec = timeout/1000000000;
    251         timeout=timeout-(timeout/1000000000)*1000000000;
    252         tv.tv_usec = timeout/1000;
    253         if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO,&tv,sizeof(tv)) < 0) {
    254             self->Err("setsockopt SO_RCVTIMEO failed\n");
    255         }
     217  socklen_t sinsize = sizeof(sock_in);
     218  struct timeval tv;
     219
     220  if (timeout != TIME_NONBLOCK) {
     221    int attr = fcntl(fd, F_GETFL, 0);
     222    fcntl(fd, F_SETFL, attr & (~O_NONBLOCK));
     223
     224    tv.tv_sec = timeout / 1000000000;
     225    timeout = timeout - (timeout / 1000000000) * 1000000000;
     226    tv.tv_usec = timeout / 1000;
     227    if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
     228      self->Err("setsockopt SO_RCVTIMEO failed\n");
     229    }
     230  } else {
     231    fcntl(fd, F_SETFL, O_NONBLOCK);
     232  }
     233
     234  if (broadcast == false) {
     235    nb_read =
     236        recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *)&sock_in, &sinsize);
     237  } else {
     238    nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, NULL, NULL);
     239  }
     240#endif
     241  if (nb_read <= 0) {
     242    return nb_read;
     243  } else {
     244    // printf("%s\n",buffer);
     245    if (broadcast == true) {
     246      int index = -1;
     247      for (int i = 0; i < nb_read; i++) {
     248        if (buffer[i] == ':') {
     249          index = i;
     250          break;
     251        }
     252      }
     253      if (index < 0) {
     254        self->Warn("Malformed message\n");
     255        return -1;
     256      } else if (src_len != NULL && src != NULL) {
     257        if (index + 1 > (int)(*src_len) &&
     258            src != NULL) { //+1 pour inserer un 0)
     259          self->Warn("insufficent src size\n");
     260          return -1;
     261        }
     262      } else if (nb_read - index - 1 + 1 >
     263                 (int)msg_len) { //+1 pour inserer un 0
     264        self->Warn("insufficent msg size (%i/%i)\n", nb_read - index - 1 + 1,
     265                   msg_len);
     266        return -1;
     267      }
     268      if (src != NULL) {
     269        memcpy(src, buffer, index);
     270        src[index] = 0;
     271      }
     272      memcpy(msg, &buffer[index + 1], nb_read - index - 1);
     273      msg[nb_read - index - 1] = 0;
     274      return nb_read - index;
    256275    } else {
    257         fcntl(fd, F_SETFL, O_NONBLOCK);
    258     }
    259 
    260     if(broadcast==false) {
    261         nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *) &sock_in, &sinsize);
     276      if (nb_read > (int)msg_len) {
     277        self->Warn("insufficent msg size (%i/%i)\n", nb_read, msg_len);
     278        return -1;
     279      }
     280      memcpy(msg, buffer, nb_read);
     281      return nb_read;
     282    }
     283  }
     284}
     285
     286#ifdef __XENO__
     287void *Socket_impl::user(void *arg) {
     288  Socket_impl *caller = (Socket_impl *)arg;
     289  int pipe_fd = -1;
     290  string devname;
     291
     292  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
     293            caller->self->ObjectName() + "-pipe";
     294  while (pipe_fd < 0) {
     295    pipe_fd = open(devname.c_str(), O_RDWR);
     296    if (pipe_fd < 0 && errno != ENOENT)
     297      caller->self->Err("open pipe_fd error (%s)\n", strerror(-errno));
     298    usleep(1000);
     299  }
     300
     301  while (caller->is_running == true) {
     302    fd_set set;
     303    struct timeval timeout;
     304    int rv;
     305
     306    FD_ZERO(&set);            // clear the set
     307    FD_SET(caller->fd, &set); // add our file descriptor to the set
     308    FD_SET(pipe_fd, &set);    // add our file descriptor to the set
     309
     310    timeout.tv_sec = 0;
     311    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
     312    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
     313
     314    if (rv == -1) {
     315      caller->self->Err("select error\n"); // an error occured
     316    } else if (rv == 0) {
     317      // printf("timeout\n");
    262318    } else {
    263         nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, NULL,NULL);
    264     }
    265 #endif
    266     if(nb_read<=0) {
    267         return nb_read;
    268     } else {
    269         //printf("%s\n",buffer);
    270         if(broadcast==true) {
    271             int index=-1;
    272             for(int i=0;i<nb_read;i++) {
    273                 if(buffer[i]==':') {
    274                     index=i;
    275                     break;
    276                 }
    277             }
    278             if(index<0) {
    279                 self->Warn("Malformed message\n");
    280                 return -1;
    281             } else if(src_len!=NULL && src!=NULL) {
    282                 if(index+1>(int)(*src_len) && src!=NULL) { //+1 pour inserer un 0)
    283                     self->Warn("insufficent src size\n");
    284                     return -1;
    285                 }
    286             } else if(nb_read-index-1+1>(int)msg_len) { //+1 pour inserer un 0
    287                 self->Warn("insufficent msg size (%i/%i)\n",nb_read-index-1+1,msg_len);
    288                 return -1;
    289             }
    290             if(src!=NULL) {
    291                 memcpy(src,buffer,index);
    292                 src[index]=0;
    293             }
    294             memcpy(msg,&buffer[index+1],nb_read-index-1);
    295             msg[nb_read-index-1]=0;
    296             return nb_read-index;
     319      ssize_t nb_read, nb_write;
     320      char buffer[1024];
     321
     322      if (FD_ISSET(caller->fd, &set)) {
     323        socklen_t sinsize = sizeof(caller->sock_in);
     324        if (caller->broadcast == false) {
     325          nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0,
     326                             (sockaddr *)&(caller->sock_in), &sinsize);
    297327        } else {
    298             if(nb_read>(int)msg_len) {
    299                 self->Warn("insufficent msg size (%i/%i)\n",nb_read,msg_len);
    300                 return -1;
    301             }
    302             memcpy(msg,buffer,nb_read);
    303             return nb_read;
    304         }
    305     }
    306 }
    307 
    308 #ifdef __XENO__
    309 void* Socket_impl::user(void * arg)
    310 {
    311     Socket_impl *caller = (Socket_impl*)arg;
    312     int pipe_fd=-1;
    313     string devname;
    314 
    315     devname= NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" + caller->self->ObjectName()+ "-pipe";
    316     while(pipe_fd<0)
    317     {
    318         pipe_fd = open(devname.c_str(), O_RDWR);
    319         if (pipe_fd < 0 && errno!=ENOENT) caller->self->Err("open pipe_fd error (%s)\n",strerror(-errno));
    320         usleep(1000);
    321     }
    322 
    323     while(caller->is_running==true)
    324     {
    325         fd_set set;
    326         struct timeval timeout;
    327         int rv;
    328 
    329         FD_ZERO(&set); // clear the set
    330         FD_SET(caller->fd, &set); // add our file descriptor to the set
    331         FD_SET(pipe_fd, &set); // add our file descriptor to the set
    332 
    333         timeout.tv_sec = 0;
    334         timeout.tv_usec = SELECT_TIMEOUT_MS*1000;
    335         rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
    336 
    337         if(rv == -1)
    338         {
    339             caller->self->Err("select error\n"); // an error occured
    340         }
    341         else if(rv == 0)
    342         {
    343             //printf("timeout\n");
    344         }
    345         else
    346         {
    347             ssize_t nb_read,nb_write;
    348             char buffer[1024];
    349 
    350             if(FD_ISSET(caller->fd, &set))
    351             {
    352                 socklen_t sinsize = sizeof(caller->sock_in);
    353                 if (caller->broadcast==false)
    354                 {
    355                     nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0, (sockaddr *) &(caller->sock_in), &sinsize);
    356                 }
    357                 else
    358                 {
    359                     nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0, NULL,NULL);
    360                 }
    361                 if(nb_read  < 0)
    362                 {
    363                     caller->self->Err("recvfrom error\n");
    364                 }
    365 //printf("user %s\n",buffer);
    366                 //on ne garde que les messages venant pas de soi meme
    367                 if (caller->broadcast==false || (caller->broadcast==true && getFrameworkManager()->ObjectName().compare(0,getFrameworkManager()->ObjectName().size(),buffer,getFrameworkManager()->ObjectName().size()) != 0))
    368                 {
    369                     nb_write=write(pipe_fd,buffer,nb_read);
    370                     if(nb_write!=nb_read)
    371                     {
    372                         caller->self->Err("write error\n");
    373                     }
    374                 }
    375                 else
    376                 {
    377                     //printf("self %s\n",buffer);
    378                 }
    379             }
    380             if(FD_ISSET(pipe_fd, &set))
    381             {
    382                 nb_read=read(pipe_fd,buffer,sizeof(buffer));
    383                 //printf("read pipe %i %s\n",nb_read,buffer);
    384                 if(nb_read>0)
    385                 {
    386                     //printf("send %s\n",buffer);
    387                     nb_write=sendto(caller->fd,buffer,nb_read, 0, (struct sockaddr *) &(caller->sock_in), sizeof(caller->sock_in));
    388                     if(nb_write!=nb_read)
    389                     {
    390                         caller->self->Err("sendto error\n");
    391                     }
    392                 }
    393             }
    394         }
    395     }
    396 
    397     close(pipe_fd);
    398     pthread_exit(0);
    399 }
    400 #endif
     328          nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0, NULL, NULL);
     329        }
     330        if (nb_read < 0) {
     331          caller->self->Err("recvfrom error\n");
     332        }
     333        // printf("user %s\n",buffer);
     334        // on ne garde que les messages venant pas de soi meme
     335        if (caller->broadcast == false ||
     336            (caller->broadcast == true &&
     337             getFrameworkManager()->ObjectName().compare(
     338                 0, getFrameworkManager()->ObjectName().size(), buffer,
     339                 getFrameworkManager()->ObjectName().size()) != 0)) {
     340          nb_write = write(pipe_fd, buffer, nb_read);
     341          if (nb_write != nb_read) {
     342            caller->self->Err("write error\n");
     343          }
     344        } else {
     345          // printf("self %s\n",buffer);
     346        }
     347      }
     348      if (FD_ISSET(pipe_fd, &set)) {
     349        nb_read = read(pipe_fd, buffer, sizeof(buffer));
     350        // printf("read pipe %i %s\n",nb_read,buffer);
     351        if (nb_read > 0) {
     352          // printf("send %s\n",buffer);
     353          nb_write = sendto(caller->fd, buffer, nb_read, 0,
     354                            (struct sockaddr *)&(caller->sock_in),
     355                            sizeof(caller->sock_in));
     356          if (nb_write != nb_read) {
     357            caller->self->Err("sendto error\n");
     358          }
     359        }
     360      }
     361    }
     362  }
     363
     364  close(pipe_fd);
     365  pthread_exit(0);
     366}
     367#endif

  • trunk/lib/FlairCore/src/SpinBox.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair { namespace gui {
     22namespace flair {
     23namespace gui {
    2324
    24 SpinBox::SpinBox(const LayoutPosition* position,string name,int min,int max,int step,int default_value):Box(position,name,"SpinBox") {
    25     //update value from xml file
    26     if(default_value<min) default_value=min;
    27     if(default_value>max) default_value=max;
    28     box_value=default_value;
     25SpinBox::SpinBox(const LayoutPosition *position, string name, int min, int max,
     26                 int step, int default_value)
     27    : Box(position, name, "SpinBox") {
     28  // update value from xml file
     29  if (default_value < min)
     30    default_value = min;
     31  if (default_value > max)
     32    default_value = max;
     33  box_value = default_value;
    2934
    30     SetVolatileXmlProp("min",min);
    31     SetVolatileXmlProp("max",max);
    32     SetVolatileXmlProp("step",step);
    33     GetPersistentXmlProp("value",box_value);
    34     SetPersistentXmlProp("value",box_value);
     35  SetVolatileXmlProp("min", min);
     36  SetVolatileXmlProp("max", max);
     37  SetVolatileXmlProp("step", step);
     38  GetPersistentXmlProp("value", box_value);
     39  SetPersistentXmlProp("value", box_value);
    3540
    36     SendXml();
     41  SendXml();
    3742}
    3843
    39 SpinBox::SpinBox(const LayoutPosition* position,string name,string suffix,int min,int max,int step,int default_value):Box(position,name,"SpinBox") {
    40     //update value from xml file
    41     if(default_value<min) default_value=min;
    42     if(default_value>max) default_value=max;
    43     box_value=default_value;
     44SpinBox::SpinBox(const LayoutPosition *position, string name, string suffix,
     45                 int min, int max, int step, int default_value)
     46    : Box(position, name, "SpinBox") {
     47  // update value from xml file
     48  if (default_value < min)
     49    default_value = min;
     50  if (default_value > max)
     51    default_value = max;
     52  box_value = default_value;
    4453
    45     SetVolatileXmlProp("suffix",suffix);
    46     SetVolatileXmlProp("min",min);
    47     SetVolatileXmlProp("max",max);
    48     SetVolatileXmlProp("step",step);
    49     GetPersistentXmlProp("value",box_value);
    50     SetPersistentXmlProp("value",box_value);
    51     SendXml();
     54  SetVolatileXmlProp("suffix", suffix);
     55  SetVolatileXmlProp("min", min);
     56  SetVolatileXmlProp("max", max);
     57  SetVolatileXmlProp("step", step);
     58  GetPersistentXmlProp("value", box_value);
     59  SetPersistentXmlProp("value", box_value);
     60  SendXml();
    5261}
    5362
    54 SpinBox::~SpinBox() {
    55 }
     63SpinBox::~SpinBox() {}
    5664
    5765int SpinBox::Value(void) const {
    58     int tmp;
     66  int tmp;
    5967
    60     GetMutex();
    61     tmp=box_value;
    62     ReleaseMutex();
     68  GetMutex();
     69  tmp = box_value;
     70  ReleaseMutex();
    6371
    64     return tmp;
     72  return tmp;
    6573}
    6674
    6775void SpinBox::XmlEvent(void) {
    68     GetMutex();
    69     if(GetPersistentXmlProp("value",box_value)) SetValueChanged();
    70     ReleaseMutex();
     76  GetMutex();
     77  if (GetPersistentXmlProp("value", box_value))
     78    SetValueChanged();
     79  ReleaseMutex();
    7180}
    7281

  • trunk/lib/FlairCore/src/SpinBox.h

    r2 r15  
    1616#include <Box.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class LayoutPosition;
     21class LayoutPosition;
    2422
    25     /*! \class SpinBox
    26     *
    27     * \brief Class displaying a QSpinBox on the ground station
    28     *
    29     */
    30     class SpinBox : public Box
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a QSpinBox at given position. \n
    37             * The QSpinBox is saturated to min and max values.
    38             *
    39             * \param position position to display the QSpinBox
    40             * \param name name
    41             * \param min minimum value
    42             * \param max maximum value
    43             * \param step step
    44             * \param default_value default value if not in the xml config file
    45             */
    46             SpinBox(const LayoutPosition* position,std::string name,int min,int max,int step,int default_value=0);/*!
     23/*! \class SpinBox
     24*
     25* \brief Class displaying a QSpinBox on the ground station
     26*
     27*/
     28class SpinBox : public Box {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a QSpinBox at given position. \n
     34  * The QSpinBox is saturated to min and max values.
     35  *
     36  * \param position position to display the QSpinBox
     37  * \param name name
     38  * \param min minimum value
     39  * \param max maximum value
     40  * \param step step
     41  * \param default_value default value if not in the xml config file
     42  */
     43  SpinBox(const LayoutPosition *position, std::string name, int min, int max,
     44          int step, int default_value = 0); /*!
    4745
    48             * \brief Constructor
    49             *
    50             * Construct a QSpinBox at given position. \n
    51             * The QSpinBox is saturated to min and max values.
    52             *
    53             * \param position position to display the QSpinBox
    54             * \param name name
    55             * \param suffix suffix for the value (eg unit)
    56             * \param min minimum value
    57             * \param max maximum value
    58             * \param step step
    59             * \param default_value default value if not in the xml config file
    60             */
    61             SpinBox(const LayoutPosition* position,std::string name,std::string suffix,int min,int max,int step,int default_value=0);
     46* \brief Constructor
     47*
     48* Construct a QSpinBox at given position. \n
     49* The QSpinBox is saturated to min and max values.
     50*
     51* \param position position to display the QSpinBox
     52* \param name name
     53* \param suffix suffix for the value (eg unit)
     54* \param min minimum value
     55* \param max maximum value
     56* \param step step
     57* \param default_value default value if not in the xml config file
     58*/
     59  SpinBox(const LayoutPosition *position, std::string name, std::string suffix,
     60          int min, int max, int step, int default_value = 0);
    6261
    63             /*!
    64             * \brief Destructor
    65             *
    66             */
    67             ~SpinBox();
     62  /*!
     63  * \brief Destructor
     64  *
     65  */
     66  ~SpinBox();
    6867
    69            /*!
    70             * \brief Value
    71             *
    72             * \return value
    73             */
    74             int Value(void) const;
     68  /*!
     69   * \brief Value
     70   *
     71   * \return value
     72   */
     73  int Value(void) const;
    7574
    76         private:
    77             /*!
    78             * \brief XmlEvent from ground station
    79             *
    80             * Reimplemented from Widget.
    81             *
    82             */
    83             void XmlEvent(void);
     75private:
     76  /*!
     77  * \brief XmlEvent from ground station
     78  *
     79  * Reimplemented from Widget.
     80  *
     81  */
     82  void XmlEvent(void);
    8483
    85             int box_value;
    86     };
     84  int box_value;
     85};
    8786
    8887} // end namespace gui

  • trunk/lib/FlairCore/src/Tab.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair
    23 {
    24 namespace gui
    25 {
     22namespace flair {
     23namespace gui {
    2624
    27 Tab::Tab(const TabWidget* parent,string name,int position): Layout(parent,name,"Tab")
    28 {
    29     SetVolatileXmlProp("position",position);
    30     SendXml();
     25Tab::Tab(const TabWidget *parent, string name, int position)
     26    : Layout(parent, name, "Tab") {
     27  SetVolatileXmlProp("position", position);
     28  SendXml();
    3129}
    3230
    33 Tab::~Tab()
    34 {
    35 
    36 }
     31Tab::~Tab() {}
    3732
    3833} // end namespace gui

  • trunk/lib/FlairCore/src/Tab.h

    r2 r15  
    1616#include <Layout.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class TabWidget;
     21class TabWidget;
    2422
    25     /*! \class Tab
    26     *
    27     * \brief Class displaying a QTab on the ground station
    28     *
    29     * Tabs are displayed in a TabWidget.
    30     */
    31     class Tab: public Layout
    32     {
    33         public:
    34             /*!
    35             * \brief Constructor
    36             *
    37             * Construct a Tab in the TabWidget.
    38             *
    39             * \param parent parent
    40             * \param name name
    41             * \param position tab position, -1 to put at the last position
    42             */
    43             Tab(const TabWidget* parent,std::string name,int position=-1);
     23/*! \class Tab
     24*
     25* \brief Class displaying a QTab on the ground station
     26*
     27* Tabs are displayed in a TabWidget.
     28*/
     29class Tab : public Layout {
     30public:
     31  /*!
     32  * \brief Constructor
     33  *
     34  * Construct a Tab in the TabWidget.
     35  *
     36  * \param parent parent
     37  * \param name name
     38  * \param position tab position, -1 to put at the last position
     39  */
     40  Tab(const TabWidget *parent, std::string name, int position = -1);
    4441
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~Tab();
     42  /*!
     43  * \brief Destructor
     44  *
     45  */
     46  ~Tab();
    5047
    51         private:
    52     };
     48private:
     49};
    5350
    5451} // end namespace gui

  • trunk/lib/FlairCore/src/TabWidget.cpp

    r2 r15  
    2222using std::string;
    2323
    24 namespace flair
    25 {
    26 namespace gui
    27 {
     24namespace flair {
     25namespace gui {
    2826
    29 TabWidget::TabWidget(const LayoutPosition* position,string name,TabPosition_t tabPosition): Widget(position->getLayout(),name,"TabWidget")
    30 {
    31     SetVolatileXmlProp("row",position->Row());
    32     SetVolatileXmlProp("col",position->Col());
    33     SetVolatileXmlProp("position",(int)tabPosition);
    34     SendXml();
     27TabWidget::TabWidget(const LayoutPosition *position, string name,
     28                     TabPosition_t tabPosition)
     29    : Widget(position->getLayout(), name, "TabWidget") {
     30  SetVolatileXmlProp("row", position->Row());
     31  SetVolatileXmlProp("col", position->Col());
     32  SetVolatileXmlProp("position", (int)tabPosition);
     33  SendXml();
    3534
    36     delete position;
     35  delete position;
    3736}
    3837
    39 TabWidget::~TabWidget()
    40 {
    41 }
     38TabWidget::~TabWidget() {}
    4239
    4340} // end namespace gui

  • trunk/lib/FlairCore/src/TabWidget.h

    r2 r15  
    1616#include <Widget.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
    22     class LayoutPosition;
     18namespace flair {
     19namespace gui {
     20class LayoutPosition;
    2321
    24     /*! \class TabWidget
    25     *
    26     * \brief Class displaying a QTabWidget on the ground station
    27     *
    28     * TabWidget contains Tabs.
    29     *
    30     */
    31     class TabWidget:public Widget
    32     {
    33         public:
    34             /*!
    35             * \enum TabPosition_t
    36             * \brief Position of tabs
    37             */
    38             typedef enum { North/*! north */, South/*! south */, West/*! west */, East/*! east */} TabPosition_t;
     22/*! \class TabWidget
     23*
     24* \brief Class displaying a QTabWidget on the ground station
     25*
     26* TabWidget contains Tabs.
     27*
     28*/
     29class TabWidget : public Widget {
     30public:
     31  /*!
     32  * \enum TabPosition_t
     33  * \brief Position of tabs
     34  */
     35  typedef enum {
     36    North /*! north */,
     37    South /*! south */,
     38    West /*! west */,
     39    East /*! east */
     40  } TabPosition_t;
    3941
    40             /*!
    41             * \brief Constructor
    42             *
    43             * Construct a QTabWidget at given position. \n
    44             * The TabWidget will automatically be child of position->getLayout() Layout. After calling this constructor,
    45             * position will be deleted as it is no longer usefull.
    46             *
    47             * \param position position
    48             * \param name name
    49             * \param tabPosition position of tabs
    50             */
    51             TabWidget(const LayoutPosition* position,std::string name,TabPosition_t tabPosition=TabWidget::West);
     42  /*!
     43  * \brief Constructor
     44  *
     45  * Construct a QTabWidget at given position. \n
     46  * The TabWidget will automatically be child of position->getLayout() Layout.
     47  *After calling this constructor,
     48  * position will be deleted as it is no longer usefull.
     49  *
     50  * \param position position
     51  * \param name name
     52  * \param tabPosition position of tabs
     53  */
     54  TabWidget(const LayoutPosition *position, std::string name,
     55            TabPosition_t tabPosition = TabWidget::West);
    5256
    53             /*!
    54             * \brief Destructor
    55             *
    56             */
    57             ~TabWidget();
     57  /*!
     58  * \brief Destructor
     59  *
     60  */
     61  ~TabWidget();
    5862
    59         private:
    60     };
     63private:
     64};
    6165
    6266} // end namespace core

  • trunk/lib/FlairCore/src/TcpSocket.cpp

    r2 r15  
    2828namespace core {
    2929
    30 TcpSocket::TcpSocket(const Object* parent,const std::string name,bool _blockOnSend,bool _blockOnReceive):ConnectedSocket(parent,name),isConnected(false) {
    31     blockOnSend=_blockOnSend;
    32     blockOnReceive=_blockOnReceive;
     30TcpSocket::TcpSocket(const Object *parent, const std::string name,
     31                     bool _blockOnSend, bool _blockOnReceive)
     32    : ConnectedSocket(parent, name), isConnected(false) {
     33  blockOnSend = _blockOnSend;
     34  blockOnReceive = _blockOnReceive;
    3335}
    3436
    3537TcpSocket::~TcpSocket() {
    36     //Info("Debug: destroying TCP socket %s", ObjectName().c_str());
     38  // Info("Debug: destroying TCP socket %s", ObjectName().c_str());
     39  close(socket);
     40}
     41
     42void TcpSocket::Listen(const unsigned int port,
     43                       const std::string localAddress) {
     44  socket = ::socket(AF_INET, SOCK_STREAM, 0);
     45
     46  sockaddr_in my_addr;
     47  my_addr.sin_family = AF_INET;
     48  my_addr.sin_port = htons(port);
     49  if (localAddress == "ANY") {
     50    my_addr.sin_addr.s_addr = INADDR_ANY;
     51  } else {
     52    inet_aton(localAddress.c_str(), &(my_addr.sin_addr));
     53  }
     54  memset(&(my_addr.sin_zero), '\0', 8);
     55
     56  if (bind(socket, (sockaddr *)&my_addr, sizeof(my_addr)) < 0) {
     57    char errorMsg[256];
     58    Err("TCP bind, %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
     59  }
     60
     61  listen(socket, 1);
     62}
     63
     64TcpSocket *TcpSocket::Accept(Time timeout) {
     65  char errorMsg[256];
     66  TcpSocket *acceptedSocket = nullptr;
     67
     68  struct timeval tv;
     69  if (timeout != 0) {
     70    tv.tv_sec = timeout / 1000; // timeout is in ms
     71    tv.tv_usec = (timeout % 1000) * 1000;
     72  }
     73  fd_set rset;
     74  FD_ZERO(&rset);
     75  FD_SET(socket, &rset);
     76  int ret = select(socket + 1, &rset, nullptr, nullptr,
     77                   (timeout == 0) ? nullptr : &tv); // man 2 accept
     78  if (ret < 0) {
     79    Err("select: %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
     80  } else {
     81    if (ret == 0) {
     82      // timeout reached
     83      // Err("timeout reached\n");
     84    } else {
     85      // our socket is readable, a new connection can be accepted
     86      acceptedSocket = new TcpSocket(this->Parent(), this->ObjectName(),
     87                                     blockOnSend, blockOnReceive);
     88      sockaddr_in their_addr;
     89      socklen_t namelen = sizeof(their_addr);
     90      if ((acceptedSocket->socket =
     91               accept(socket, (sockaddr *)&their_addr, &namelen)) < 0) {
     92        Err("error: %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
     93        delete acceptedSocket;
     94        acceptedSocket = nullptr;
     95      }
     96    }
     97  }
     98
     99  return acceptedSocket;
     100}
     101
     102bool TcpSocket::Connect(const unsigned int distantPort,
     103                        const std::string distantAddress, Time timeout) {
     104  bool success = false;
     105  char errorMsg[256];
     106
     107  if (isConnected) {
     108    if (this->distantPort == distantPort &&
     109        this->distantAddress == distantAddress) {
     110      return true;
     111    } else {
     112      close(socket);
     113    }
     114  }
     115  socket = ::socket(AF_INET, SOCK_STREAM, 0);
     116  if (socket == -1)
     117    return false;
     118
     119  sockaddr_in serv_addr;
     120  serv_addr.sin_family = AF_INET;
     121  serv_addr.sin_port = htons(short(distantPort));
     122  if (inet_pton(AF_INET, distantAddress.c_str(), &serv_addr.sin_addr) <= 0) {
     123    printf("incorrect network address.");
    37124    close(socket);
    38 }
    39 
    40 void TcpSocket::Listen(const unsigned int port,const std::string localAddress) {
    41     socket=::socket(AF_INET,SOCK_STREAM,0);
    42 
    43     sockaddr_in my_addr;
    44     my_addr.sin_family = AF_INET;
    45     my_addr.sin_port = htons(port);
    46     if (localAddress=="ANY") {
    47         my_addr.sin_addr.s_addr=INADDR_ANY;
     125    return false;
     126  }
     127  memset(&(serv_addr.sin_zero), '\0', 8);
     128
     129  // go non blocking
     130  int flags = fcntl(socket, F_GETFL);
     131  fcntl(socket, F_SETFL, flags | O_NONBLOCK);
     132  if (connect(socket, (sockaddr *)&serv_addr, sizeof(serv_addr)) == -1) {
     133    if ((errno != EINPROGRESS) && (errno != EAGAIN)) {
     134      Err("socket connect: %s\n",
     135          strerror_r(errno, errorMsg, sizeof(errorMsg)));
     136      success = false;
    48137    } else {
    49         inet_aton(localAddress.c_str(),&(my_addr.sin_addr));
     138      // now block with a timeout
     139      struct timeval tv;
     140      if (timeout != 0) {
     141        tv.tv_sec = timeout / 1000; // timeout is in ms
     142        tv.tv_usec = (timeout % 1000) * 1000;
     143      }
     144      fd_set wset;
     145      FD_ZERO(&wset);
     146      FD_SET(socket, &wset);
     147      int ret =
     148          select(socket + 1, NULL, &wset, NULL,
     149                 (timeout == 0) ? NULL : &tv); // man 2 connect EINPROGRESS
     150      if (ret < 0) {
     151        Err("select: %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
     152        success = false;
     153      } else {
     154        if (ret == 0) {
     155          // timeout reached
     156          // Err("timeout reached\n");
     157          success = false;
     158        } else {
     159          // something happened on our socket. Check if an error occured
     160          int error;
     161          socklen_t len = sizeof(error);
     162          if (getsockopt(socket, SOL_SOCKET, SO_ERROR, &error, &len) != 0) {
     163            // Err("getsockopt:
     164            // %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
     165            success = false;
     166          } else if (error != 0) {
     167            // Err("socket error:
     168            // %d(%s)\n",error,strerror_r(error,errorMsg,sizeof(errorMsg)));
     169            success = false;
     170          } else {
     171            if (connect(socket, (sockaddr *)&serv_addr, sizeof(serv_addr)) ==
     172                -1) {
     173              success = false;
     174            } else {
     175              // Info("connected indeed ^^\n");
     176              success = true;
     177            }
     178          }
     179        }
     180      }
    50181    }
    51     memset(&(my_addr.sin_zero), '\0', 8);
    52 
    53     if (bind(socket,(sockaddr*)&my_addr,sizeof(my_addr))<0) {
    54         char errorMsg[256];
    55         Err("TCP bind, %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
    56     }
    57 
    58     listen(socket,1);
    59 }
    60 
    61 TcpSocket *TcpSocket::Accept(Time timeout) {
    62     char errorMsg[256];
    63     TcpSocket *acceptedSocket=nullptr;
    64 
     182  } else {
     183    success = true; // never reached suprisingly...
     184  }
     185  // switch back to blocking mode (default)
     186  fcntl(socket, F_SETFL, flags);
     187
     188  if (!success) {
     189    close(socket);
     190    // Info("Debug: Connect to %s:%d failed\n", distantAddress.c_str(),
     191    // distantPort);
     192    return false;
     193  } else {
     194    isConnected = true;
     195    this->distantPort = distantPort;
     196    this->distantAddress = distantAddress;
     197    // Info("Debug: Connect to %s:%d succeeded\n", distantAddress.c_str(),
     198    // distantPort);
     199    return true;
     200  }
     201}
     202
     203ssize_t TcpSocket::SendMessage(const char *buffer, size_t bufferSize,
     204                               Time timeout) {
     205  int flags = 0;
     206  if (!blockOnSend) {
     207    flags |= MSG_DONTWAIT;
     208  } else {
    65209    struct timeval tv;
    66     if (timeout!=0) {
    67         tv.tv_sec=timeout/1000; //timeout is in ms
    68         tv.tv_usec=(timeout%1000)*1000;
    69     }
    70     fd_set rset;
    71     FD_ZERO(&rset);
    72     FD_SET(socket, &rset);
    73     int ret=select(socket + 1, &rset, nullptr, nullptr,(timeout==0)?nullptr:&tv); //man 2 accept
    74     if (ret<0) {
    75         Err("select: %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
    76     } else {
    77         if (ret==0) {
    78             //timeout reached
    79             //Err("timeout reached\n");
    80         } else {
    81             //our socket is readable, a new connection can be accepted
    82             acceptedSocket=new TcpSocket(this->Parent(),this->ObjectName(),blockOnSend,blockOnReceive);
    83             sockaddr_in their_addr;
    84             socklen_t namelen = sizeof(their_addr);
    85             if ((acceptedSocket->socket=accept(socket,(sockaddr*)&their_addr, &namelen))<0) {
    86                 Err("error: %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
    87                 delete acceptedSocket;
    88                 acceptedSocket=nullptr;
    89             }
    90         }
    91     }
    92 
    93     return acceptedSocket;
    94 }
    95 
    96 bool TcpSocket::Connect(const unsigned int distantPort,const std::string distantAddress,Time timeout) {
    97     bool success=false;
    98     char errorMsg[256];
    99 
    100     if (isConnected) {
    101         if (this->distantPort==distantPort && this->distantAddress==distantAddress) {
    102             return true;
    103         } else {
    104             close(socket);
    105         }
    106     }
    107     socket=::socket(AF_INET,SOCK_STREAM,0);
    108     if (socket==-1) return false;
    109 
    110     sockaddr_in serv_addr;
    111     serv_addr.sin_family = AF_INET;
    112     serv_addr.sin_port = htons(short(distantPort));
    113     if (inet_pton(AF_INET, distantAddress.c_str(), &serv_addr.sin_addr) <= 0) {
    114         printf("incorrect network address.");
    115         close(socket);
    116         return false;
    117     }
    118     memset(&(serv_addr.sin_zero), '\0', 8);
    119 
    120     //go non blocking
    121     int flags=fcntl(socket, F_GETFL);
    122     fcntl(socket, F_SETFL, flags | O_NONBLOCK);
    123     if (connect(socket, (sockaddr*)&serv_addr, sizeof(serv_addr))==-1) {
    124         if ((errno != EINPROGRESS) && (errno != EAGAIN)){
    125             Err("socket connect: %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
    126             success=false;
    127         } else {
    128             //now block with a timeout
    129             struct timeval tv;
    130             if (timeout!=0) {
    131                 tv.tv_sec=timeout/1000; //timeout is in ms
    132                 tv.tv_usec=(timeout%1000)*1000;
    133             }
    134             fd_set wset;
    135             FD_ZERO(&wset);
    136             FD_SET(socket, &wset);
    137             int ret=select(socket + 1, NULL, &wset, NULL,(timeout==0)?NULL:&tv); //man 2 connect EINPROGRESS
    138             if (ret<0) {
    139                 Err("select: %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
    140                 success=false;
    141             } else {
    142                 if (ret==0) {
    143                     //timeout reached
    144                     //Err("timeout reached\n");
    145                     success=false;
    146                 } else {
    147                     //something happened on our socket. Check if an error occured
    148                     int error;
    149                     socklen_t len = sizeof(error);
    150                     if (getsockopt(socket, SOL_SOCKET, SO_ERROR, &error, &len)!=0) {
    151                         //Err("getsockopt: %s\n",strerror_r(errno,errorMsg,sizeof(errorMsg)));
    152                         success=false;
    153                     } else if (error!=0) {
    154                         //Err("socket error: %d(%s)\n",error,strerror_r(error,errorMsg,sizeof(errorMsg)));
    155                         success=false;
    156                     } else {
    157                         if (connect(socket, (sockaddr*)&serv_addr, sizeof(serv_addr))==-1) {
    158                             success=false;
    159                         } else {
    160                             //Info("connected indeed ^^\n");
    161                             success=true;
    162                         }
    163                     }
    164                 }
    165             }
    166         }
    167     } else {
    168         success=true; //never reached suprisingly...
    169     }
    170     //switch back to blocking mode (default)
    171     fcntl(socket, F_SETFL, flags);
    172 
    173     if (!success) {
    174         close(socket);
    175         //Info("Debug: Connect to %s:%d failed\n", distantAddress.c_str(), distantPort);
    176         return false;
    177     } else {
    178         isConnected=true;
    179         this->distantPort=distantPort;
    180         this->distantAddress=distantAddress;
    181         //Info("Debug: Connect to %s:%d succeeded\n", distantAddress.c_str(), distantPort);
    182         return true;
    183     }
    184 }
    185 
    186 ssize_t TcpSocket::SendMessage(const char* buffer,size_t bufferSize,Time timeout){
    187     int flags=0;
    188     if (!blockOnSend) {
    189         flags|=MSG_DONTWAIT;
    190     } else {
    191         struct timeval tv;
    192         tv.tv_sec=timeout/1000; //timeout is in ms
    193         tv.tv_usec=(timeout%1000)*1000;
    194 
    195         setsockopt(socket,SOL_SOCKET,SO_SNDTIMEO,(char *)&tv,sizeof(struct timeval));
    196     }
    197     ssize_t bytesSent=send(socket, buffer, bufferSize, flags);
    198 
    199     return bytesSent;
    200 }
    201 
    202 ssize_t TcpSocket::RecvMessage(char* buffer,size_t bufferSize,Time timeout){
    203     int flags=0;
    204     if (!blockOnReceive) {
    205         flags|=MSG_DONTWAIT;
    206     } else {
    207         struct timeval tv;
    208         tv.tv_sec=timeout/1000; //timeout is in ms
    209         tv.tv_usec=(timeout%1000)*1000;
    210 
    211         setsockopt(socket,SOL_SOCKET,SO_RCVTIMEO,(char *)&tv,sizeof(struct timeval));
    212     }
    213     ssize_t bytesRead=recv(socket,buffer,bufferSize,flags);
    214 
    215     return bytesRead;
    216 }
    217 
    218 uint16_t TcpSocket::NetworkToHost16(uint16_t data) {
    219     return ntohs(data);
    220 }
    221 
    222 uint16_t TcpSocket::HostToNetwork16(uint16_t data) {
    223     return htons(data);
    224 }
    225 
    226 uint32_t TcpSocket::NetworkToHost32(uint32_t data) {
    227     return ntohl(data);
    228 }
    229 
    230 uint32_t TcpSocket::HostToNetwork32(uint32_t data) {
    231     return htonl(data);
    232 }
     210    tv.tv_sec = timeout / 1000; // timeout is in ms
     211    tv.tv_usec = (timeout % 1000) * 1000;
     212
     213    setsockopt(socket, SOL_SOCKET, SO_SNDTIMEO, (char *)&tv,
     214               sizeof(struct timeval));
     215  }
     216  ssize_t bytesSent = send(socket, buffer, bufferSize, flags);
     217
     218  return bytesSent;
     219}
     220
     221ssize_t TcpSocket::RecvMessage(char *buffer, size_t bufferSize, Time timeout) {
     222  int flags = 0;
     223  if (!blockOnReceive) {
     224    flags |= MSG_DONTWAIT;
     225  } else {
     226    struct timeval tv;
     227    tv.tv_sec = timeout / 1000; // timeout is in ms
     228    tv.tv_usec = (timeout % 1000) * 1000;
     229
     230    setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv,
     231               sizeof(struct timeval));
     232  }
     233  ssize_t bytesRead = recv(socket, buffer, bufferSize, flags);
     234
     235  return bytesRead;
     236}
     237
     238uint16_t TcpSocket::NetworkToHost16(uint16_t data) { return ntohs(data); }
     239
     240uint16_t TcpSocket::HostToNetwork16(uint16_t data) { return htons(data); }
     241
     242uint32_t TcpSocket::NetworkToHost32(uint32_t data) { return ntohl(data); }
     243
     244uint32_t TcpSocket::HostToNetwork32(uint32_t data) { return htonl(data); }
    233245
    234246} // end namespace core

  • trunk/lib/FlairCore/src/TcpSocket.h

    r2 r15  
    1616#include <ConnectedSocket.h>
    1717
    18 namespace flair
    19 {
    20 namespace core
    21 {
    22     /*! \class TcpSocket
    23     *
    24     * \brief Class encapsulating a TCP socket
    25     *
    26     */
    27     class TcpSocket:public ConnectedSocket {
    28     public:
    29         TcpSocket(const Object* parent,const std::string name,bool blockOnSend=false,bool blockOnReceive=true);
    30         ~TcpSocket();
    31         void Listen(const unsigned int port,const std::string localAddress="ANY");
    32         TcpSocket *Accept(Time timeout=0); //should throw an exception if not a listening socket
    33         bool Connect(const unsigned int distantPort,const std::string distantAddress,Time timeout=0); // timeout in milliseconds
    34         ssize_t SendMessage(const char* message,size_t message_len,Time timeout=0); // timeout in milliseconds
    35         ssize_t RecvMessage(char* buf,size_t buf_len,Time timeout=0); // timeout in milliseconds
     18namespace flair {
     19namespace core {
     20/*! \class TcpSocket
     21*
     22* \brief Class encapsulating a TCP socket
     23*
     24*/
     25class TcpSocket : public ConnectedSocket {
     26public:
     27  TcpSocket(const Object *parent, const std::string name,
     28            bool blockOnSend = false, bool blockOnReceive = true);
     29  ~TcpSocket();
     30  void Listen(const unsigned int port, const std::string localAddress = "ANY");
     31  TcpSocket *Accept(
     32      Time timeout = 0); // should throw an exception if not a listening socket
     33  bool Connect(const unsigned int distantPort, const std::string distantAddress,
     34               Time timeout = 0); // timeout in milliseconds
     35  ssize_t SendMessage(const char *message, size_t message_len,
     36                      Time timeout = 0); // timeout in milliseconds
     37  ssize_t RecvMessage(char *buf, size_t buf_len,
     38                      Time timeout = 0); // timeout in milliseconds
    3639
    37         uint16_t NetworkToHost16(uint16_t data);
    38         uint16_t HostToNetwork16(uint16_t data);
    39         uint32_t NetworkToHost32(uint32_t data);
    40         uint32_t HostToNetwork32(uint32_t data);
     40  uint16_t NetworkToHost16(uint16_t data);
     41  uint16_t HostToNetwork16(uint16_t data);
     42  uint32_t NetworkToHost32(uint32_t data);
     43  uint32_t HostToNetwork32(uint32_t data);
    4144
    42     private:
    43         int socket; // socket file descriptor
    44         bool blockOnSend;
    45         bool blockOnReceive;
    46         bool isConnected;
    47         unsigned int distantPort;
    48         std::string distantAddress;
    49     };
     45private:
     46  int socket; // socket file descriptor
     47  bool blockOnSend;
     48  bool blockOnReceive;
     49  bool isConnected;
     50  unsigned int distantPort;
     51  std::string distantAddress;
     52};
    5053
    5154} // end namespace core

  • trunk/lib/FlairCore/src/TextEdit.cpp

    r2 r15  
    2222using std::string;
    2323
    24 namespace flair
    25 {
    26 namespace gui
    27 {
     24namespace flair {
     25namespace gui {
    2826
    29 TextEdit::TextEdit(const LayoutPosition* position,string name,size_t buf_size): Widget(position->getLayout(),name,"TextEdit")
    30 {
    31     SetVolatileXmlProp("row",position->Row());
    32     SetVolatileXmlProp("col",position->Col());
    33     SendXml();
     27TextEdit::TextEdit(const LayoutPosition *position, string name, size_t buf_size)
     28    : Widget(position->getLayout(), name, "TextEdit") {
     29  SetVolatileXmlProp("row", position->Row());
     30  SetVolatileXmlProp("col", position->Col());
     31  SendXml();
    3432
    35     delete position;
     33  delete position;
    3634
    37 //    text_node=AddXmlChild("Text");
     35  //    text_node=AddXmlChild("Text");
    3836
    39     printf_buffer=(char*)malloc(buf_size);
    40     if(printf_buffer==NULL) Err("erreur malloc\n");
     37  printf_buffer = (char *)malloc(buf_size);
     38  if (printf_buffer == NULL)
     39    Err("erreur malloc\n");
    4140}
    4241
    43 TextEdit::~TextEdit()
    44 {
    45     free(printf_buffer);
    46 }
     42TextEdit::~TextEdit() { free(printf_buffer); }
    4743
     44void TextEdit::Append(const char *format, ...) {
     45  int n;
    4846
    49 void TextEdit::Append(const char * format, ...)
    50 {
    51     int n;
     47  va_list args;
     48  va_start(args, format);
     49  n = vsprintf(printf_buffer, format, args);
     50  va_end(args);
     51  if (n <= 0)
     52    return;
    5253
    53     va_list args;
    54     va_start(args, format);
    55     n = vsprintf(printf_buffer,format, args);
    56     va_end (args);
    57     if (n<=0) return;
    58 
    59     SetVolatileXmlProp("value",printf_buffer,text_node);
    60     SendXml();
    61 
     54  SetVolatileXmlProp("value", printf_buffer, text_node);
     55  SendXml();
    6256}
    6357

  • trunk/lib/FlairCore/src/TextEdit.h

    r2 r15  
    1616#include <Widget.h>
    1717
    18 namespace flair
    19 {
    20 namespace gui
    21 {
     18namespace flair {
     19namespace gui {
    2220
    23     class LayoutPosition;
     21class LayoutPosition;
    2422
    25     /*! \class TextEdit
    26     *
    27     * \brief Class displaying a QTextEdit on the ground station
    28     *
    29     * QTextEdit allows printing on multiple lines. \n
    30     *
    31     */
    32     class TextEdit:public Widget
    33     {
    34         public:
    35             /*!
    36             * \brief Constructor
    37             *
    38             * Construct a QTabWidget at given position. \n
    39             * The TextEdit will automatically be child of position->getLayout() Layout. After calling this constructor,
    40             * position will be deleted as it is no longer usefull.
    41             *
    42             * \param parent parent
    43             * \param name name
    44             * \param buf_size size of the text buffer
    45             */
    46             TextEdit(const LayoutPosition* position,std::string name,size_t buf_size=255);
     23/*! \class TextEdit
     24*
     25* \brief Class displaying a QTextEdit on the ground station
     26*
     27* QTextEdit allows printing on multiple lines. \n
     28*
     29*/
     30class TextEdit : public Widget {
     31public:
     32  /*!
     33  * \brief Constructor
     34  *
     35  * Construct a QTabWidget at given position. \n
     36  * The TextEdit will automatically be child of position->getLayout() Layout.
     37  *After calling this constructor,
     38  * position will be deleted as it is no longer usefull.
     39  *
     40  * \param parent parent
     41  * \param name name
     42  * \param buf_size size of the text buffer
     43  */
     44  TextEdit(const LayoutPosition *position, std::string name,
     45           size_t buf_size = 255);
    4746
    48             /*!
    49             * \brief Destructor
    50             *
    51             */
    52             ~TextEdit();
     47  /*!
     48  * \brief Destructor
     49  *
     50  */
     51  ~TextEdit();
    5352
    54             /*!
    55             * \brief Append a line
    56             *
    57             * \param format text string to display, see standard printf
    58             */
    59             void Append(const char * format, ...);
     53  /*!
     54  * \brief Append a line
     55  *
     56  * \param format text string to display, see standard printf
     57  */
     58  void Append(const char *format, ...);
    6059
    61         private:
    62             char* printf_buffer;
    63             xmlNodePtr text_node;
    64     };
     60private:
     61  char *printf_buffer;
     62  xmlNodePtr text_node;
     63};
    6564
    6665} // end namespace gui

  • trunk/lib/FlairCore/src/Thread.cpp

    r2 r15  
    2727using std::string;
    2828
    29 namespace flair
    30 {
    31 namespace core
    32 {
     29namespace flair {
     30namespace core {
    3331
    34 Thread::Thread(const Object* parent,string name,uint8_t priority): Object(parent,name,"Thread")
    35 {
    36      pimpl_=new Thread_impl(this,priority);
     32Thread::Thread(const Object *parent, string name, uint8_t priority)
     33    : Object(parent, name, "Thread") {
     34  pimpl_ = new Thread_impl(this, priority);
    3735}
    3836
    39 Thread::~Thread()
    40 {
    41     delete pimpl_;
    42 }
     37Thread::~Thread() { delete pimpl_; }
    4338
    44 void Thread::Start(void)
    45 {
    46     pimpl_->Start();
    47 }
     39void Thread::Start(void) { pimpl_->Start(); }
    4840
    49 void Thread::SafeStop(void)
    50 {
    51     pimpl_->SafeStop();
    52 }
     41void Thread::SafeStop(void) { pimpl_->SafeStop(); }
    5342
    54 bool Thread::ToBeStopped(void) const
    55 {
    56     return pimpl_->ToBeStopped();
    57 }
     43bool Thread::ToBeStopped(void) const { return pimpl_->ToBeStopped(); }
    5844
    5945#ifdef __XENO__
    60 void Thread::WarnUponSwitches(bool value)
    61 {
    62     // Ask Xenomai to warn us upon switches to secondary mode.
    63     if(value==true)
    64     {
    65         rt_task_set_mode(0, T_WARNSW, NULL);
    66     }
    67     else
    68     {
    69         rt_task_set_mode(T_WARNSW, 0, NULL);
    70     }
     46void Thread::WarnUponSwitches(bool value) {
     47  // Ask Xenomai to warn us upon switches to secondary mode.
     48  if (value == true) {
     49    rt_task_set_mode(0, T_WARNSW, NULL);
     50  } else {
     51    rt_task_set_mode(T_WARNSW, 0, NULL);
     52  }
    7153}
    7254#else
    73 void Thread::WarnUponSwitches(bool value)
    74 {
    75     //Warn("Not applicable in non real time\n");
     55void Thread::WarnUponSwitches(bool value) {
     56  // Warn("Not applicable in non real time\n");
    7657}
    7758#endif
    7859
    79 void Thread::Join(void)
    80 {
    81     pimpl_->Join();
     60void Thread::Join(void) { pimpl_->Join(); }
     61
     62void Thread::SetPeriodUS(uint32_t period) { pimpl_->SetPeriodUS(period); }
     63
     64uint32_t Thread::GetPeriodUS(void) const { return pimpl_->GetPeriodUS(); }
     65
     66void Thread::SetPeriodMS(uint32_t period) { pimpl_->SetPeriodMS(period); }
     67
     68uint32_t Thread::GetPeriodMS(void) const { return pimpl_->GetPeriodMS(); }
     69
     70bool Thread::IsPeriodSet(void) { return pimpl_->period_set; }
     71
     72void Thread::WaitPeriod(void) const { pimpl_->WaitPeriod(); }
     73
     74void Thread::Suspend(void) { pimpl_->Suspend(); }
     75
     76bool Thread::SuspendUntil(Time date) { return pimpl_->SuspendUntil(date); }
     77
     78bool Thread::IsSuspended(void) const { return pimpl_->IsSuspended(); }
     79
     80void Thread::Resume(void) { pimpl_->Resume(); }
     81
     82int Thread::WaitUpdate(const IODevice *device) {
     83  return pimpl_->WaitUpdate(device);
    8284}
    8385
    84 void Thread::SetPeriodUS(uint32_t period) {
    85     pimpl_->SetPeriodUS(period);
    86 }
    87 
    88 uint32_t Thread::GetPeriodUS(void) const {
    89     return pimpl_->GetPeriodUS();
    90 }
    91 
    92 void Thread::SetPeriodMS(uint32_t period) {
    93     pimpl_->SetPeriodMS(period);
    94 }
    95 
    96 uint32_t Thread::GetPeriodMS(void) const {
    97     return pimpl_->GetPeriodMS();
    98 }
    99 
    100 
    101 bool Thread::IsPeriodSet(void)
    102 {
    103     return pimpl_->period_set;
    104 }
    105 
    106 void Thread::WaitPeriod(void) const
    107 {
    108     pimpl_->WaitPeriod();
    109 }
    110 
    111 void Thread::Suspend(void)
    112 {
    113     pimpl_->Suspend();
    114 }
    115 
    116 bool Thread::SuspendUntil(Time date){
    117     return pimpl_->SuspendUntil(date);
    118 }
    119 
    120 bool Thread::IsSuspended(void) const
    121 {
    122     return pimpl_->IsSuspended();
    123 }
    124 
    125 void Thread::Resume(void)
    126 {
    127     pimpl_->Resume();
    128 }
    129 
    130 int Thread::WaitUpdate(const IODevice* device)
    131 {
    132     return pimpl_->WaitUpdate(device);
    133 }
    134 
    135 void Thread::SleepUntil(Time time) const
    136 {
     86void Thread::SleepUntil(Time time) const {
    13787#ifdef __XENO__
    138     int status=rt_task_sleep_until(time);
    139     if(status!=0) Err("%s, error rt_task_sleep_until (%s), resume time: %lld, actual time: %lld\n",
    140                       ObjectName().c_str(),
    141                       strerror(-status),
    142                       time,
    143                       GetTime());
    144     //Printf("rt_task_sleep_until, resume time: %lld, actual time: %lld\n",time,GetTime());
     88  int status = rt_task_sleep_until(time);
     89  if (status != 0)
     90    Err("%s, error rt_task_sleep_until (%s), resume time: %lld, actual time: "
     91        "%lld\n",
     92        ObjectName().c_str(), strerror(-status), time, GetTime());
     93// Printf("rt_task_sleep_until, resume time: %lld, actual time:
     94// %lld\n",time,GetTime());
    14595#else
    146     Time current=GetTime();
    147     if(current<time)
    148     {
    149         usleep((time-current)/1000);
    150     }
     96  Time current = GetTime();
     97  if (current < time) {
     98    usleep((time - current) / 1000);
     99  }
    151100#endif
    152101}
    153102
    154 void Thread::SleepMS(uint32_t time) const
    155 {
     103void Thread::SleepMS(uint32_t time) const {
    156104#ifdef __XENO__
    157     int status=rt_task_sleep(time*1000*1000);
    158     if(status!=0) Err("erreur rt_task_sleep (%s)\n",strerror(-status));
     105  int status = rt_task_sleep(time * 1000 * 1000);
     106  if (status != 0)
     107    Err("erreur rt_task_sleep (%s)\n", strerror(-status));
    159108#else
    160     usleep(time*1000);
     109  usleep(time * 1000);
    161110#endif
    162111}
    163112
    164 void Thread::SleepUS(uint32_t time) const
    165 {
     113void Thread::SleepUS(uint32_t time) const {
    166114#ifdef __XENO__
    167     int status=rt_task_sleep(time*1000);
    168     if(status!=0) Err("erreur rt_task_sleep (%s)\n",strerror(-status));
     115  int status = rt_task_sleep(time * 1000);
     116  if (status != 0)
     117    Err("erreur rt_task_sleep (%s)\n", strerror(-status));
    169118#else
    170     usleep(time);
     119  usleep(time);
    171120#endif
    172121}

  • trunk/lib/FlairCore/src/Thread.h

    r2 r15  
    1919class Thread_impl;
    2020
    21 namespace flair
    22 {
    23 namespace core
    24 {
    25 
    26     class IODevice;
    27 
    28     /*! \class Thread
    29     *
    30     * \brief Abstract class for a thread
    31     *
    32     * To implement a thread, Run() method must be reimplemented. \n
    33     * When Start() is called, it will automatically call Run() reimplemented method.
    34     * A thread can be periodic, in this case WaitPeriod() will block untill period is met.
    35     * Thread can also e synnchronized with an IODevice, using WaitUpdate() method. \n
    36     * Thread period is by default 100ms.
    37     */
    38     class Thread: public Object
    39     {
    40         friend class ::Thread_impl;
    41 
    42         public:
    43             /*!
    44             * \brief Constructor
    45             *
    46             * \param parent parent
    47             * \param name name
    48             * \param priority priority, should be >20 (<20 is reserved for internal use)
    49             */
    50             Thread(const Object* parent,std::string name,uint8_t priority);//priority>20, for real time only
    51 
    52             /*!
    53             * \brief Destructor
    54             *
    55             * If thread is started, SafeStop() and Join() will
    56             * be automatically called.
    57             *
    58             */
    59             virtual ~Thread();
    60 
    61             /*!
    62             * \brief Start the thread
    63             *
    64             */
    65             void Start(void);
    66 
    67             /*!
    68             * \brief Set a stop flag
    69             *
    70             * ToBeStopped() will return true after calling this method.
    71             */
    72             void SafeStop(void);
    73 
    74             /*!
    75             * \brief Set a stop flag
    76             *
    77             * Reimplemented Run() can poll this method to
    78             * determine when to stop the thread.
    79             *
    80             * \return true if SafeStop() was called
    81             */
    82             bool ToBeStopped(void) const;
    83 
    84             /*!
    85             * \brief Join the thread
    86             *
    87             * This method will block untill Run() returns.
    88             *
    89             */
    90             void Join(void);
    91 
    92             /*!
    93             * \brief Set the period in micro second
    94             *
    95             * After calling this method, IsPeriodSet will return true.
    96             *
    97             * \param period_us period in us
    98             */
    99             void SetPeriodUS(uint32_t period_us);
    100 
    101             uint32_t GetPeriodUS() const;
    102 
    103             /*!
    104             * \brief Set the period in milli second
    105             *
    106             * After calling this method, IsPeriodSet will return true.
    107             *
    108             * \param period_ums period in ms
    109             */
    110             void SetPeriodMS(uint32_t period_ms);
    111 
    112             uint32_t GetPeriodMS() const;
    113 
    114             /*!
    115             * \brief Returns if period was set
    116             *
    117             * \return true if a period was set using SetPeriodUS or SetPeriodMS
    118             * false otherwise
    119             */
    120             bool IsPeriodSet(void);
    121 
    122             /*!
    123             * \brief Wait the period
    124             *
    125             * This method will block untill period is met. \n
    126             * If no period was set (see SetPeriodUS, SetPeriodMS and IsPeriodSet), this method
    127             * returns immediately.
    128             *
    129             */
    130             void WaitPeriod(void) const;
    131 
    132             /*!
    133             * \brief Wait update of an IODevice
    134             *
    135             * This method will block untill IODevice::ProcessUpdate
    136             * is called. \n
    137             * This method is usefull to synchronize a thread with an IODevice.
    138             *
    139             * \param device IODevice to wait update from
    140             */
    141             int WaitUpdate(const IODevice* device);
    142 
    143             /*!
    144             * \brief Suspend the thread
    145             *
    146             * This method will block untill Resume() is called.
    147             *
    148             */
    149             void Suspend(void);
    150 
    151             /*!
    152             * \brief Suspend the thread with timeout
    153             *
    154             * This method will block until Resume() is called or the absolute date specified occurs
    155             *
    156             * \param date absolute date in ns
    157             * \return true if thread is woken up by a call to Resume, false otherwise
    158             */
    159             bool SuspendUntil(Time date);
    160 
    161             /*!
    162             * \brief Resume the thread
    163             *
    164             * This method will unblock the call to Suspend().
    165             *
    166             */
    167             void Resume(void);
    168 
    169             /*!
    170             * \brief Is the thread suspended?
    171             *
    172             * \return true if thread is suspended
    173             *
    174             */
    175             bool IsSuspended(void) const;
    176 
    177             /*!
    178             * \brief Sleep until absolute time
    179             *
    180             * This method will block untill time is reached.
    181             *
    182             * \param time absolute time
    183             */
    184             void SleepUntil(Time time) const;
    185 
    186             /*!
    187             * \brief Sleep for a certain time in micro second
    188             *
    189             * This method will block untill time is elapsed.
    190             *
    191             * \param time_us time to wait in micro second
    192             */
    193             void SleepUS(uint32_t time_us) const;
    194 
    195             /*!
    196             * \brief Sleep for a cartain time in milli second
    197             *
    198             * This method will block untill time is elapsed.
    199             *
    200             * \param time_ms time to wait in milli second
    201             */
    202             void SleepMS(uint32_t time_ms) const;
    203 
    204             /*!
    205             * \brief Warn if real time / non real time switches occur
    206             *
    207             * If enabled, a message with the call stack will be displayed
    208             * in case of real time / non real time switches. \n
    209             * This method can help to debug application and see if switches occur. \n
    210             * Note that it as no effect if this method is called from the non real time
    211             * Framework library.
    212             *
    213             * \param enable enable or disable warns
    214             */
    215             static void WarnUponSwitches(bool enable);
    216 
    217         private:
    218             /*!
    219             * \brief Run method
    220             *
    221             * This method is automatically called by Start(). \n
    222             * This method must be reimplemented, in order to implement the thread.
    223             *
    224             */
    225             virtual void Run(void)=0;
    226 
    227             class Thread_impl* pimpl_;
    228     };
     21namespace flair {
     22namespace core {
     23
     24class IODevice;
     25
     26/*! \class Thread
     27*
     28* \brief Abstract class for a thread
     29*
     30* To implement a thread, Run() method must be reimplemented. \n
     31* When Start() is called, it will automatically call Run() reimplemented method.
     32* A thread can be periodic, in this case WaitPeriod() will block untill period
     33*is met.
     34* Thread can also e synnchronized with an IODevice, using WaitUpdate() method.
     35*\n
     36* Thread period is by default 100ms.
     37*/
     38class Thread : public Object {
     39  friend class ::Thread_impl;
     40
     41public:
     42  /*!
     43  * \brief Constructor
     44  *
     45  * \param parent parent
     46  * \param name name
     47  * \param priority priority, should be >20 (<20 is reserved for internal use)
     48  */
     49  Thread(const Object *parent, std::string name,
     50         uint8_t priority); // priority>20, for real time only
     51
     52  /*!
     53  * \brief Destructor
     54  *
     55  * If thread is started, SafeStop() and Join() will
     56  * be automatically called.
     57  *
     58  */
     59  virtual ~Thread();
     60
     61  /*!
     62  * \brief Start the thread
     63  *
     64  */
     65  void Start(void);
     66
     67  /*!
     68  * \brief Set a stop flag
     69  *
     70  * ToBeStopped() will return true after calling this method.
     71  */
     72  void SafeStop(void);
     73
     74  /*!
     75  * \brief Set a stop flag
     76  *
     77  * Reimplemented Run() can poll this method to
     78  * determine when to stop the thread.
     79  *
     80  * \return true if SafeStop() was called
     81  */
     82  bool ToBeStopped(void) const;
     83
     84  /*!
     85  * \brief Join the thread
     86  *
     87  * This method will block untill Run() returns.
     88  *
     89  */
     90  void Join(void);
     91
     92  /*!
     93  * \brief Set the period in micro second
     94  *
     95  * After calling this method, IsPeriodSet will return true.
     96  *
     97  * \param period_us period in us
     98  */
     99  void SetPeriodUS(uint32_t period_us);
     100
     101  uint32_t GetPeriodUS() const;
     102
     103  /*!
     104  * \brief Set the period in milli second
     105  *
     106  * After calling this method, IsPeriodSet will return true.
     107  *
     108  * \param period_ums period in ms
     109  */
     110  void SetPeriodMS(uint32_t period_ms);
     111
     112  uint32_t GetPeriodMS() const;
     113
     114  /*!
     115  * \brief Returns if period was set
     116  *
     117  * \return true if a period was set using SetPeriodUS or SetPeriodMS
     118  * false otherwise
     119  */
     120  bool IsPeriodSet(void);
     121
     122  /*!
     123  * \brief Wait the period
     124  *
     125  * This method will block untill period is met. \n
     126  * If no period was set (see SetPeriodUS, SetPeriodMS and IsPeriodSet), this
     127  *method
     128  * returns immediately.
     129  *
     130  */
     131  void WaitPeriod(void) const;
     132
     133  /*!
     134  * \brief Wait update of an IODevice
     135  *
     136  * This method will block untill IODevice::ProcessUpdate
     137  * is called. \n
     138  * This method is usefull to synchronize a thread with an IODevice.
     139  *
     140  * \param device IODevice to wait update from
     141  */
     142  int WaitUpdate(const IODevice *device);
     143
     144  /*!
     145  * \brief Suspend the thread
     146  *
     147  * This method will block untill Resume() is called.
     148  *
     149  */
     150  void Suspend(void);
     151
     152  /*!
     153  * \brief Suspend the thread with timeout
     154  *
     155  * This method will block until Resume() is called or the absolute date
     156  *specified occurs
     157  *
     158  * \param date absolute date in ns
     159  * \return true if thread is woken up by a call to Resume, false otherwise
     160  */
     161  bool SuspendUntil(Time date);
     162
     163  /*!
     164  * \brief Resume the thread
     165  *
     166  * This method will unblock the call to Suspend().
     167  *
     168  */
     169  void Resume(void);
     170
     171  /*!
     172  * \brief Is the thread suspended?
     173  *
     174  * \return true if thread is suspended
     175  *
     176  */
     177  bool IsSuspended(void) const;
     178
     179  /*!
     180  * \brief Sleep until absolute time
     181  *
     182  * This method will block untill time is reached.
     183  *
     184  * \param time absolute time
     185  */
     186  void SleepUntil(Time time) const;
     187
     188  /*!
     189  * \brief Sleep for a certain time in micro second
     190  *
     191  * This method will block untill time is elapsed.
     192  *
     193  * \param time_us time to wait in micro second
     194  */
     195  void SleepUS(uint32_t time_us) const;
     196
     197  /*!
     198  * \brief Sleep for a cartain time in milli second
     199  *
     200  * This method will block untill time is elapsed.
     201  *
     202  * \param time_ms time to wait in milli second
     203  */
     204  void SleepMS(uint32_t time_ms) const;
     205
     206  /*!
     207  * \brief Warn if real time / non real time switches occur
     208  *
     209  * If enabled, a message with the call stack will be displayed
     210  * in case of real time / non real time switches. \n
     211  * This method can help to debug application and see if switches occur. \n
     212  * Note that it as no effect if this method is called from the non real time
     213  * Framework library.
     214  *
     215  * \param enable enable or disable warns
     216  */
     217  static void WarnUponSwitches(bool enable);
     218
     219private:
     220  /*!
     221  * \brief Run method
     222  *
     223  * This method is automatically called by Start(). \n
     224  * This method must be reimplemented, in order to implement the thread.
     225  *
     226  */
     227  virtual void Run(void) = 0;
     228
     229  class Thread_impl *pimpl_;
     230};
    229231
    230232} // end namespace core

  • trunk/lib/FlairCore/src/Thread_impl.cpp

    r2 r15  
    3333using namespace flair::core;
    3434
    35 Thread_impl::Thread_impl(Thread* self,uint8_t priority)
    36 {
    37     isRunning=false;
    38     tobestopped=false;
    39     is_suspended=false;
    40     period_set=false;
    41 
    42     this->self=self;
    43     cond=new ConditionVariable(self,self->ObjectName());
    44 
    45     if(priority<MIN_THREAD_PRIORITY)
    46     {
    47         priority=MIN_THREAD_PRIORITY;
    48 //        printf("Thread::Thread, %s: priority set to %i\n",self->ObjectName().c_str(), priority);
     35Thread_impl::Thread_impl(Thread *self, uint8_t priority) {
     36  isRunning = false;
     37  tobestopped = false;
     38  is_suspended = false;
     39  period_set = false;
     40
     41  this->self = self;
     42  cond = new ConditionVariable(self, self->ObjectName());
     43
     44  if (priority < MIN_THREAD_PRIORITY) {
     45    priority = MIN_THREAD_PRIORITY;
     46    //        printf("Thread::Thread, %s: priority set to
     47    //        %i\n",self->ObjectName().c_str(), priority);
     48  }
     49  if (priority > MAX_THREAD_PRIORITY) {
     50    priority = MAX_THREAD_PRIORITY;
     51    self->Warn("priority set to %i\n", MAX_THREAD_PRIORITY);
     52  }
     53
     54  this->priority = priority;
     55  period = 100 * 1000 * 1000; // 100ms par defaut
     56  min_jitter = 1000 * 1000 * 1000;
     57  max_jitter = 0;
     58  mean_jitter = 0;
     59  last = 0;
     60  cpt = 0;
     61}
     62
     63Thread_impl::~Thread_impl() {
     64  SafeStop();
     65  Join();
     66}
     67
     68void Thread_impl::Start(void) {
     69  int status;
     70
     71  isRunning = true;
     72
     73#ifdef __XENO__
     74  string th_name =
     75      getFrameworkManager()->ObjectName() + "-" + self->ObjectName();
     76
     77#ifdef RT_STACK_SIZE
     78  status = rt_task_create(&task_rt, th_name.c_str(), RT_STACK_SIZE,
     79                          (int)priority, T_JOINABLE);
     80#else
     81  status =
     82      rt_task_create(&task_rt, th_name.c_str(), 0, (int)priority, T_JOINABLE);
     83#endif // RT_STACK_SIZE
     84  if (status != 0) {
     85    self->Err("rt_task_create error (%s)\n", strerror(-status));
     86  } else {
     87    //_printf("rt_task_create ok %s\n",th_name);
     88  }
     89
     90  status = rt_task_start(&task_rt, &main_rt, (void *)this);
     91  if (status != 0) {
     92    self->Err("rt_task_start error (%s)\n", strerror(-status));
     93  } else {
     94    //_printf("rt_task_start ok %s\n",th_name);
     95  }
     96
     97  // Initialise the rt_print buffer for this task explicitly
     98  rt_print_init(512, th_name.c_str());
     99
     100#else //__XENO__
     101
     102  // Initialize thread creation attributes
     103  pthread_attr_t attr;
     104  if (pthread_attr_init(&attr) != 0) {
     105    self->Err("Error pthread_attr_init\n");
     106  }
     107
     108#ifdef NRT_STACK_SIZE
     109  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
     110    self->Err("Error pthread_attr_setstacksize\n");
     111  }
     112#endif // NRT_STACK_SIZE
     113
     114  if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) != 0) {
     115    self->Err("Error pthread_attr_setinheritsched\n");
     116  }
     117
     118  if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO) != 0) {
     119    self->Err("Error pthread_attr_setschedpolicy\n");
     120  }
     121
     122  struct sched_param parm;
     123  parm.sched_priority = priority;
     124  if (pthread_attr_setschedparam(&attr, &parm) != 0) {
     125    self->Err("Error pthread_attr_setschedparam\n");
     126  }
     127
     128  next_time = GetTime() + period;
     129
     130  if (pthread_create(&task_nrt, &attr, main_nrt, (void *)this) != 0) {
     131    self->Err("pthread_create error\n");
     132  }
     133
     134  if (pthread_attr_destroy(&attr) != 0) {
     135    self->Err("Error pthread_attr_destroy\n");
     136  }
     137
     138#endif //__XENO__
     139}
     140
     141#ifdef __XENO__
     142void Thread_impl::main_rt(void *arg) {
     143  Thread_impl *caller = (Thread_impl *)arg;
     144
     145  // Perform auto-init of rt_print buffers if the task doesn't do so
     146  rt_print_auto_init(1);
     147
     148  caller->self->Run();
     149
     150  caller->PrintStats();
     151}
     152#else
     153void *Thread_impl::main_nrt(void *arg) {
     154  Thread_impl *caller = (Thread_impl *)arg;
     155
     156  caller->self->Run();
     157
     158  caller->PrintStats();
     159
     160  pthread_exit(0);
     161}
     162#endif //__XENO__
     163
     164void Thread_impl::SetPeriodUS(uint32_t period) {
     165  if (period == 0) {
     166    self->Err("Period must be>0\n");
     167    return;
     168  }
     169
     170#ifdef __XENO__
     171  int status = rt_task_set_periodic(&task_rt, TM_NOW, period * 1000);
     172  if (status != 0)
     173    self->Err("Error rt_task_set_periodic %s\n", strerror(-status));
     174#else
     175  next_time -= period;
     176  next_time += period * 1000;
     177#endif
     178  this->period = period * 1000;
     179  period_set = true;
     180}
     181
     182uint32_t Thread_impl::GetPeriodUS() const { return this->period / 1000; }
     183
     184void Thread_impl::SetPeriodMS(uint32_t period) {
     185  if (period == 0) {
     186    self->Err("Period must be>0\n");
     187    return;
     188  }
     189
     190#ifdef __XENO__
     191  int status = rt_task_set_periodic(&task_rt, TM_NOW, period * 1000 * 1000);
     192  if (status != 0)
     193    self->Err("Error rt_task_set_periodic %s\n", strerror(-status));
     194#else
     195  next_time -= period;
     196  next_time += period * 1000 * 1000;
     197#endif
     198  this->period = period * 1000 * 1000;
     199  period_set = true;
     200}
     201
     202uint32_t Thread_impl::GetPeriodMS() const { return this->period / 1000 / 1000; }
     203
     204void Thread_impl::WaitPeriod(void) {
     205  if (period_set == false) {
     206    self->Err("Period must be set befaore calling this method\n");
     207    return;
     208  }
     209#ifdef __XENO__
     210  unsigned long overruns_r;
     211  int status = rt_task_wait_period(&overruns_r);
     212  if (status != 0)
     213    self->Err("Error rt_task_wait_period %s\n", strerror(-status));
     214  if (status == -ETIMEDOUT)
     215    self->Err("overrun: %lld\n", overruns_r);
     216#else
     217  self->SleepUntil(next_time);
     218  next_time += period;
     219#endif
     220  ComputeJitter(GetTime());
     221}
     222
     223void Thread_impl::Suspend(void) {
     224  if (isRunning == false) {
     225    self->Err("thread is not started\n");
     226    return;
     227  }
     228
     229  cond->GetMutex(), is_suspended = true;
     230  cond->CondWait();
     231  is_suspended = false;
     232  cond->ReleaseMutex();
     233}
     234
     235bool Thread_impl::SuspendUntil(Time date) {
     236  if (isRunning == false) {
     237    self->Err("thread is not started\n");
     238    return false;
     239  }
     240
     241  cond->GetMutex(), is_suspended = true;
     242  bool success = cond->CondWaitUntil(date);
     243  is_suspended = false;
     244  cond->ReleaseMutex();
     245  return success;
     246}
     247
     248bool Thread_impl::IsSuspended(void) {
     249  bool result;
     250
     251  cond->GetMutex();
     252  result = is_suspended;
     253  cond->ReleaseMutex();
     254
     255  return result;
     256}
     257
     258void Thread_impl::Resume(void) {
     259  if (isRunning == false) {
     260    self->Err("thread is not started\n");
     261    return;
     262  }
     263
     264  cond->GetMutex();
     265  if (is_suspended == true) {
     266    cond->CondSignal();
     267  } else {
     268    self->Err("thread is not suspended\n");
     269  }
     270  cond->ReleaseMutex();
     271}
     272
     273int Thread_impl::WaitUpdate(const IODevice *device) {
     274  int status = 0;
     275
     276  if (IsSuspended() == true) {
     277    self->Err("thread is already supended\n");
     278    status = -1;
     279  } else {
     280    cond->GetMutex();
     281
     282    if (device->pimpl_->SetToWake(self) == 0) {
     283      is_suspended = true;
     284      cond->CondWait();
     285      is_suspended = false;
     286    } else {
     287      self->Err("%s is already waiting an update\n",
     288                device->ObjectName().c_str());
     289      status = -1;
    49290    }
    50     if(priority>MAX_THREAD_PRIORITY)
    51     {
    52         priority=MAX_THREAD_PRIORITY;
    53         self->Warn("priority set to %i\n",MAX_THREAD_PRIORITY);
    54     }
    55 
    56     this->priority=priority;
    57     period=100*1000*1000;//100ms par defaut
    58     min_jitter=1000*1000*1000;
    59     max_jitter=0;
    60     mean_jitter=0;
    61     last=0;
    62     cpt=0;
    63 }
    64 
    65 Thread_impl::~Thread_impl()
    66 {
    67     SafeStop();
    68     Join();
    69 }
    70 
    71 void Thread_impl::Start(void)
    72 {
    73     int status;
    74 
    75     isRunning=true;
    76 
    77 #ifdef __XENO__
    78     string th_name=getFrameworkManager()->ObjectName()+ "-" + self->ObjectName();
    79 
    80 #ifdef RT_STACK_SIZE
    81     status=rt_task_create(&task_rt, th_name.c_str(), RT_STACK_SIZE, (int)priority, T_JOINABLE);
    82 #else
    83     status=rt_task_create(&task_rt, th_name.c_str(), 0,(int)priority, T_JOINABLE);
    84 #endif //RT_STACK_SIZE
    85     if(status!=0)
    86     {
    87         self->Err("rt_task_create error (%s)\n",strerror(-status));
    88     }
    89     else
    90     {
    91         //_printf("rt_task_create ok %s\n",th_name);
    92     }
    93 
    94     status=rt_task_start(&task_rt, &main_rt, (void*)this);
    95     if(status!=0)
    96     {
    97         self->Err("rt_task_start error (%s)\n",strerror(-status));
    98     }
    99      else
    100      {
    101         //_printf("rt_task_start ok %s\n",th_name);
    102      }
    103 
    104     // Initialise the rt_print buffer for this task explicitly
    105     rt_print_init(512, th_name.c_str());
    106 
    107 #else //__XENO__
    108 
    109     // Initialize thread creation attributes
    110     pthread_attr_t attr;
    111     if(pthread_attr_init(&attr) != 0)
    112     {
    113         self->Err("Error pthread_attr_init\n");
    114     }
    115 
    116 #ifdef NRT_STACK_SIZE
    117     if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0)
    118     {
    119         self->Err("Error pthread_attr_setstacksize\n");
    120     }
    121 #endif //NRT_STACK_SIZE
    122 
    123     if(pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED)!=0)
    124     {
    125         self->Err("Error pthread_attr_setinheritsched\n");
    126     }
    127 
    128     if(pthread_attr_setschedpolicy(&attr, SCHED_FIFO)!=0)
    129     {
    130         self->Err("Error pthread_attr_setschedpolicy\n");
    131     }
    132 
    133     struct sched_param parm;
    134     parm.sched_priority = priority;
    135     if(pthread_attr_setschedparam(&attr,&parm)!=0)
    136     {
    137         self->Err("Error pthread_attr_setschedparam\n");
    138     }
    139 
    140     next_time=GetTime()+period;
    141 
    142     if(pthread_create(&task_nrt,  &attr, main_nrt, (void*)this) != 0)
    143     {
    144         self->Err("pthread_create error\n");
    145     }
    146 
    147     if(pthread_attr_destroy(&attr) != 0)
    148     {
    149         self->Err("Error pthread_attr_destroy\n");
    150     }
    151 
    152 #endif //__XENO__
    153 }
    154 
    155 #ifdef __XENO__
    156 void Thread_impl::main_rt(void * arg)
    157 {
    158     Thread_impl *caller = (Thread_impl*)arg;
    159 
    160     // Perform auto-init of rt_print buffers if the task doesn't do so
    161     rt_print_auto_init(1);
    162 
    163     caller->self->Run();
    164 
    165     caller->PrintStats();
    166 }
    167 #else
    168 void* Thread_impl::main_nrt(void * arg)
    169 {
    170     Thread_impl *caller = (Thread_impl*)arg;
    171 
    172     caller->self->Run();
    173 
    174     caller->PrintStats();
    175 
    176     pthread_exit(0);
    177 }
    178 #endif //__XENO__
    179 
    180 void Thread_impl::SetPeriodUS(uint32_t period) {
    181     if(period==0) {
    182         self->Err("Period must be>0\n");
    183         return;
    184     }
    185 
    186 #ifdef __XENO__
    187     int status=rt_task_set_periodic(&task_rt, TM_NOW, period*1000);
    188     if(status!=0) self->Err("Error rt_task_set_periodic %s\n",strerror(-status));
    189 #else
    190     next_time-=period;
    191     next_time+=period*1000;
    192 #endif
    193     this->period=period*1000;
    194     period_set=true;
    195 }
    196 
    197 uint32_t Thread_impl::GetPeriodUS() const {
    198     return this->period/1000;
    199 }
    200 
    201 void Thread_impl::SetPeriodMS(uint32_t period) {
    202     if(period==0) {
    203         self->Err("Period must be>0\n");
    204         return;
    205     }
    206 
    207 #ifdef __XENO__
    208     int status=rt_task_set_periodic(&task_rt, TM_NOW, period*1000*1000);
    209     if(status!=0) self->Err("Error rt_task_set_periodic %s\n",strerror(-status));
    210 #else
    211     next_time-=period;
    212     next_time+=period*1000*1000;
    213 #endif
    214     this->period=period*1000*1000;
    215     period_set=true;
    216 }
    217 
    218 uint32_t Thread_impl::GetPeriodMS() const {
    219     return this->period/1000/1000;
    220 }
    221 
    222 void Thread_impl::WaitPeriod(void)
    223 {
    224     if(period_set==false)
    225     {
    226         self->Err("Period must be set befaore calling this method\n");
    227         return;
    228     }
    229 #ifdef __XENO__
    230     unsigned long overruns_r;
    231     int status=rt_task_wait_period(&overruns_r);
    232     if(status!=0) self->Err("Error rt_task_wait_period %s\n",strerror(-status));
    233     if(status==-ETIMEDOUT) self->Err("overrun: %lld\n",overruns_r);
    234 #else
    235     self->SleepUntil(next_time);
    236     next_time+=period;
    237 #endif
    238     ComputeJitter(GetTime());
    239 }
    240 
    241 
    242 void Thread_impl::Suspend(void) {
    243     if(isRunning==false) {
    244         self->Err("thread is not started\n");
    245         return;
    246     }
    247 
    248     cond->GetMutex(),
    249     is_suspended=true;
    250     cond->CondWait();
    251     is_suspended=false;
     291
    252292    cond->ReleaseMutex();
    253 }
    254 
    255 bool Thread_impl::SuspendUntil(Time date) {
    256     if(isRunning==false) {
    257         self->Err("thread is not started\n");
    258         return false;
    259     }
    260 
    261     cond->GetMutex(),
    262     is_suspended=true;
    263     bool success=cond->CondWaitUntil(date);
    264     is_suspended=false;
    265     cond->ReleaseMutex();
    266     return success;
    267 }
    268 
    269 bool Thread_impl::IsSuspended(void)
    270 {
    271     bool result;
    272 
    273     cond->GetMutex();
    274     result=is_suspended;
    275     cond->ReleaseMutex();
    276 
    277     return result;
    278 }
    279 
    280 void Thread_impl::Resume(void)
    281 {
    282     if(isRunning==false)
    283     {
    284         self->Err("thread is not started\n");
    285         return;
    286     }
    287 
    288     cond->GetMutex();
    289     if(is_suspended==true)
    290     {
    291         cond->CondSignal();
    292     }
    293     else
    294     {
    295         self->Err("thread is not suspended\n");
    296     }
    297     cond->ReleaseMutex();
    298 }
    299 
    300 int Thread_impl::WaitUpdate(const IODevice* device)
    301 {
    302     int status=0;
    303 
    304     if(IsSuspended()==true)
    305     {
    306         self->Err("thread is already supended\n");
    307         status=-1;
    308     }
    309     else
    310     {
    311         cond->GetMutex();
    312 
    313         if(device->pimpl_->SetToWake(self)==0)
    314         {
    315             is_suspended=true;
    316             cond->CondWait();
    317             is_suspended=false;
    318         }
    319         else
    320         {
    321             self->Err("%s is already waiting an update\n",device->ObjectName().c_str());
    322             status=-1;
    323         }
    324 
    325         cond->ReleaseMutex();
    326     }
    327 
    328     return status;
    329 }
    330 
    331 void Thread_impl::PrintStats(void)
    332 {
    333 #ifdef __XENO__
    334     RT_TASK_INFO info;
    335 
    336     int status=rt_task_inquire(NULL, &info);
    337     if(status!=0)
    338     {
    339         self->Err("Error rt_task_inquire %s\n",strerror(-status));
    340     }
    341     else
    342 #endif
    343     {
     293  }
     294
     295  return status;
     296}
     297
     298void Thread_impl::PrintStats(void) {
     299#ifdef __XENO__
     300  RT_TASK_INFO info;
     301
     302  int status = rt_task_inquire(NULL, &info);
     303  if (status != 0) {
     304    self->Err("Error rt_task_inquire %s\n", strerror(-status));
     305  } else
     306#endif
     307  {
    344308#ifndef __XENO__
    345         //if(last!=0)
    346 #endif
    347         {
    348             Printf("Thread::%s :\n",self->ObjectName().c_str());
    349         }
    350 #ifdef __XENO__
    351         Printf("    number of context switches: %i\n",info.ctxswitches);
    352         Printf("    number of primary->secondary mode switch: %i\n",info.modeswitches);
    353         //printf("number of page faults: %i\n",info.pagefaults);
    354         Printf("    execution time (ms) in primary mode: %lld\n",info.exectime/1000000);
     309// if(last!=0)
     310#endif
     311    { Printf("Thread::%s :\n", self->ObjectName().c_str()); }
     312#ifdef __XENO__
     313    Printf("    number of context switches: %i\n", info.ctxswitches);
     314    Printf("    number of primary->secondary mode switch: %i\n",
     315           info.modeswitches);
     316    // printf("number of page faults: %i\n",info.pagefaults);
     317    Printf("    execution time (ms) in primary mode: %lld\n",
     318           info.exectime / 1000000);
    355319#else
    356320/*
    … …  
    358322        getrusage(RUSAGE_THREAD,&r_usage);
    359323        printf("    memory usage = %ld\n",r_usage.ru_maxrss);
    360         printf("RUSAGE :ru_utime => %lld [sec] : %lld [usec], :ru_stime => %lld [sec] : %lld [usec] \n",
     324        printf("RUSAGE :ru_utime => %lld [sec] : %lld [usec], :ru_stime => %lld
     325   [sec] : %lld [usec] \n",
    361326           (int64_t)r_usage.ru_utime.tv_sec, (int64_t)r_usage.ru_utime.tv_usec,
    362            (int64_t)r_usage.ru_stime.tv_sec, (int64_t)r_usage.ru_stime.tv_usec);*/
    363 #endif
    364         if(last!=0)
    365         {
    366             Printf("    min jitter (ns): %lld\n",min_jitter);
    367             Printf("    max jitter (ns): %lld\n",max_jitter);
    368             Printf("    jitter moy (ns): %lld\n",mean_jitter/cpt);
    369             Printf("    itertions: %lld\n",cpt);
    370         }
     327           (int64_t)r_usage.ru_stime.tv_sec,
     328   (int64_t)r_usage.ru_stime.tv_usec);*/
     329#endif
     330    if (last != 0) {
     331      Printf("    min jitter (ns): %lld\n", min_jitter);
     332      Printf("    max jitter (ns): %lld\n", max_jitter);
     333      Printf("    jitter moy (ns): %lld\n", mean_jitter / cpt);
     334      Printf("    itertions: %lld\n", cpt);
    371335    }
    372 }
    373 
    374 void Thread_impl::Join(void)
    375 {
    376     if(isRunning==true)
    377     {
    378         int status;
    379 
    380 #ifdef __XENO__
    381         status=rt_task_join(&task_rt);
    382 #else
    383         status=pthread_join(task_nrt,NULL);
    384 #endif
    385         if(status!=0) self->Err("error %s\n",strerror(-status));
    386         isRunning=false;
    387     }
    388 }
    389 
    390 void Thread_impl::ComputeJitter(Time time)
    391 {
    392     Time diff,delta;
    393     diff=time-last;
    394 
    395     if(diff>=period)
    396     {
    397         delta=diff-period;
    398     }
    399     else
    400     {
    401         delta=period-diff;
    402     }
    403     //if(delta==0) rt_printf("%lld %lld\n",time,last);
    404     last=time;
    405     if(diff==time) return;
    406 
    407     if(delta>max_jitter) max_jitter=delta;
    408     if(delta<min_jitter) min_jitter=delta;
    409     mean_jitter+=delta;
    410     cpt++;
    411 
    412     //Printf("Thread::%s jitter moy (ns): %lld\n",self->ObjectName().c_str(),mean_jitter/cpt);
    413 
    414 }
    415 
    416 void Thread_impl::SafeStop(void)
    417 {
    418     tobestopped=true;
    419     if(IsSuspended()) Resume();
    420 }
    421 
    422 bool Thread_impl::ToBeStopped(void)
    423 {
    424     return tobestopped;
    425 }
     336  }
     337}
     338
     339void Thread_impl::Join(void) {
     340  if (isRunning == true) {
     341    int status;
     342
     343#ifdef __XENO__
     344    status = rt_task_join(&task_rt);
     345#else
     346    status = pthread_join(task_nrt, NULL);
     347#endif
     348    if (status != 0)
     349      self->Err("error %s\n", strerror(-status));
     350    isRunning = false;
     351  }
     352}
     353
     354void Thread_impl::ComputeJitter(Time time) {
     355  Time diff, delta;
     356  diff = time - last;
     357
     358  if (diff >= period) {
     359    delta = diff - period;
     360  } else {
     361    delta = period - diff;
     362  }
     363  // if(delta==0) rt_printf("%lld %lld\n",time,last);
     364  last = time;
     365  if (diff == time)
     366    return;
     367
     368  if (delta > max_jitter)
     369    max_jitter = delta;
     370  if (delta < min_jitter)
     371    min_jitter = delta;
     372  mean_jitter += delta;
     373  cpt++;
     374
     375  // Printf("Thread::%s jitter moy (ns):
     376  // %lld\n",self->ObjectName().c_str(),mean_jitter/cpt);
     377}
     378
     379void Thread_impl::SafeStop(void) {
     380  tobestopped = true;
     381  if (IsSuspended())
     382    Resume();
     383}
     384
     385bool Thread_impl::ToBeStopped(void) { return tobestopped; }

  • trunk/lib/FlairCore/src/UdtSocket.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair
    27 {
    28 namespace core
    29 {
     26namespace flair {
     27namespace core {
    3028
    31 UdtSocket::UdtSocket(const Object* parent,const std::string name,bool _blockOnSend,bool _blockOnReceive):ConnectedSocket(parent,name) {
    32     UDT::startup();
    33     blockOnSend=_blockOnSend;
    34     blockOnReceive=_blockOnReceive;
     29UdtSocket::UdtSocket(const Object *parent, const std::string name,
     30                     bool _blockOnSend, bool _blockOnReceive)
     31    : ConnectedSocket(parent, name) {
     32  UDT::startup();
     33  blockOnSend = _blockOnSend;
     34  blockOnReceive = _blockOnReceive;
    3535}
    3636
    37 UdtSocket::~UdtSocket(){
    38 }
     37UdtSocket::~UdtSocket() {}
    3938
    40 void UdtSocket::Listen(const unsigned int port,const std::string localAddress) {
    41     socket = UDT::socket(AF_INET, SOCK_DGRAM, 0);
     39void UdtSocket::Listen(const unsigned int port,
     40                       const std::string localAddress) {
     41  socket = UDT::socket(AF_INET, SOCK_DGRAM, 0);
    4242
    43     UDT::setsockopt(socket, 0, UDT_SNDSYN, &blockOnSend, sizeof(bool));
    44     UDT::setsockopt(socket, 0, UDT_RCVSYN, &blockOnReceive, sizeof(bool));
    45     bool reuse = true;
    46     UDT::setsockopt(socket, 0, UDT_REUSEADDR, &reuse, sizeof(bool));
     43  UDT::setsockopt(socket, 0, UDT_SNDSYN, &blockOnSend, sizeof(bool));
     44  UDT::setsockopt(socket, 0, UDT_RCVSYN, &blockOnReceive, sizeof(bool));
     45  bool reuse = true;
     46  UDT::setsockopt(socket, 0, UDT_REUSEADDR, &reuse, sizeof(bool));
    4747
    48     sockaddr_in my_addr;
    49     my_addr.sin_family = AF_INET;
    50     my_addr.sin_port = htons(port);
    51     if (localAddress=="ANY") {
    52         my_addr.sin_addr.s_addr=INADDR_ANY;
    53     } else {
    54         inet_aton(localAddress.c_str(),&(my_addr.sin_addr));
    55     }
    56     memset(&(my_addr.sin_zero), '\0', 8);
     48  sockaddr_in my_addr;
     49  my_addr.sin_family = AF_INET;
     50  my_addr.sin_port = htons(port);
     51  if (localAddress == "ANY") {
     52    my_addr.sin_addr.s_addr = INADDR_ANY;
     53  } else {
     54    inet_aton(localAddress.c_str(), &(my_addr.sin_addr));
     55  }
     56  memset(&(my_addr.sin_zero), '\0', 8);
    5757
    58     if (UDT::ERROR == UDT::bind(socket, (sockaddr*)&my_addr, sizeof(my_addr))) {
    59       Err("bind, %s\n",UDT::getlasterror().getErrorMessage());
    60     }
     58  if (UDT::ERROR == UDT::bind(socket, (sockaddr *)&my_addr, sizeof(my_addr))) {
     59    Err("bind, %s\n", UDT::getlasterror().getErrorMessage());
     60  }
    6161
    62     UDT::listen(socket, 1);
     62  UDT::listen(socket, 1);
    6363}
    6464
    6565UdtSocket *UdtSocket::Accept(Time timeout) {
    66     // TIMEOUT UNSUPPORTED!!
    67     UdtSocket *acceptedSocket=new UdtSocket(this->Parent(),this->ObjectName());
    68     acceptedSocket->blockOnSend=this->blockOnSend;
    69     acceptedSocket->blockOnReceive=this->blockOnReceive;
     66  // TIMEOUT UNSUPPORTED!!
     67  UdtSocket *acceptedSocket = new UdtSocket(this->Parent(), this->ObjectName());
     68  acceptedSocket->blockOnSend = this->blockOnSend;
     69  acceptedSocket->blockOnReceive = this->blockOnReceive;
    7070
    71     sockaddr_in their_addr;
    72     int namelen = sizeof(their_addr);
     71  sockaddr_in their_addr;
     72  int namelen = sizeof(their_addr);
    7373
    74     if ((acceptedSocket->socket = UDT::accept(socket, (sockaddr*)&their_addr, &namelen))==UDT::INVALID_SOCK) {
    75         Err("accept: %s, code %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
    76     }
     74  if ((acceptedSocket->socket = UDT::accept(socket, (sockaddr *)&their_addr,
     75                                            &namelen)) == UDT::INVALID_SOCK) {
     76    Err("accept: %s, code %i\n", UDT::getlasterror().getErrorMessage(),
     77        UDT::getlasterror().getErrorCode());
     78  }
    7779
    78     return acceptedSocket;
     80  return acceptedSocket;
    7981}
    8082
    81 bool UdtSocket::Connect(const unsigned int port,const std::string distantAddress,Time timeout) {
    82     bool success=true;
    83     socket=UDT::socket(AF_INET, SOCK_DGRAM, 0);
     83bool UdtSocket::Connect(const unsigned int port,
     84                        const std::string distantAddress, Time timeout) {
     85  bool success = true;
     86  socket = UDT::socket(AF_INET, SOCK_DGRAM, 0);
    8487
    85     UDT::setsockopt(socket, 0, UDT_SNDSYN, &blockOnSend, sizeof(bool));
    86     UDT::setsockopt(socket, 0, UDT_RCVSYN, &blockOnReceive, sizeof(bool));
    87     bool reuse = true;
    88     UDT::setsockopt(socket, 0, UDT_REUSEADDR, &reuse, sizeof(bool));
     88  UDT::setsockopt(socket, 0, UDT_SNDSYN, &blockOnSend, sizeof(bool));
     89  UDT::setsockopt(socket, 0, UDT_RCVSYN, &blockOnReceive, sizeof(bool));
     90  bool reuse = true;
     91  UDT::setsockopt(socket, 0, UDT_REUSEADDR, &reuse, sizeof(bool));
    8992
    90     sockaddr_in serv_addr;
    91     serv_addr.sin_family = AF_INET;
    92     serv_addr.sin_port = htons(short(port));
    93     if (inet_pton(AF_INET, distantAddress.c_str(), &serv_addr.sin_addr) <= 0) {
    94         printf("incorrect network address.");
    95         success=false;
    96     }
    97     memset(&(serv_addr.sin_zero), '\0', 8);
     93  sockaddr_in serv_addr;
     94  serv_addr.sin_family = AF_INET;
     95  serv_addr.sin_port = htons(short(port));
     96  if (inet_pton(AF_INET, distantAddress.c_str(), &serv_addr.sin_addr) <= 0) {
     97    printf("incorrect network address.");
     98    success = false;
     99  }
     100  memset(&(serv_addr.sin_zero), '\0', 8);
    98101
    99     if (UDT::ERROR == UDT::connect(socket, (sockaddr*)&serv_addr, sizeof(serv_addr))) {
    100         success=false;
    101     }
    102     if (!success) {
    103         UDT::close(socket);
    104         return false;
    105     } else return true;
     102  if (UDT::ERROR ==
     103      UDT::connect(socket, (sockaddr *)&serv_addr, sizeof(serv_addr))) {
     104    success = false;
     105  }
     106  if (!success) {
     107    UDT::close(socket);
     108    return false;
     109  } else
     110    return true;
    106111}
    107112
    108 ssize_t UdtSocket::SendMessage(const char* buffer,size_t bufferSize,Time timeout){
    109     int udtTimeout=timeout;
    110     if (blockOnSend) {
    111         if(UDT::setsockopt(socket, 0, UDT_SNDTIMEO, &udtTimeout, sizeof(udtTimeout))!=0) Err("error UDT_SNDTIMEO %s\n", UDT::getlasterror().getErrorMessage());
    112     }
    113     ssize_t bytesSent=UDT::sendmsg(socket, buffer, bufferSize, -1,true);
     113ssize_t UdtSocket::SendMessage(const char *buffer, size_t bufferSize,
     114                               Time timeout) {
     115  int udtTimeout = timeout;
     116  if (blockOnSend) {
     117    if (UDT::setsockopt(socket, 0, UDT_SNDTIMEO, &udtTimeout,
     118                        sizeof(udtTimeout)) != 0)
     119      Err("error UDT_SNDTIMEO %s\n", UDT::getlasterror().getErrorMessage());
     120  }
     121  ssize_t bytesSent = UDT::sendmsg(socket, buffer, bufferSize, -1, true);
    114122
    115     return bytesSent;
     123  return bytesSent;
    116124}
    117125
    118 ssize_t UdtSocket::RecvMessage(char* buffer,size_t bufferSize,Time timeout){
    119     int udtTimeout=timeout;
    120     if (blockOnReceive) {
    121         if(UDT::setsockopt(socket, 0, UDT_RCVTIMEO, &udtTimeout, sizeof(udtTimeout))!=0) Err("error UDT_RCVTIMEO %s\n", UDT::getlasterror().getErrorMessage());
    122     }
    123     ssize_t bytesRead= UDT::recvmsg(socket,buffer,bufferSize);
     126ssize_t UdtSocket::RecvMessage(char *buffer, size_t bufferSize, Time timeout) {
     127  int udtTimeout = timeout;
     128  if (blockOnReceive) {
     129    if (UDT::setsockopt(socket, 0, UDT_RCVTIMEO, &udtTimeout,
     130                        sizeof(udtTimeout)) != 0)
     131      Err("error UDT_RCVTIMEO %s\n", UDT::getlasterror().getErrorMessage());
     132  }
     133  ssize_t bytesRead = UDT::recvmsg(socket, buffer, bufferSize);
    124134
    125 /*
    126     if(bytesRead<0) {
    127         if(UDT::getlasterror().getErrorCode()==CUDTException::ECONNLOST) {
    128             connection_lost=true;
    129         }
    130     }
    131 */
    132     return bytesRead;
     135  /*
     136      if(bytesRead<0) {
     137          if(UDT::getlasterror().getErrorCode()==CUDTException::ECONNLOST) {
     138              connection_lost=true;
     139          }
     140      }
     141  */
     142  return bytesRead;
    133143}
    134144
    135 uint16_t UdtSocket::NetworkToHost16(uint16_t data) {
    136     return ntohs(data);
    137 }
     145uint16_t UdtSocket::NetworkToHost16(uint16_t data) { return ntohs(data); }
    138146
    139 uint16_t UdtSocket::HostToNetwork16(uint16_t data) {
    140     return htons(data);
    141 }
     147uint16_t UdtSocket::HostToNetwork16(uint16_t data) { return htons(data); }
    142148
    143 uint32_t UdtSocket::NetworkToHost32(uint32_t data) {
    144     return ntohl(data);
    145 }
     149uint32_t UdtSocket::NetworkToHost32(uint32_t data) { return ntohl(data); }
    146150
    147 uint32_t UdtSocket::HostToNetwork32(uint32_t data) {
    148     return htonl(data);
    149 }
     151uint32_t UdtSocket::HostToNetwork32(uint32_t data) { return htonl(data); }
    150152
    151153} // end namespace core

  • trunk/lib/FlairCore/src/UdtSocket.h

    r2 r15  
    1818#include <ConnectedSocket.h>
    1919
    20 namespace flair
    21 {
    22 namespace core
    23 {
    24     /*! \class UdtSocket
    25     *
    26     * \brief Class encapsulating a UDT socket
    27     *
    28     */
    29     class UdtSocket:public ConnectedSocket {
    30     public:
    31         UdtSocket(const Object* parent,const std::string name,bool blockOnSend=false,bool blockOnReceive=true);
    32         ~UdtSocket();
    33         void Listen(const unsigned int port,const std::string localAddress="ANY");
    34         UdtSocket *Accept(Time timeout); //should throw an exception if not a listening socket
    35         bool Connect(const unsigned int port,const std::string distantAddress,Time timeout); // /!\ timeout is ignored
    36         ssize_t SendMessage(const char* message,size_t message_len,Time timeout);
    37         ssize_t RecvMessage(char* buf,size_t buf_len,Time timeout);
     20namespace flair {
     21namespace core {
     22/*! \class UdtSocket
     23*
     24* \brief Class encapsulating a UDT socket
     25*
     26*/
     27class UdtSocket : public ConnectedSocket {
     28public:
     29  UdtSocket(const Object *parent, const std::string name,
     30            bool blockOnSend = false, bool blockOnReceive = true);
     31  ~UdtSocket();
     32  void Listen(const unsigned int port, const std::string localAddress = "ANY");
     33  UdtSocket *
     34  Accept(Time timeout); // should throw an exception if not a listening socket
     35  bool Connect(const unsigned int port, const std::string distantAddress,
     36               Time timeout); // /!\ timeout is ignored
     37  ssize_t SendMessage(const char *message, size_t message_len, Time timeout);
     38  ssize_t RecvMessage(char *buf, size_t buf_len, Time timeout);
    3839
    39         uint16_t NetworkToHost16(uint16_t data);
    40         uint16_t HostToNetwork16(uint16_t data);
    41         uint32_t NetworkToHost32(uint32_t data);
    42         uint32_t HostToNetwork32(uint32_t data);
     40  uint16_t NetworkToHost16(uint16_t data);
     41  uint16_t HostToNetwork16(uint16_t data);
     42  uint32_t NetworkToHost32(uint32_t data);
     43  uint32_t HostToNetwork32(uint32_t data);
    4344
    44     private:
    45         UDTSOCKET socket;
    46         bool blockOnSend;
    47         bool blockOnReceive;
    48     };
     45private:
     46  UDTSOCKET socket;
     47  bool blockOnSend;
     48  bool blockOnReceive;
     49};
    4950
    5051} // end namespace core

  • trunk/lib/FlairCore/src/Unix_I2cPort.cpp

    r2 r15  
    1818#include "Unix_I2cPort.h"
    1919#include <errno.h>
    20 #include <fcntl.h>   /* File control definitions */
     20#include <fcntl.h> /* File control definitions */
    2121#include <unistd.h>
    2222#include <sys/ioctl.h>
    2323#include <linux/i2c-dev.h>
    2424
    25 
    2625using std::string;
    2726
    28 namespace flair
    29 {
    30 namespace core
    31 {
     27namespace flair {
     28namespace core {
    3229
    33 Unix_I2cPort::Unix_I2cPort(const Object* parent,string name,string device) : I2cPort(parent,name)
    34 {
    35     //open port
    36     fd = open( device.c_str(), O_RDWR );
    37         if (fd == -1)
    38         {
    39                 Err("open_port: Unable to open %s\n",device.c_str());
    40         }
     30Unix_I2cPort::Unix_I2cPort(const Object *parent, string name, string device)
     31    : I2cPort(parent, name) {
     32  // open port
     33  fd = open(device.c_str(), O_RDWR);
     34  if (fd == -1) {
     35    Err("open_port: Unable to open %s\n", device.c_str());
     36  }
    4137}
    4238
    43 Unix_I2cPort::~Unix_I2cPort()
    44 {
    45     close(fd);
     39Unix_I2cPort::~Unix_I2cPort() { close(fd); }
     40
     41void Unix_I2cPort::SetRxTimeout(core::Time timeout_ns) {
     42  Warn("not implemented\n");
    4643}
    4744
    48 void Unix_I2cPort::SetRxTimeout(core::Time timeout_ns)
    49 {
    50     Warn("not implemented\n");
     45void Unix_I2cPort::SetTxTimeout(core::Time timeout_ns) {
     46  Warn("not implemented\n");
    5147}
    5248
    53 void Unix_I2cPort::SetTxTimeout(core::Time timeout_ns)
    54 {
    55     Warn("not implemented\n");
     49int Unix_I2cPort::SetSlave(uint16_t address) {
     50  int err = ioctl(fd, I2C_SLAVE_FORCE, address);
     51  if (err < 0) {
     52    Err("Failed to set slave address\n");
     53  }
     54
     55  return err;
    5656}
    5757
    58 int Unix_I2cPort::SetSlave(uint16_t address)
    59 {
    60     int err=ioctl( fd, I2C_SLAVE_FORCE, address);
    61         if( err < 0 )
    62     {
    63         Err("Failed to set slave address\n");
    64     }
    65 
    66         return err;
     58ssize_t Unix_I2cPort::Write(const void *buf, size_t nbyte) {
     59  return write(fd, buf, nbyte);
    6760}
    6861
    69 ssize_t Unix_I2cPort::Write(const void *buf,size_t nbyte)
    70 {
    71     return write(fd,buf, nbyte);
    72 }
    73 
    74 ssize_t Unix_I2cPort::Read(void *buf,size_t nbyte)
    75 {
    76     return read(fd,buf, nbyte);
     62ssize_t Unix_I2cPort::Read(void *buf, size_t nbyte) {
     63  return read(fd, buf, nbyte);
    7764}
    7865
    7966} // end namespace core
    8067} // end namespace flair
    81 

  • trunk/lib/FlairCore/src/Unix_I2cPort.h

    r2 r15  
    1616#include <I2cPort.h>
    1717
    18 namespace flair
    19 {
    20 namespace core
    21 {
    22     /*! \class Unix_I2cPort
    23     *
    24     * \brief Class for unix serial port
    25     *
    26     */
    27     class Unix_I2cPort: public I2cPort
    28     {
     18namespace flair {
     19namespace core {
     20/*! \class Unix_I2cPort
     21*
     22* \brief Class for unix serial port
     23*
     24*/
     25class Unix_I2cPort : public I2cPort {
    2926
    30         public:
    31             /*!
    32             * \brief Constructor
    33             *
    34             * Construct an unix i2c port
    35             *
    36             * \param parent parent
    37             * \param name name
    38             * \param device serial device (ex /dev/i2c-1)
    39             */
    40             Unix_I2cPort(const Object* parent,std::string port_name,std::string device);
     27public:
     28  /*!
     29  * \brief Constructor
     30  *
     31  * Construct an unix i2c port
     32  *
     33  * \param parent parent
     34  * \param name name
     35  * \param device serial device (ex /dev/i2c-1)
     36  */
     37  Unix_I2cPort(const Object *parent, std::string port_name, std::string device);
    4138
    42             /*!
    43             * \brief Destructor
    44             *
    45             */
    46             ~Unix_I2cPort();
     39  /*!
     40  * \brief Destructor
     41  *
     42  */
     43  ~Unix_I2cPort();
    4744
    48             /*!
    49             * \brief Set slave's address
    50             *
    51             * This method need to be called before any communication.
    52             *
    53             * \param address slave's address
    54             */
    55             int SetSlave(uint16_t address);
     45  /*!
     46  * \brief Set slave's address
     47  *
     48  * This method need to be called before any communication.
     49  *
     50  * \param address slave's address
     51  */
     52  int SetSlave(uint16_t address);
    5653
    57             /*!
    58             * \brief Set RX timeout
    59             *
    60             * Timeout for waiting datas.
    61             *
    62             * \param timeout_ns timeout in nano second
    63             */
    64             void SetRxTimeout(Time timeout_ns);
     54  /*!
     55  * \brief Set RX timeout
     56  *
     57  * Timeout for waiting datas.
     58  *
     59  * \param timeout_ns timeout in nano second
     60  */
     61  void SetRxTimeout(Time timeout_ns);
    6562
    66             /*!
    67             * \brief Set TX timeout
    68             *
    69             * Timeout for waiting an ACK from the slave.
    70             *
    71             * \param timeout_ns timeout in nano second
    72             */
    73             void SetTxTimeout(Time timeout_ns);
     63  /*!
     64  * \brief Set TX timeout
     65  *
     66  * Timeout for waiting an ACK from the slave.
     67  *
     68  * \param timeout_ns timeout in nano second
     69  */
     70  void SetTxTimeout(Time timeout_ns);
    7471
    75             /*!
    76             * \brief Write datas
    77             *
    78             * \param buf pointer to datas
    79             * \param nbyte length of datas
    80             *
    81             * \return amount of written datas
    82             */
    83             ssize_t Write(const void *buf,size_t nbyte);
     72  /*!
     73  * \brief Write datas
     74  *
     75  * \param buf pointer to datas
     76  * \param nbyte length of datas
     77  *
     78  * \return amount of written datas
     79  */
     80  ssize_t Write(const void *buf, size_t nbyte);
    8481
    85             /*!
    86             * \brief Read datas
    87             *
    88             * \param buf pointer to datas
    89             * \param nbyte length of datas
    90             *
    91             * \return amount of read datas
    92             */
    93             ssize_t Read(void *buf,size_t nbyte);
     82  /*!
     83  * \brief Read datas
     84  *
     85  * \param buf pointer to datas
     86  * \param nbyte length of datas
     87  *
     88  * \return amount of read datas
     89  */
     90  ssize_t Read(void *buf, size_t nbyte);
    9491
    95         private:
    96             int fd;
    97     };
     92private:
     93  int fd;
     94};
    9895} // end namespace core
    9996} // end namespace flair

  • trunk/lib/FlairCore/src/Unix_SerialPort.cpp

    r2 r15  
    1717
    1818#include "Unix_SerialPort.h"
    19 #include <fcntl.h>   /* File control definitions */
     19#include <fcntl.h> /* File control definitions */
    2020#include <unistd.h>
    2121
    2222using std::string;
    2323
    24 namespace flair
    25 {
    26 namespace core
    27 {
     24namespace flair {
     25namespace core {
    2826
    29 Unix_SerialPort::Unix_SerialPort(const Object* parent,string name,string device) : SerialPort(parent,name)
    30 {
    31     //open port
    32         fd = open(device.c_str(), O_RDWR | O_NOCTTY);// |O_NDELAY|O_NONBLOCK);
    33         if (fd == -1)
    34         {
    35                 Err("open_port: Unable to open %s\n",device.c_str());
    36         }
    37         //fcntl(fd, F_SETFL, 0); //read calls are non blocking
     27Unix_SerialPort::Unix_SerialPort(const Object *parent, string name,
     28                                 string device)
     29    : SerialPort(parent, name) {
     30  // open port
     31  fd = open(device.c_str(), O_RDWR | O_NOCTTY); // |O_NDELAY|O_NONBLOCK);
     32  if (fd == -1) {
     33    Err("open_port: Unable to open %s\n", device.c_str());
     34  }
     35  // fcntl(fd, F_SETFL, 0); //read calls are non blocking
    3836
    39         //Get the current options for the port
     37  // Get the current options for the port
    4038
    41         tcgetattr(fd, &options);
    42         //Set the baud rates to 115200
    43         cfsetispeed(&options, B115200);
    44         cfsetospeed(&options, B115200);
     39  tcgetattr(fd, &options);
     40  // Set the baud rates to 115200
     41  cfsetispeed(&options, B115200);
     42  cfsetospeed(&options, B115200);
    4543
    46         options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
    47         options.c_iflag = 0; //clear input options
    48         options.c_lflag=0; //clear local options
    49         options.c_oflag &= ~OPOST; //clear output options (raw output)
     44  options.c_cflag |= (CLOCAL | CREAD); // Enable the receiver and set local mode
     45  options.c_iflag = 0;                 // clear input options
     46  options.c_lflag = 0;                 // clear local options
     47  options.c_oflag &= ~OPOST;           // clear output options (raw output)
    5048
    51         //Set the new options for the port
    52     options.c_cc[VTIME] = 0;
    53     options.c_cc[VMIN] = 1;
    54         FlushInput();
    55         tcsetattr(fd, TCSANOW, &options);
     49  // Set the new options for the port
     50  options.c_cc[VTIME] = 0;
     51  options.c_cc[VMIN] = 1;
     52  FlushInput();
     53  tcsetattr(fd, TCSANOW, &options);
    5654}
    5755
    58 Unix_SerialPort::~Unix_SerialPort()
    59 {
    60     close(fd);
     56Unix_SerialPort::~Unix_SerialPort() { close(fd); }
     57
     58void Unix_SerialPort::SetBaudrate(int baudrate) {
     59  // set port options
     60  struct termios options;
     61  // Get the current options for the port
     62  tcgetattr(fd, &options);
     63  // Set the baud rates to 115200
     64
     65  switch (baudrate) {
     66  case 1200:
     67    cfsetispeed(&options, B1200);
     68    cfsetospeed(&options, B1200);
     69    break;
     70  case 2400:
     71    cfsetispeed(&options, B2400);
     72    cfsetospeed(&options, B2400);
     73    break;
     74  case 4800:
     75    cfsetispeed(&options, B4800);
     76    cfsetospeed(&options, B4800);
     77    break;
     78  case 9600:
     79    cfsetispeed(&options, B9600);
     80    cfsetospeed(&options, B9600);
     81    break;
     82  case 19200:
     83    cfsetispeed(&options, B19200);
     84    cfsetospeed(&options, B19200);
     85    break;
     86  case 38400:
     87    cfsetispeed(&options, B38400);
     88    cfsetospeed(&options, B38400);
     89    break;
     90  case 115200:
     91    cfsetispeed(&options, B115200);
     92    cfsetospeed(&options, B115200);
     93    break;
     94  case 460800:
     95    cfsetispeed(&options, B460800);
     96    cfsetospeed(&options, B460800);
     97    break;
     98  case 921600:
     99    cfsetispeed(&options, B921600);
     100    cfsetospeed(&options, B921600);
     101    break;
     102  default:
     103    Err("unsupported baudrate\n");
     104  }
     105  tcsetattr(fd, TCSANOW, &options);
     106  FlushInput();
    61107}
    62108
    63 void Unix_SerialPort::SetBaudrate(int baudrate)
    64 {
    65     //set port options
    66         struct termios options;
    67         //Get the current options for the port
    68         tcgetattr(fd, &options);
    69         //Set the baud rates to 115200
     109void Unix_SerialPort::SetRxTimeout(core::Time timeout_ns) {}
    70110
    71         switch(baudrate)
    72         {
    73             case 1200:
    74             cfsetispeed(&options, B1200);
    75             cfsetospeed(&options, B1200);
    76             break;
    77         case 2400:
    78             cfsetispeed(&options, B2400);
    79             cfsetospeed(&options, B2400);
    80             break;
    81         case 4800:
    82             cfsetispeed(&options, B4800);
    83             cfsetospeed(&options, B4800);
    84             break;
    85         case 9600:
    86             cfsetispeed(&options, B9600);
    87             cfsetospeed(&options, B9600);
    88             break;
    89         case 19200:
    90             cfsetispeed(&options, B19200);
    91             cfsetospeed(&options, B19200);
    92             break;
    93         case 38400:
    94             cfsetispeed(&options, B38400);
    95             cfsetospeed(&options, B38400);
    96             break;
    97         case 115200:
    98             cfsetispeed(&options, B115200);
    99             cfsetospeed(&options, B115200);
    100             break;
    101         case 460800:
    102             cfsetispeed(&options, B460800);
    103             cfsetospeed(&options, B460800);
    104             break;
    105         case 921600:
    106             cfsetispeed(&options, B921600);
    107             cfsetospeed(&options, B921600);
    108             break;
    109         default:
    110             Err("unsupported baudrate\n");
    111         }
    112         tcsetattr(fd, TCSANOW, &options);
    113         FlushInput();
     111void Unix_SerialPort::FlushInput(void) { tcflush(fd, TCIFLUSH); }
    114112
     113ssize_t Unix_SerialPort::Write(const void *buf, size_t nbyte) {
     114  return write(fd, buf, nbyte);
    115115}
    116116
    117 void Unix_SerialPort::SetRxTimeout(core::Time timeout_ns)
    118 {
     117ssize_t Unix_SerialPort::Read(void *buf, size_t nbyte) {
     118  if (options.c_cc[VMIN] != nbyte) {
     119    tcgetattr(fd, &options); // bug if not called?
     120    options.c_cc[VTIME] = 0;
     121    options.c_cc[VMIN] = nbyte;
     122    tcsetattr(fd, TCSANOW, &options);
     123  }
    119124
    120 }
    121 
    122 void Unix_SerialPort::FlushInput(void)
    123 {
    124     tcflush(fd, TCIFLUSH);
    125 }
    126 
    127 ssize_t Unix_SerialPort::Write(const void *buf,size_t nbyte)
    128 {
    129     return write(fd, buf, nbyte);
    130 }
    131 
    132 ssize_t Unix_SerialPort::Read(void *buf,size_t nbyte)
    133 {
    134     if(options.c_cc[VMIN] != nbyte)
    135     {
    136         tcgetattr(fd, &options);//bug if not called?
    137         options.c_cc[VTIME] = 0;
    138         options.c_cc[VMIN] = nbyte;
    139         tcsetattr(fd, TCSANOW, &options);
    140     }
    141 
    142     return read(fd, buf, nbyte);
     125  return read(fd, buf, nbyte);
    143126}
    144127
    145128} // end namespace core
    146129} // end namespace flair
    147 

  • trunk/lib/FlairCore/src/Unix_SerialPort.h

    r2 r15  
    1717#include <termios.h> /* POSIX terminal control definitions */
    1818
    19 namespace flair
    20 {
    21 namespace core
    22 {
    23     /*! \class RTDM_I2cPort
    24     *
    25     * \brief Class for unix serial port
    26     *
    27     */
    28     class Unix_SerialPort: public SerialPort
    29     {
     19namespace flair {
     20namespace core {
     21/*! \class RTDM_I2cPort
     22*
     23* \brief Class for unix serial port
     24*
     25*/
     26class Unix_SerialPort : public SerialPort {
    3027
    31         public:
    32             /*!
    33             * \brief Constructor
    34             *
    35             * Construct an unix serial port, with the following default values: \n
    36             * - 115200bps baudrate
    37             *
    38             * \param parent parent
    39             * \param name name
    40             * \param device serial device (ex rtser1)
    41             */
    42             Unix_SerialPort(const Object* parent,std::string port_name,std::string device);
     28public:
     29  /*!
     30  * \brief Constructor
     31  *
     32  * Construct an unix serial port, with the following default values: \n
     33  * - 115200bps baudrate
     34  *
     35  * \param parent parent
     36  * \param name name
     37  * \param device serial device (ex rtser1)
     38  */
     39  Unix_SerialPort(const Object *parent, std::string port_name,
     40                  std::string device);
    4341
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~Unix_SerialPort();
     42  /*!
     43  * \brief Destructor
     44  *
     45  */
     46  ~Unix_SerialPort();
    4947
    50             /*!
    51             * \brief Set baudrate
    52             *
    53             * \param baudrate baudrate
    54             *
    55             */
    56             void SetBaudrate(int baudrate);
     48  /*!
     49  * \brief Set baudrate
     50  *
     51  * \param baudrate baudrate
     52  *
     53  */
     54  void SetBaudrate(int baudrate);
    5755
    58             /*!
    59             * \brief Set RX timeout
    60             *
    61             * Timeout for waiting datas.
    62             *
    63             * \param timeout_ns timeout in nano second
    64             */
    65             void SetRxTimeout(Time timeout_ns);
     56  /*!
     57  * \brief Set RX timeout
     58  *
     59  * Timeout for waiting datas.
     60  *
     61  * \param timeout_ns timeout in nano second
     62  */
     63  void SetRxTimeout(Time timeout_ns);
    6664
    67             /*!
    68             * \brief Write datas
    69             *
    70             * \param buf pointer to datas
    71             * \param nbyte length of datas
    72             *
    73             * \return amount of written datas
    74             */
    75             ssize_t Write(const void *buf,size_t nbyte);
     65  /*!
     66  * \brief Write datas
     67  *
     68  * \param buf pointer to datas
     69  * \param nbyte length of datas
     70  *
     71  * \return amount of written datas
     72  */
     73  ssize_t Write(const void *buf, size_t nbyte);
    7674
    77             /*!
    78             * \brief Read datas
    79             *
    80             * \param buf pointer to datas
    81             * \param nbyte length of datas
    82             *
    83             * \return amount of read datas
    84             */
    85             ssize_t Read(void *buf,size_t nbyte);
     75  /*!
     76  * \brief Read datas
     77  *
     78  * \param buf pointer to datas
     79  * \param nbyte length of datas
     80  *
     81  * \return amount of read datas
     82  */
     83  ssize_t Read(void *buf, size_t nbyte);
    8684
    87             /*!
    88             * \brief Flush input datas
    89             *
    90             */
    91             void FlushInput(void);
     85  /*!
     86  * \brief Flush input datas
     87  *
     88  */
     89  void FlushInput(void);
    9290
    93         private:
    94             int fd;
    95             struct termios options;
    96     };
     91private:
     92  int fd;
     93  struct termios options;
     94};
    9795} // end namespace core
    9896} // end namespace flair

  • trunk/lib/FlairCore/src/Vector2D.cpp

    r2 r15  
    2020#include <math.h>
    2121
    22 namespace flair { namespace core {
     22namespace flair {
     23namespace core {
    2324
    24 Vector2D::Vector2D(float inX,float inY): x(inX),y(inY) {
     25Vector2D::Vector2D(float inX, float inY) : x(inX), y(inY) {}
     26
     27Vector2D::~Vector2D() {}
     28
     29Vector2D &Vector2D::operator=(const Vector2D &vector) {
     30  x = vector.x;
     31  y = vector.y;
     32  return (*this);
    2533}
    2634
    27 Vector2D::~Vector2D() {
    28 
     35Vector2D operator+(const Vector2D &vectorA, const Vector2D &vectorB) {
     36  return Vector2D(vectorA.x + vectorB.x, vectorA.y + vectorB.y);
    2937}
    3038
    31 Vector2D &Vector2D::operator=(const Vector2D &vector) {
    32     x=vector.x;
    33     y=vector.y;
    34     return (*this);
     39Vector2D operator-(const Vector2D &vectorA, const Vector2D &vectorB) {
     40  return Vector2D(vectorA.x - vectorB.x, vectorA.y - vectorB.y);
    3541}
    3642
    37 Vector2D operator+ (const Vector2D &vectorA,const Vector2D &vectorB) {
    38     return Vector2D(vectorA.x + vectorB.x,vectorA.y + vectorB.y);
     43Vector2D operator/(const Vector2D &vector, float coeff) {
     44  if (coeff != 0) {
     45    return Vector2D(vector.x / coeff, vector.y / coeff);
     46  } else {
     47    return Vector2D(0, 0);
     48  }
    3949}
    4050
    41 Vector2D operator- (const Vector2D &vectorA,const Vector2D  &vectorB) {
    42     return Vector2D(vectorA.x - vectorB.x,vectorA.y - vectorB.y);
     51Vector2D operator*(const Vector2D &vector, float coeff) {
     52  return Vector2D(vector.x * coeff, vector.y * coeff);
    4353}
    4454
    45 Vector2D operator/ (const Vector2D &vector, float coeff) {
    46     if(coeff!=0) {
    47         return Vector2D(vector.x/ coeff,vector.y/ coeff);
    48     } else {
    49         return Vector2D(0,0);
    50     }
    51 }
    52 
    53 Vector2D operator * (const Vector2D &vector, float coeff) {
    54     return Vector2D(vector.x * coeff,vector.y * coeff);
    55 }
    56 
    57 Vector2D operator * (float coeff,const Vector2D &vector) {
    58     return Vector2D(vector.x * coeff,vector.y * coeff);
     55Vector2D operator*(float coeff, const Vector2D &vector) {
     56  return Vector2D(vector.x * coeff, vector.y * coeff);
    5957}
    6058
    6159void Vector2D::Rotate(float value) {
    62     float xTmp;
    63     xTmp=x*cosf(value)-y*sinf(value);
    64     y=x*sinf(value)+y*cosf(value);
    65     x=xTmp;
     60  float xTmp;
     61  xTmp = x * cosf(value) - y * sinf(value);
     62  y = x * sinf(value) + y * cosf(value);
     63  x = xTmp;
    6664}
    6765
    68 void Vector2D::RotateDeg(float value) {
    69     Rotate(Euler::ToRadian(value));
     66void Vector2D::RotateDeg(float value) { Rotate(Euler::ToRadian(value)); }
     67
     68float Vector2D::GetNorm(void) const { return sqrt(x * x + y * y); }
     69
     70void Vector2D::Normalize(void) {
     71  float n = GetNorm();
     72  if (n != 0) {
     73    x = x / n;
     74    y = y / n;
     75  }
    7076}
    7177
    72 float Vector2D::GetNorm(void) const {
    73     return sqrt(x*x+y*y);
     78void Vector2D::Saturate(Vector2D min, Vector2D max) {
     79  if (x < min.x)
     80    x = min.x;
     81  if (x > max.x)
     82    x = max.x;
     83
     84  if (y < min.y)
     85    y = min.y;
     86  if (y > max.y)
     87    y = max.y;
    7488}
    7589
    76 void Vector2D::Normalize(void) {
    77     float n=GetNorm();
    78     if(n!=0) {
    79         x=x/n;
    80         y=y/n;
    81     }
    82 }
    83 
    84 void Vector2D::Saturate(Vector2D min,Vector2D max) {
    85     if(x<min.x) x=min.x;
    86     if(x>max.x) x=max.x;
    87 
    88     if(y<min.y) y=min.y;
    89     if(y>max.y) y=max.y;
    90 }
    91 
    92 void Vector2D::Saturate(float min,float max) {
    93     Saturate(Vector2D(min,min),Vector2D(max,max));
     90void Vector2D::Saturate(float min, float max) {
     91  Saturate(Vector2D(min, min), Vector2D(max, max));
    9492}
    9593
    9694void Vector2D::Saturate(const Vector2D &value) {
    97     float x=fabs(value.x);
    98     float y=fabs(value.y);
    99     Saturate(Vector2D(-x,-y),Vector2D(x,y));
     95  float x = fabs(value.x);
     96  float y = fabs(value.y);
     97  Saturate(Vector2D(-x, -y), Vector2D(x, y));
    10098}
    10199
    102100void Vector2D::Saturate(float value) {
    103     float sat=fabs(value);
    104     Saturate(Vector2D(-sat,-sat),Vector2D(sat,sat));
     101  float sat = fabs(value);
     102  Saturate(Vector2D(-sat, -sat), Vector2D(sat, sat));
    105103}
    106104

  • trunk/lib/FlairCore/src/Vector2D.h

    r2 r15  
    1414#define VECTOR2D_H
    1515
    16 namespace flair { namespace core {
     16namespace flair {
     17namespace core {
    1718
    18     /*! \class Vector2D
    19     *
    20     * \brief Class defining a 2D vector
    21     */
    22     class Vector2D {
    23         public:
    24             /*!
    25             * \brief Constructor
    26             *
    27             * Construct a Vector2D using specified values.
    28             *
    29             * \param x
    30             * \param y
    31             */
    32             Vector2D(float x=0,float y=0);
     19/*! \class Vector2D
     20*
     21* \brief Class defining a 2D vector
     22*/
     23class Vector2D {
     24public:
     25  /*!
     26  * \brief Constructor
     27  *
     28  * Construct a Vector2D using specified values.
     29  *
     30  * \param x
     31  * \param y
     32  */
     33  Vector2D(float x = 0, float y = 0);
    3334
    34             /*!
    35             * \brief Destructor
    36             *
    37             */
    38             ~Vector2D();
     35  /*!
     36  * \brief Destructor
     37  *
     38  */
     39  ~Vector2D();
    3940
    40             /*!
    41             * \brief Rotation
    42             *
    43             * \param value rotation value in radians
    44             */
    45             void Rotate(float value);
     41  /*!
     42  * \brief Rotation
     43  *
     44  * \param value rotation value in radians
     45  */
     46  void Rotate(float value);
    4647
    47             /*!
    48             * \brief Rotation
    49             *
    50             * \param value rotation value in degrees
    51             */
    52             void RotateDeg(float value);
     48  /*!
     49  * \brief Rotation
     50  *
     51  * \param value rotation value in degrees
     52  */
     53  void RotateDeg(float value);
    5354
    54             /*!
    55             * \brief Norm
    56             *
    57             * \return value
    58             */
    59             float GetNorm(void) const;
     55  /*!
     56  * \brief Norm
     57  *
     58  * \return value
     59  */
     60  float GetNorm(void) const;
    6061
    61             /*!
    62             * \brief Normalize
    63             */
    64             void Normalize(void);
     62  /*!
     63  * \brief Normalize
     64  */
     65  void Normalize(void);
    6566
    66             /*!
    67             * \brief Saturate
    68             *
    69             * Saturate between min and max
    70             *
    71             * \param min minimum Vector2D value
    72             * \param max maximum Vector2D value
    73             */
    74             void Saturate(Vector2D min,Vector2D max);
     67  /*!
     68  * \brief Saturate
     69  *
     70  * Saturate between min and max
     71  *
     72  * \param min minimum Vector2D value
     73  * \param max maximum Vector2D value
     74  */
     75  void Saturate(Vector2D min, Vector2D max);
    7576
    76             /*!
    77             * \brief Saturate
    78             *
    79             * Saturate between min and max
    80             *
    81             * \param min minimum Vector2D(min,min) value
    82             * \param max maximum Vector2D(max,max) value
    83             */
    84             void Saturate(float min,float max);
     77  /*!
     78  * \brief Saturate
     79  *
     80  * Saturate between min and max
     81  *
     82  * \param min minimum Vector2D(min,min) value
     83  * \param max maximum Vector2D(max,max) value
     84  */
     85  void Saturate(float min, float max);
    8586
    86             /*!
    87             * \brief Saturate
    88             *
    89             * Saturate between -abs(value) and abs(value)
    90             *
    91             * \param value saturation Vector2D value
    92             */
    93             void Saturate(const Vector2D &value);
     87  /*!
     88  * \brief Saturate
     89  *
     90  * Saturate between -abs(value) and abs(value)
     91  *
     92  * \param value saturation Vector2D value
     93  */
     94  void Saturate(const Vector2D &value);
    9495
    95             /*!
    96             * \brief Saturate
    97             *
    98             * Saturate between -abs(Vector2D(value,value)) and abs(Vector2D(value,value))
    99             *
    100             * \param value saturation Vector2D(value,value)
    101             */
    102             void Saturate(float value);
     96  /*!
     97  * \brief Saturate
     98  *
     99  * Saturate between -abs(Vector2D(value,value)) and abs(Vector2D(value,value))
     100  *
     101  * \param value saturation Vector2D(value,value)
     102  */
     103  void Saturate(float value);
    103104
     105  /*!
     106  * \brief x
     107  */
     108  float x;
    104109
    105             /*!
    106             * \brief x
    107             */
    108             float x;
     110  /*!
     111  * \brief y
     112  */
     113  float y;
    109114
    110             /*!
    111             * \brief y
    112             */
    113             float y;
     115  Vector2D &operator=(const Vector2D &vector);
     116};
    114117
    115             Vector2D &operator=(const Vector2D &vector);
    116     };
     118/*! Add
     119*
     120* \brief Add
     121*
     122* \param vectorA vector
     123* \param vectorB vector
     124*/
     125Vector2D operator+(const Vector2D &vectorA, const Vector2D &vectorB);
    117126
    118     /*! Add
    119     *
    120     * \brief Add
    121     *
    122     * \param vectorA vector
    123     * \param vectorB vector
    124     */
    125     Vector2D operator + (const Vector2D &vectorA,const Vector2D &vectorB);
     127/*! Substract
     128*
     129* \brief Substract
     130*
     131* \param vectorA vector
     132* \param vectorB vector
     133*/
     134Vector2D operator-(const Vector2D &vectorA, const Vector2D &vectorB);
    126135
    127     /*! Substract
    128     *
    129     * \brief Substract
    130     *
    131     * \param vectorA vector
    132     * \param vectorB vector
    133     */
    134     Vector2D operator - (const Vector2D &vectorA,const Vector2D &vectorB);
     136/*! Divid
     137*
     138* \brief Divid
     139*
     140* \param vector vector
     141* \param coeff coefficent
     142* \return vector/coefficient
     143*/
     144Vector2D operator/(const Vector2D &vector, float coeff);
    135145
    136     /*! Divid
    137     *
    138     * \brief Divid
    139     *
    140     * \param vector vector
    141     * \param coeff coefficent
    142     * \return vector/coefficient
    143     */
    144     Vector2D operator / (const Vector2D &vector, float coeff);
     146/*! Multiply
     147*
     148* \brief Multiplyf
     149*
     150* \param vector vector
     151* \param coeff coefficent
     152* \return coefficient*vector
     153*/
     154Vector2D operator*(const Vector2D &vector, float coeff);
    145155
    146     /*! Multiply
    147     *
    148     * \brief Multiplyf
    149     *
    150     * \param vector vector
    151     * \param coeff coefficent
    152     * \return coefficient*vector
    153     */
    154     Vector2D operator * (const Vector2D &vector, float coeff);
    155 
    156     /*! Multiply
    157     *
    158     * \brief Multiply
    159     *
    160     * \param coeff coefficent
    161     * \param vector vector
    162     * \return coefficient*vector
    163     */
    164     Vector2D operator * (float coeff,const Vector2D &vector);
     156/*! Multiply
     157*
     158* \brief Multiply
     159*
     160* \param coeff coefficent
     161* \param vector vector
     162* \return coefficient*vector
     163*/
     164Vector2D operator*(float coeff, const Vector2D &vector);
    165165
    166166} // end namespace core

  • trunk/lib/FlairCore/src/Vector3D.cpp

    r2 r15  
    2525//#include "Vector3DSpinBox.h"
    2626
    27 namespace flair { namespace core {
    28 
    29 Vector3D::Vector3D(float inX,float inY,float inZ): x(inX),y(inY),z(inZ){
    30 }
    31 
    32 Vector3D::~Vector3D() {
    33 
    34 }
     27namespace flair {
     28namespace core {
     29
     30Vector3D::Vector3D(float inX, float inY, float inZ) : x(inX), y(inY), z(inZ) {}
     31
     32Vector3D::~Vector3D() {}
    3533/*
    3634void Vector3D::operator=(const gui::Vector3DSpinBox *vector) {
    … …  
    4240
    4341Vector3D &Vector3D::operator=(const Vector3D &vector) {
    44     x=vector.x;
    45     y=vector.y;
    46     z=vector.z;
    47     return (*this);
     42  x = vector.x;
     43  y = vector.y;
     44  z = vector.z;
     45  return (*this);
    4846}
    4947
    5048Vector3D &Vector3D::operator+=(const Vector3D &vector) {
    51     x+=vector.x;
    52     y+=vector.y;
    53     z+=vector.z;
    54     return (*this);
     49  x += vector.x;
     50  y += vector.y;
     51  z += vector.z;
     52  return (*this);
    5553}
    5654
    5755Vector3D &Vector3D::operator-=(const Vector3D &vector) {
    58     x-=vector.x;
    59     y-=vector.y;
    60     z-=vector.z;
    61     return (*this);
     56  x -= vector.x;
     57  y -= vector.y;
     58  z -= vector.z;
     59  return (*this);
    6260}
    6361
    6462float &Vector3D::operator[](size_t idx) {
    65     if(idx==0) {
    66         return x;
    67     } else if(idx==1) {
    68         return y;
    69     } else if(idx==2) {
    70         return z;
    71     } else {
    72         Printf("Vector3D: index %i out of bound\n",idx);
    73         return z;
     63  if (idx == 0) {
     64    return x;
     65  } else if (idx == 1) {
     66    return y;
     67  } else if (idx == 2) {
     68    return z;
     69  } else {
     70    Printf("Vector3D: index %i out of bound\n", idx);
     71    return z;
     72  }
     73}
     74
     75const float &Vector3D::operator[](size_t idx) const {
     76  if (idx == 0) {
     77    return x;
     78  } else if (idx == 1) {
     79    return y;
     80  } else if (idx == 2) {
     81    return z;
     82  } else {
     83    Printf("Vector3D: index %i out of bound\n", idx);
     84    return z;
     85  }
     86}
     87
     88Vector3D CrossProduct(const Vector3D &vectorA, const Vector3D &vectorB) {
     89  return Vector3D(vectorA.y * vectorB.z - vectorA.z * vectorB.y,
     90                  vectorA.z * vectorB.x - vectorA.x * vectorB.z,
     91                  vectorA.x * vectorB.y - vectorA.y * vectorB.x);
     92}
     93
     94float DotProduct(const Vector3D &vectorA, const Vector3D &vectorB) {
     95  return vectorA.x * vectorB.x + vectorA.y * vectorB.y + vectorA.z * vectorB.z;
     96}
     97
     98Vector3D operator+(const Vector3D &vectorA, const Vector3D &vectorB) {
     99  return Vector3D(vectorA.x + vectorB.x, vectorA.y + vectorB.y,
     100                  vectorA.z + vectorB.z);
     101}
     102
     103Vector3D operator-(const Vector3D &vectorA, const Vector3D &vectorB) {
     104  return Vector3D(vectorA.x - vectorB.x, vectorA.y - vectorB.y,
     105                  vectorA.z - vectorB.z);
     106}
     107
     108Vector3D operator-(const Vector3D &vector) {
     109  return Vector3D(-vector.x, -vector.y, -vector.z);
     110}
     111
     112Vector3D operator*(const Vector3D &vectorA, const Vector3D &vectorB) {
     113  return Vector3D(vectorA.x * vectorB.x, vectorA.y * vectorB.y,
     114                  vectorA.z * vectorB.z);
     115}
     116
     117Vector3D operator*(const Vector3D &vector, float coeff) {
     118  return Vector3D(vector.x * coeff, vector.y * coeff, vector.z * coeff);
     119}
     120
     121Vector3D operator*(float coeff, const Vector3D &vector) {
     122  return Vector3D(vector.x * coeff, vector.y * coeff, vector.z * coeff);
     123}
     124
     125Vector3D operator/(const Vector3D &vector, float coeff) {
     126  if (coeff != 0) {
     127    return Vector3D(vector.x / coeff, vector.y / coeff, vector.z / coeff);
     128  } else {
     129    printf("Vector3D: err divinding by 0\n");
     130    return Vector3D(0, 0, 0);
     131  }
     132}
     133
     134void Vector3D::RotateX(float value) {
     135  float y_tmp;
     136  y_tmp = y * cosf(value) - z * sinf(value);
     137  z = y * sinf(value) + z * cosf(value);
     138  y = y_tmp;
     139}
     140
     141void Vector3D::RotateXDeg(float value) { RotateX(Euler::ToRadian(value)); }
     142
     143void Vector3D::RotateY(float value) {
     144  float x_tmp;
     145  x_tmp = x * cosf(value) + z * sinf(value);
     146  z = -x * sinf(value) + z * cosf(value);
     147  x = x_tmp;
     148}
     149
     150void Vector3D::RotateYDeg(float value) { RotateY(Euler::ToRadian(value)); }
     151
     152void Vector3D::RotateZ(float value) {
     153  float x_tmp;
     154  x_tmp = x * cosf(value) - y * sinf(value);
     155  y = x * sinf(value) + y * cosf(value);
     156  x = x_tmp;
     157}
     158
     159void Vector3D::RotateZDeg(float value) { RotateZ(Euler::ToRadian(value)); }
     160
     161void Vector3D::Rotate(const RotationMatrix &matrix) {
     162  float a[3] = {0, 0, 0};
     163  float b[3] = {x, y, z};
     164
     165  for (int i = 0; i < 3; i++) {
     166    for (int j = 0; j < 3; j++) {
     167      a[i] += matrix.m[i][j] * b[j];
    74168    }
    75 }
    76 
    77 const float &Vector3D::operator[](size_t idx) const {
    78     if(idx==0) {
    79         return x;
    80     } else if(idx==1) {
    81         return y;
    82     } else if(idx==2) {
    83         return z;
    84     } else {
    85         Printf("Vector3D: index %i out of bound\n",idx);
    86         return z;
    87     }
    88 }
    89 
    90 Vector3D CrossProduct(const Vector3D &vectorA,const Vector3D &vectorB) {
    91     return Vector3D(vectorA.y*vectorB.z - vectorA.z*vectorB.y,
    92                     vectorA.z*vectorB.x - vectorA.x*vectorB.z,
    93                     vectorA.x*vectorB.y - vectorA.y*vectorB.x);
    94 }
    95 
    96 float DotProduct(const Vector3D &vectorA,const Vector3D &vectorB) {
    97     return vectorA.x*vectorB.x + vectorA.y*vectorB.y+ vectorA.z*vectorB.z;
    98 }
    99 
    100 Vector3D operator+ (const Vector3D &vectorA,const Vector3D &vectorB) {
    101     return Vector3D(vectorA.x + vectorB.x,
    102                     vectorA.y + vectorB.y,
    103                     vectorA.z + vectorB.z);
    104 }
    105 
    106 Vector3D operator- (const Vector3D &vectorA,const Vector3D &vectorB) {
    107     return Vector3D(vectorA.x - vectorB.x,
    108                     vectorA.y - vectorB.y,
    109                     vectorA.z - vectorB.z);
    110 }
    111 
    112 Vector3D operator-(const Vector3D &vector) {
    113     return Vector3D(-vector.x,-vector.y,-vector.z);
    114 }
    115 
    116 Vector3D operator * (const Vector3D &vectorA,const Vector3D &vectorB) {
    117     return Vector3D(vectorA.x* vectorB.x,
    118                     vectorA.y* vectorB.y,
    119                     vectorA.z* vectorB.z);
    120 }
    121 
    122 Vector3D operator * (const Vector3D &vector, float coeff) {
    123     return Vector3D(vector.x* coeff,
    124                     vector.y* coeff,
    125                     vector.z* coeff);
    126 }
    127 
    128 Vector3D operator * (float coeff,const Vector3D &vector) {
    129     return Vector3D(vector.x* coeff,
    130                     vector.y* coeff,
    131                     vector.z* coeff);
    132 }
    133 
    134 Vector3D operator/ (const Vector3D &vector, float coeff) {
    135     if(coeff!=0) {
    136         return Vector3D(vector.x/ coeff,
    137                         vector.y/ coeff,
    138                         vector.z/ coeff);
    139     } else {
    140         printf("Vector3D: err divinding by 0\n");
    141         return Vector3D(0,0,0);
    142     }
    143 }
    144 
    145 void Vector3D::RotateX(float value) {
    146     float y_tmp;
    147     y_tmp=y*cosf(value)-z*sinf(value);
    148     z=y*sinf(value)+z*cosf(value);
    149     y=y_tmp;
    150 }
    151 
    152 void Vector3D::RotateXDeg(float value) {
    153     RotateX(Euler::ToRadian(value));
    154 }
    155 
    156 void Vector3D::RotateY(float value) {
    157     float x_tmp;
    158     x_tmp=x*cosf(value)+z*sinf(value);
    159     z=-x*sinf(value)+z*cosf(value);
    160     x=x_tmp;
    161 }
    162 
    163 void Vector3D::RotateYDeg(float value) {
    164     RotateY(Euler::ToRadian(value));
    165 }
    166 
    167 void Vector3D::RotateZ(float value) {
    168     float x_tmp;
    169     x_tmp=x*cosf(value)-y*sinf(value);
    170     y=x*sinf(value)+y*cosf(value);
    171     x=x_tmp;
    172 }
    173 
    174 void Vector3D::RotateZDeg(float value) {
    175     RotateZ(Euler::ToRadian(value));
    176 }
    177 
    178 void Vector3D::Rotate(const RotationMatrix &matrix) {
    179     float a[3]= {0,0,0};
    180     float b[3]= {x,y,z};
    181 
    182     for(int i=0; i<3; i++) {
    183         for(int j=0; j<3; j++) {
    184             a[i]+=matrix.m[i][j]*b[j];
    185         }
    186     }
    187 
    188     x=a[0];
    189     y=a[1];
    190     z=a[2];
     169  }
     170
     171  x = a[0];
     172  y = a[1];
     173  z = a[2];
    191174}
    192175
    193176void Vector3D::Rotate(const Quaternion &quaternion) {
    194     RotationMatrix matrix;
    195     quaternion.ToRotationMatrix(matrix);
    196     Rotate(matrix);
     177  RotationMatrix matrix;
     178  quaternion.ToRotationMatrix(matrix);
     179  Rotate(matrix);
    197180}
    198181
    199182void Vector3D::To2Dxy(Vector2D &vector) const {
    200     vector.x=x;
    201     vector.y=y;
     183  vector.x = x;
     184  vector.y = y;
    202185}
    203186
    204187Vector2D Vector3D::To2Dxy(void) const {
    205     Vector2D vect;
    206     To2Dxy(vect);
    207     return vect;
    208 }
    209 
    210 float Vector3D::GetNorm(void) const {
    211     return sqrt(x*x+y*y+z*z);
    212 }
     188  Vector2D vect;
     189  To2Dxy(vect);
     190  return vect;
     191}
     192
     193float Vector3D::GetNorm(void) const { return sqrt(x * x + y * y + z * z); }
    213194
    214195void Vector3D::Normalize(void) {
    215     float n=GetNorm();
    216     if(n!=0) {
    217         x=x/n;
    218         y=y/n;
    219         z=z/n;
    220     }
    221 }
    222 
    223 void Vector3D::Saturate(const Vector3D &min,const Vector3D &max) {
    224     if(x<min.x) x=min.x;
    225     if(x>max.x) x=max.x;
    226 
    227     if(y<min.y) y=min.y;
    228     if(y>max.y) y=max.y;
    229 
    230     if(z<min.z) z=min.z;
    231     if(z>max.z) z=max.z;
    232 }
    233 
    234 void Vector3D::Saturate(float min,float max) {
    235     Saturate(Vector3D(min,min,min),Vector3D(max,max,max));
     196  float n = GetNorm();
     197  if (n != 0) {
     198    x = x / n;
     199    y = y / n;
     200    z = z / n;
     201  }
     202}
     203
     204void Vector3D::Saturate(const Vector3D &min, const Vector3D &max) {
     205  if (x < min.x)
     206    x = min.x;
     207  if (x > max.x)
     208    x = max.x;
     209
     210  if (y < min.y)
     211    y = min.y;
     212  if (y > max.y)
     213    y = max.y;
     214
     215  if (z < min.z)
     216    z = min.z;
     217  if (z > max.z)
     218    z = max.z;
     219}
     220
     221void Vector3D::Saturate(float min, float max) {
     222  Saturate(Vector3D(min, min, min), Vector3D(max, max, max));
    236223}
    237224
    238225void Vector3D::Saturate(const Vector3D &value) {
    239     float x=fabs(value.x);
    240     float y=fabs(value.y);
    241     float z=fabs(value.z);
    242     Saturate(Vector3D(-x,-y,-z),Vector3D(x,y,z));
     226  float x = fabs(value.x);
     227  float y = fabs(value.y);
     228  float z = fabs(value.z);
     229  Saturate(Vector3D(-x, -y, -z), Vector3D(x, y, z));
    243230}
    244231
    245232void Vector3D::Saturate(float value) {
    246     float sat=fabs(value);
    247     Saturate(Vector3D(-sat,-sat,-sat),Vector3D(sat,sat,sat));
     233  float sat = fabs(value);
     234  Saturate(Vector3D(-sat, -sat, -sat), Vector3D(sat, sat, sat));
    248235}
    249236

  • trunk/lib/FlairCore/src/Vector3D.h

    r2 r15  
    1616#include <stddef.h>
    1717
    18 namespace flair { namespace core {
    19     class Vector2D;
    20     class RotationMatrix;
    21     class Quaternion;
    22 
    23     /*! \class Vector3D
    24     *
    25     * \brief Class defining a 3D vector
    26     */
    27     class Vector3D {
    28         public:
    29             /*!
    30             * \brief Constructor
    31             *
    32             * Construct a Vector3D using specified values.
    33             *
    34             * \param x
    35             * \param y
    36             * \param z
    37             */
    38             Vector3D(float x=0,float y=0,float z=0);
    39 
    40             /*!
    41             * \brief Destructor
    42             *
    43             */
    44             ~Vector3D();
    45 
    46             /*!
    47             * \brief x
    48             */
    49             float x;
    50 
    51             /*!
    52             * \brief y
    53             */
    54             float y;
    55 
    56             /*!
    57             * \brief z
    58             */
    59             float z;
    60 
    61             /*!
    62             * \brief x axis rotation
    63             *
    64             * \param value rotation value in radians
    65             */
    66             void RotateX(float value);
    67 
    68             /*!
    69             * \brief x axis rotation
    70             *
    71             * \param value rotation value in degrees
    72             */
    73             void RotateXDeg(float value);
    74 
    75             /*!
    76             * \brief y axis rotation
    77             *
    78             * \param value rotation value in radians
    79             */
    80             void RotateY(float value);
    81 
    82             /*!
    83             * \brief y axis rotation
    84             *
    85             * \param value rotation value in degrees
    86             */
    87             void RotateYDeg(float value);
    88 
    89             /*!
    90             * \brief z axis rotation
    91             *
    92             * \param value rotation value in radians
    93             */
    94             void RotateZ(float value);
    95 
    96             /*!
    97             * \brief z axis rotation
    98             *
    99             * \param value rotation value in degrees
    100             */
    101             void RotateZDeg(float value);
    102 
    103             /*!
    104             * \brief rotation
    105             *
    106             * \param matrix rotation matrix
    107             */
    108             void Rotate(const RotationMatrix &matrix);
    109 
    110             /*!
    111             * \brief rotation
    112             *
    113             * Compute a rotation from a quaternion. This method uses a rotation matrix
    114             * internaly.
    115             *
    116             * \param quaternion quaternion
    117             */
    118             void Rotate(const Quaternion &quaternion);
    119 
    120             /*!
    121             * \brief Convert to a Vector2D
    122             *
    123             * Uses x and y coordinates.
    124             *
    125             * \param vector destination
    126             */
    127             void To2Dxy(Vector2D &vector) const;
    128 
    129             /*!
    130             * \brief Convert to a Vector2D
    131             *
    132             * Uses x and y coordinates.
    133             *
    134             * \return destination
    135             */
    136             Vector2D To2Dxy(void) const;
    137 
    138             /*!
    139             * \brief Norm
    140             *
    141             * \return value
    142             */
    143             float GetNorm(void) const;
    144 
    145             /*!
    146             * \brief Normalize
    147             */
    148             void Normalize(void);
    149 
    150             /*!
    151             * \brief Saturate
    152             *
    153             * Saturate between min and max
    154             *
    155             * \param min minimum value
    156             * \param max maximum value
    157             */
    158             void Saturate(const Vector3D &min,const Vector3D &max);
    159 
    160             /*!
    161             * \brief Saturate
    162             *
    163             * Saturate between min and max
    164             *
    165             * \param min minimum Vector3D(min,min,min) value
    166             * \param max maximum Vector3D(max,max,max) value
    167             */
    168             void Saturate(float min,float max);
    169 
    170             /*!
    171             * \brief Saturate
    172             *
    173             * Saturate between -abs(value) and abs(value)
    174             *
    175             * \param value saturation Vector3D value
    176             */
    177             void Saturate(const Vector3D &value);
    178 
    179             /*!
    180             * \brief Saturate
    181             *
    182             * Saturate between -abs(Vector3D(value,value,value)) and abs(Vector3D(value,value,value))
    183             *
    184             * \param value saturation Vector3D(value,value,value)
    185             */
    186             void Saturate(float value);
    187 
    188             float &operator[](size_t idx);
    189             const float &operator[](size_t idx) const;
    190             Vector3D &operator=(const Vector3D& vector);
    191             Vector3D &operator+=(const Vector3D& vector);
    192             Vector3D &operator-=(const Vector3D& vector);
    193 
    194         private:
    195 
    196     };
    197 
    198     /*! Add
    199     *
    200     * \brief Add
    201     *
    202     * \param vectorA vector
    203     * \param vectorB vector
    204     *
    205     * \return vectorA+vectorB
    206     */
    207     Vector3D operator + (const Vector3D &vectorA,const Vector3D &vectorB);
    208 
    209     /*! Substract
    210     *
    211     * \brief Substract
    212     *
    213     * \param vectorA vector
    214     * \param vectorB vector
    215     *
    216     * \return vectorA-vectorB
    217     */
    218     Vector3D operator - (const Vector3D &vectorA,const Vector3D &vectorB);
    219 
    220     /*! Minus
    221     *
    222     * \brief Minus
    223     *
    224     * \param vector vector
    225     *
    226     * \return -vector
    227     */
    228     Vector3D operator-(const Vector3D &vector);
    229 
    230     /*! Divid
    231     *
    232     * \brief Divid
    233     *
    234     * \param vector vector
    235     * \param coeff coefficent
    236     *
    237     * \return vector/coefficient
    238     */
    239     Vector3D operator / (const Vector3D &vector, float coeff);
    240 
    241     /*! Hadamard product
    242     *
    243     * \brief Hadamard product
    244     *
    245     * \param vectorA vector
    246     * \param vectorBA vector
    247     *
    248     * \return Hadamard product
    249     */
    250     Vector3D operator * (const Vector3D &vectorA, const Vector3D &vectorB);
    251 
    252     /*! Multiply
    253     *
    254     * \brief Multiply
    255     *
    256     * \param vector vector
    257     * \param coeff coefficent
    258     *
    259     * \return coefficient*vector
    260     */
    261     Vector3D operator * (const Vector3D &vector, float coeff);
    262 
    263     /*! Multiply
    264     *
    265     * \brief Multiply
    266     *
    267     * \param coeff coefficent
    268     * \param vector vector
    269     *
    270     * \return coefficient*vector
    271     */
    272     Vector3D operator * (float coeff, const Vector3D &vector);
    273 
    274     /*! Cross product
    275     *
    276     * \brief Cross product
    277     *
    278     * \param vectorA first vector
    279     * \param vectorB second vector
    280     *
    281     * \return cross product
    282     */
    283     Vector3D CrossProduct(const Vector3D &vectorA, const Vector3D &vectorB);
    284 
    285     /*! Dot product
    286     *
    287     * \brief Dot product
    288     *
    289     * \param vectorA first vector
    290     * \param vectorB second vector
    291     *
    292     * \return dot product
    293     */
    294     float DotProduct(const Vector3D &vectorA, const Vector3D &vectorB);
     18namespace flair {
     19namespace core {
     20class Vector2D;
     21class RotationMatrix;
     22class Quaternion;
     23
     24/*! \class Vector3D
     25*
     26* \brief Class defining a 3D vector
     27*/
     28class Vector3D {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a Vector3D using specified values.
     34  *
     35  * \param x
     36  * \param y
     37  * \param z
     38  */
     39  Vector3D(float x = 0, float y = 0, float z = 0);
     40
     41  /*!
     42  * \brief Destructor
     43  *
     44  */
     45  ~Vector3D();
     46
     47  /*!
     48  * \brief x
     49  */
     50  float x;
     51
     52  /*!
     53  * \brief y
     54  */
     55  float y;
     56
     57  /*!
     58  * \brief z
     59  */
     60  float z;
     61
     62  /*!
     63  * \brief x axis rotation
     64  *
     65  * \param value rotation value in radians
     66  */
     67  void RotateX(float value);
     68
     69  /*!
     70  * \brief x axis rotation
     71  *
     72  * \param value rotation value in degrees
     73  */
     74  void RotateXDeg(float value);
     75
     76  /*!
     77  * \brief y axis rotation
     78  *
     79  * \param value rotation value in radians
     80  */
     81  void RotateY(float value);
     82
     83  /*!
     84  * \brief y axis rotation
     85  *
     86  * \param value rotation value in degrees
     87  */
     88  void RotateYDeg(float value);
     89
     90  /*!
     91  * \brief z axis rotation
     92  *
     93  * \param value rotation value in radians
     94  */
     95  void RotateZ(float value);
     96
     97  /*!
     98  * \brief z axis rotation
     99  *
     100  * \param value rotation value in degrees
     101  */
     102  void RotateZDeg(float value);
     103
     104  /*!
     105  * \brief rotation
     106  *
     107  * \param matrix rotation matrix
     108  */
     109  void Rotate(const RotationMatrix &matrix);
     110
     111  /*!
     112  * \brief rotation
     113  *
     114  * Compute a rotation from a quaternion. This method uses a rotation matrix
     115  * internaly.
     116  *
     117  * \param quaternion quaternion
     118  */
     119  void Rotate(const Quaternion &quaternion);
     120
     121  /*!
     122  * \brief Convert to a Vector2D
     123  *
     124  * Uses x and y coordinates.
     125  *
     126  * \param vector destination
     127  */
     128  void To2Dxy(Vector2D &vector) const;
     129
     130  /*!
     131  * \brief Convert to a Vector2D
     132  *
     133  * Uses x and y coordinates.
     134  *
     135  * \return destination
     136  */
     137  Vector2D To2Dxy(void) const;
     138
     139  /*!
     140  * \brief Norm
     141  *
     142  * \return value
     143  */
     144  float GetNorm(void) const;
     145
     146  /*!
     147  * \brief Normalize
     148  */
     149  void Normalize(void);
     150
     151  /*!
     152  * \brief Saturate
     153  *
     154  * Saturate between min and max
     155  *
     156  * \param min minimum value
     157  * \param max maximum value
     158  */
     159  void Saturate(const Vector3D &min, const Vector3D &max);
     160
     161  /*!
     162  * \brief Saturate
     163  *
     164  * Saturate between min and max
     165  *
     166  * \param min minimum Vector3D(min,min,min) value
     167  * \param max maximum Vector3D(max,max,max) value
     168  */
     169  void Saturate(float min, float max);
     170
     171  /*!
     172  * \brief Saturate
     173  *
     174  * Saturate between -abs(value) and abs(value)
     175  *
     176  * \param value saturation Vector3D value
     177  */
     178  void Saturate(const Vector3D &value);
     179
     180  /*!
     181  * \brief Saturate
     182  *
     183  * Saturate between -abs(Vector3D(value,value,value)) and
     184  *abs(Vector3D(value,value,value))
     185  *
     186  * \param value saturation Vector3D(value,value,value)
     187  */
     188  void Saturate(float value);
     189
     190  float &operator[](size_t idx);
     191  const float &operator[](size_t idx) const;
     192  Vector3D &operator=(const Vector3D &vector);
     193  Vector3D &operator+=(const Vector3D &vector);
     194  Vector3D &operator-=(const Vector3D &vector);
     195
     196private:
     197};
     198
     199/*! Add
     200*
     201* \brief Add
     202*
     203* \param vectorA vector
     204* \param vectorB vector
     205*
     206* \return vectorA+vectorB
     207*/
     208Vector3D operator+(const Vector3D &vectorA, const Vector3D &vectorB);
     209
     210/*! Substract
     211*
     212* \brief Substract
     213*
     214* \param vectorA vector
     215* \param vectorB vector
     216*
     217* \return vectorA-vectorB
     218*/
     219Vector3D operator-(const Vector3D &vectorA, const Vector3D &vectorB);
     220
     221/*! Minus
     222*
     223* \brief Minus
     224*
     225* \param vector vector
     226*
     227* \return -vector
     228*/
     229Vector3D operator-(const Vector3D &vector);
     230
     231/*! Divid
     232*
     233* \brief Divid
     234*
     235* \param vector vector
     236* \param coeff coefficent
     237*
     238* \return vector/coefficient
     239*/
     240Vector3D operator/(const Vector3D &vector, float coeff);
     241
     242/*! Hadamard product
     243*
     244* \brief Hadamard product
     245*
     246* \param vectorA vector
     247* \param vectorBA vector
     248*
     249* \return Hadamard product
     250*/
     251Vector3D operator*(const Vector3D &vectorA, const Vector3D &vectorB);
     252
     253/*! Multiply
     254*
     255* \brief Multiply
     256*
     257* \param vector vector
     258* \param coeff coefficent
     259*
     260* \return coefficient*vector
     261*/
     262Vector3D operator*(const Vector3D &vector, float coeff);
     263
     264/*! Multiply
     265*
     266* \brief Multiply
     267*
     268* \param coeff coefficent
     269* \param vector vector
     270*
     271* \return coefficient*vector
     272*/
     273Vector3D operator*(float coeff, const Vector3D &vector);
     274
     275/*! Cross product
     276*
     277* \brief Cross product
     278*
     279* \param vectorA first vector
     280* \param vectorB second vector
     281*
     282* \return cross product
     283*/
     284Vector3D CrossProduct(const Vector3D &vectorA, const Vector3D &vectorB);
     285
     286/*! Dot product
     287*
     288* \brief Dot product
     289*
     290* \param vectorA first vector
     291* \param vectorB second vector
     292*
     293* \return dot product
     294*/
     295float DotProduct(const Vector3D &vectorA, const Vector3D &vectorB);
    295296
    296297} // end namespace core

  • trunk/lib/FlairCore/src/Vector3DSpinBox.cpp

    r2 r15  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    Class displaying 3 QDoubleSpinBox for x,y,z on the ground station
     13//  purpose:    Class displaying 3 QDoubleSpinBox for x,y,z on the ground
     14//  station
    1415//
    1516//
    … …  
    2021using std::string;
    2122
    22 namespace flair { namespace gui {
     23namespace flair {
     24namespace gui {
    2325
    24 Vector3DSpinBox::Vector3DSpinBox(const LayoutPosition* position,string name,double min,double max,double step,int decimals,core::Vector3D default_value):Box(position,name,"Vector3DSpinBox") {
    25     //update value from xml file
    26     default_value.Saturate(min,max);
    27     box_value=default_value;
     26Vector3DSpinBox::Vector3DSpinBox(const LayoutPosition *position, string name,
     27                                 double min, double max, double step,
     28                                 int decimals, core::Vector3D default_value)
     29    : Box(position, name, "Vector3DSpinBox") {
     30  // update value from xml file
     31  default_value.Saturate(min, max);
     32  box_value = default_value;
    2833
    29     SetVolatileXmlProp("min",min);
    30     SetVolatileXmlProp("max",max);
    31     SetVolatileXmlProp("step",step);
    32     SetVolatileXmlProp("decimals",decimals);
    33     GetPersistentXmlProp("value_x",box_value.x);
    34     SetPersistentXmlProp("value_x",box_value.x);
    35     GetPersistentXmlProp("value_y",box_value.y);
    36     SetPersistentXmlProp("value_y",box_value.y);
    37     GetPersistentXmlProp("value_z",box_value.z);
    38     SetPersistentXmlProp("value_z",box_value.z);
     34  SetVolatileXmlProp("min", min);
     35  SetVolatileXmlProp("max", max);
     36  SetVolatileXmlProp("step", step);
     37  SetVolatileXmlProp("decimals", decimals);
     38  GetPersistentXmlProp("value_x", box_value.x);
     39  SetPersistentXmlProp("value_x", box_value.x);
     40  GetPersistentXmlProp("value_y", box_value.y);
     41  SetPersistentXmlProp("value_y", box_value.y);
     42  GetPersistentXmlProp("value_z", box_value.z);
     43  SetPersistentXmlProp("value_z", box_value.z);
    3944
    40     SendXml();
     45  SendXml();
    4146}
    4247
    43 Vector3DSpinBox::~Vector3DSpinBox() {
    44 
    45 }
     48Vector3DSpinBox::~Vector3DSpinBox() {}
    4649/*
    4750Vector3DSpinBox::operator core::Vector3D() const {
    … …  
    5053*/
    5154core::Vector3D Vector3DSpinBox::Value(void) const {
    52     core::Vector3D tmp;
     55  core::Vector3D tmp;
    5356
    54     GetMutex();
    55     tmp=box_value;
    56     ReleaseMutex();
     57  GetMutex();
     58  tmp = box_value;
     59  ReleaseMutex();
    5760
    58     return tmp;
     61  return tmp;
    5962}
    6063
    6164void Vector3DSpinBox::XmlEvent(void) {
    62     bool changed=false;
    63     GetMutex();
    64     if(GetPersistentXmlProp("value_x",box_value.x)) changed=true;
    65     if(GetPersistentXmlProp("value_y",box_value.y)) changed=true;
    66     if(GetPersistentXmlProp("value_z",box_value.z)) changed=true;
    67     if(changed) SetValueChanged();
    68     ReleaseMutex();
     65  bool changed = false;
     66  GetMutex();
     67  if (GetPersistentXmlProp("value_x", box_value.x))
     68    changed = true;
     69  if (GetPersistentXmlProp("value_y", box_value.y))
     70    changed = true;
     71  if (GetPersistentXmlProp("value_z", box_value.z))
     72    changed = true;
     73  if (changed)
     74    SetValueChanged();
     75  ReleaseMutex();
    6976}
    7077

  • trunk/lib/FlairCore/src/Vector3DSpinBox.h

    r2 r15  
    1717#include <Vector3D.h>
    1818
    19 namespace flair { namespace gui {
    20     class Layout;
     19namespace flair {
     20namespace gui {
     21class Layout;
    2122
    22     /*! \class Vector3DSpinBox
    23     *
    24     * \brief Class displaying 3 QDoubleSpinBox for x,y,z on the ground station
    25     *
    26     */
    27     class Vector3DSpinBox: public Box {
    28         public:
    29             /*!
    30             * \brief Constructor
    31             *
    32             * Construct a Vector3DSpinBox at given position. \n
    33             * Each DoubleSpinBox is saturated to min and max values.
    34             *
    35             * \param position position to display the Vector3DSpinBox
    36             * \param name name
    37             * \param min minimum value
    38             * \param max maximum value
    39             * \param step step
    40             * \param decimals number of decimals
    41             * \param default_value default value if not in the xml config file
    42             */
    43             Vector3DSpinBox(const LayoutPosition* position,std::string name,double min,double max,double step,int decimals=2,core::Vector3D default_value=core::Vector3D(0,0,0));
     23/*! \class Vector3DSpinBox
     24*
     25* \brief Class displaying 3 QDoubleSpinBox for x,y,z on the ground station
     26*
     27*/
     28class Vector3DSpinBox : public Box {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a Vector3DSpinBox at given position. \n
     34  * Each DoubleSpinBox is saturated to min and max values.
     35  *
     36  * \param position position to display the Vector3DSpinBox
     37  * \param name name
     38  * \param min minimum value
     39  * \param max maximum value
     40  * \param step step
     41  * \param decimals number of decimals
     42  * \param default_value default value if not in the xml config file
     43  */
     44  Vector3DSpinBox(const LayoutPosition *position, std::string name, double min,
     45                  double max, double step, int decimals = 2,
     46                  core::Vector3D default_value = core::Vector3D(0, 0, 0));
    4447
    45             /*!
    46             * \brief Destructor
    47             *
    48             */
    49             ~Vector3DSpinBox();
     48  /*!
     49  * \brief Destructor
     50  *
     51  */
     52  ~Vector3DSpinBox();
    5053
    51             /*!
    52             * \brief Value
    53             *
    54             * \return value
    55             */
    56             core::Vector3D Value(void) const;
    57             //operator core::Vector3D() const;
    58         private:
    59             /*!
    60             * \brief XmlEvent from ground station
    61             *
    62             * Reimplemented from Widget.
    63             *
    64             */
    65             void XmlEvent(void);
     54  /*!
     55  * \brief Value
     56  *
     57  * \return value
     58  */
     59  core::Vector3D Value(void) const;
     60  // operator core::Vector3D() const;
     61private:
     62  /*!
     63  * \brief XmlEvent from ground station
     64  *
     65  * Reimplemented from Widget.
     66  *
     67  */
     68  void XmlEvent(void);
    6669
    67             core::Vector3D box_value;
    68     };
     70  core::Vector3D box_value;
     71};
    6972
    7073} // end namespace gui

  • trunk/lib/FlairCore/src/Vector3Ddata.cpp

    r2 r15  
    2525using std::string;
    2626
    27 namespace flair { namespace core {
     27namespace flair {
     28namespace core {
    2829
    29    /*! \class Vector3DdataElement
    30     */
    31     class Vector3DdataElement: public IODataElement {
    32     public:
    33         Vector3DdataElement(const Vector3Ddata *data,string name,uint32_t elem):IODataElement(data,name) {
    34             this->data=data;
    35             this->elem=elem;
    36             size=4;
    37         }
     30/*! \class Vector3DdataElement
     31 */
     32class Vector3DdataElement : public IODataElement {
     33public:
     34  Vector3DdataElement(const Vector3Ddata *data, string name, uint32_t elem)
     35      : IODataElement(data, name) {
     36    this->data = data;
     37    this->elem = elem;
     38    size = 4;
     39  }
    3840
    39         ~Vector3DdataElement() {
    40         }
     41  ~Vector3DdataElement() {}
    4142
    42         void CopyData(char* dst) const {
    43             float value;
    44             data->GetMutex();
    45             switch(elem) {
    46                 case X:
    47                     value=data->x;
    48                     break;
    49                 case Y:
    50                     value=data->y;
    51                     break;
    52                 case Z:
    53                     value=data->z;
    54                     break;
    55                 default:
    56                     data->Err("type not handled\n");
    57                     value=0;
    58                     break;
    59             }
    60             data->ReleaseMutex();
     43  void CopyData(char *dst) const {
     44    float value;
     45    data->GetMutex();
     46    switch (elem) {
     47    case X:
     48      value = data->x;
     49      break;
     50    case Y:
     51      value = data->y;
     52      break;
     53    case Z:
     54      value = data->z;
     55      break;
     56    default:
     57      data->Err("type not handled\n");
     58      value = 0;
     59      break;
     60    }
     61    data->ReleaseMutex();
    6162
    62             memcpy(dst,&value,sizeof(float));
    63         }
     63    memcpy(dst, &value, sizeof(float));
     64  }
    6465
    65         ScalarType const &GetDataType() const {
    66             return dataType;
    67         }
     66  ScalarType const &GetDataType() const { return dataType; }
    6867
    69     private:
    70         const Vector3Ddata *data;
    71         uint32_t elem;
    72         FloatType dataType;
    73     };
     68private:
     69  const Vector3Ddata *data;
     70  uint32_t elem;
     71  FloatType dataType;
     72};
    7473
    75 Vector3Ddata::Vector3Ddata(const Object* parent, string name,float x,float y,float z,uint32_t n): io_data(parent,name,n), Vector3D(x,y,z) {
     74Vector3Ddata::Vector3Ddata(const Object *parent, string name, float x, float y,
     75                           float z, uint32_t n)
     76    : io_data(parent, name, n), Vector3D(x, y, z) {}
    7677
     78Vector3Ddata::~Vector3Ddata() {}
     79
     80void Vector3Ddata::CopyDatas(char *dst) const {
     81  GetMutex();
     82  memcpy(dst, &x, sizeof(float));
     83  memcpy(dst + sizeof(float), &y, sizeof(float));
     84  memcpy(dst + 2 * sizeof(float), &z, sizeof(float));
     85  ReleaseMutex();
    7786}
    7887
    79 Vector3Ddata::~Vector3Ddata() {
    80 
     88IODataElement *Vector3Ddata::XElement(void) const {
     89  return new Vector3DdataElement(this, "x", X);
    8190}
    8291
    83 void Vector3Ddata::CopyDatas(char* dst) const {
    84     GetMutex();
    85     memcpy(dst,&x,sizeof(float));
    86     memcpy(dst+sizeof(float),&y,sizeof(float));
    87     memcpy(dst+2*sizeof(float),&z,sizeof(float));
    88     ReleaseMutex();
     92IODataElement *Vector3Ddata::YElement(void) const {
     93  return new Vector3DdataElement(this, "y", Y);
    8994}
    9095
    91 IODataElement* Vector3Ddata::XElement(void) const {
    92     return new Vector3DdataElement(this,"x",X);
    93 }
    94 
    95 IODataElement* Vector3Ddata::YElement(void) const {
    96     return new Vector3DdataElement(this,"y",Y);
    97 }
    98 
    99 IODataElement* Vector3Ddata::ZElement(void) const {
    100     return new Vector3DdataElement(this,"z",Z);
     96IODataElement *Vector3Ddata::ZElement(void) const {
     97  return new Vector3DdataElement(this, "z", Z);
    10198}
    10299

  • trunk/lib/FlairCore/src/Vector3Ddata.h

    r2 r15  
    1818#include <Vector3D.h>
    1919
    20 namespace flair
    21 {
    22 namespace core
    23 {
     20namespace flair {
     21namespace core {
    2422
    25     /*! \class Vector3Ddata
    26     *
    27     * \brief Class defining a 3D vector and a io_data
    28     * User must manually use the io_data's Mutex to access to Vector3D values.
    29     */
    30     class Vector3Ddata: public io_data, public Vector3D
    31     {
    32         public:
    33             /*!
    34             * \brief Constructor
    35             *
    36             * Construct a Vector3D using specified values.
    37             *
    38             * \param x
    39             * \param y
    40             * \param z
    41             */
    42             Vector3Ddata(const Object* parent, std::string name,float x=0,float y=0,float z=0,uint32_t n=1);
     23/*! \class Vector3Ddata
     24*
     25* \brief Class defining a 3D vector and a io_data
     26* User must manually use the io_data's Mutex to access to Vector3D values.
     27*/
     28class Vector3Ddata : public io_data, public Vector3D {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a Vector3D using specified values.
     34  *
     35  * \param x
     36  * \param y
     37  * \param z
     38  */
     39  Vector3Ddata(const Object *parent, std::string name, float x = 0, float y = 0,
     40               float z = 0, uint32_t n = 1);
    4341
    44             /*!
    45             * \brief Destructor
    46             *
    47             */
    48             ~Vector3Ddata();
     42  /*!
     43  * \brief Destructor
     44  *
     45  */
     46  ~Vector3Ddata();
    4947
    50             /*!
    51             * \brief X Element
    52             *
    53             * Get a vectorer to x element. This pointer can be used for plotting.
    54             *
    55             * \return pointer to the element
    56             */
    57             IODataElement* XElement(void) const;
     48  /*!
     49  * \brief X Element
     50  *
     51  * Get a vectorer to x element. This pointer can be used for plotting.
     52  *
     53  * \return pointer to the element
     54  */
     55  IODataElement *XElement(void) const;
    5856
    59             /*!
    60             * \brief Y Element
    61             *
    62             * Get a pointer to y element. This pointer can be used for plotting.
    63             *
    64             * \return pointer to the element
    65             */
    66             IODataElement* YElement(void) const;
     57  /*!
     58  * \brief Y Element
     59  *
     60  * Get a pointer to y element. This pointer can be used for plotting.
     61  *
     62  * \return pointer to the element
     63  */
     64  IODataElement *YElement(void) const;
    6765
    68             /*!
    69             * \brief Z Element
    70             *
    71             * Get a pointer to z element. This pointer can be used for plotting.
    72             *
    73             * \return pointer to the element
    74             */
    75             IODataElement* ZElement(void) const;
     66  /*!
     67  * \brief Z Element
     68  *
     69  * Get a pointer to z element. This pointer can be used for plotting.
     70  *
     71  * \return pointer to the element
     72  */
     73  IODataElement *ZElement(void) const;
    7674
    77         private:
    78             /*!
    79             * \brief Copy datas
    80             *
    81             * Reimplemented from io_data. \n
    82             * See io_data::CopyDatas.
    83             *
    84             * \param dst destination buffer
    85             */
    86             void CopyDatas(char* dst) const;
    87     };
     75private:
     76  /*!
     77  * \brief Copy datas
     78  *
     79  * Reimplemented from io_data. \n
     80  * See io_data::CopyDatas.
     81  *
     82  * \param dst destination buffer
     83  */
     84  void CopyDatas(char *dst) const;
     85};
    8886
    8987} // end namespace core

  • trunk/lib/FlairCore/src/Watchdog.cpp

    r2 r15  
    2020namespace core {
    2121
    22 Watchdog::Watchdog(const Object* parent,std::function<void()> _expired,Time _timer):Thread(parent,"watchdog",0),expired(_expired),timer(_timer){
    23 }
     22Watchdog::Watchdog(const Object *parent, std::function<void()> _expired,
     23                   Time _timer)
     24    : Thread(parent, "watchdog", 0), expired(_expired), timer(_timer) {}
    2425
    2526Watchdog::~Watchdog() {
    26     SafeStop();
    27     Join();
     27  SafeStop();
     28  Join();
    2829}
    2930
    3031void Watchdog::Touch() {
    31     if (IsSuspended()) Resume();
     32  if (IsSuspended())
     33    Resume();
    3234}
    3335
    3436void Watchdog::SetTimer(Time _Timer) {
    35     timer=_Timer; Touch();
     37  timer = _Timer;
     38  Touch();
    3639}
    3740
    3841void Watchdog::Run() {
    39     while (!ToBeStopped()) {
    40         Time current=GetTime();
    41         Time date=current+timer;
    42         //Printf("watchdog goes to sleep at %llu, scheduled to wake up at %llu\n",current,date);
    43         if (!SuspendUntil(date)) {
    44             expired();
    45         }
     42  while (!ToBeStopped()) {
     43    Time current = GetTime();
     44    Time date = current + timer;
     45    // Printf("watchdog goes to sleep at %llu, scheduled to wake up at
     46    // %llu\n",current,date);
     47    if (!SuspendUntil(date)) {
     48      expired();
    4649    }
     50  }
    4751};
    4852

  • trunk/lib/FlairCore/src/Watchdog.h

    r2 r15  
    2020namespace core {
    2121
    22     /*! \class Watchdog
    23     *
    24     * \brief Watchdog class
    25     *
    26     * Calls a given function if not touched within a specified period of time
    27     *
    28     */
    29     class Watchdog:public Thread {
    30     public:
    31         Watchdog(const Object* parent,std::function<void()> _expired,Time _timer);
    32         ~Watchdog();
     22/*! \class Watchdog
     23*
     24* \brief Watchdog class
     25*
     26* Calls a given function if not touched within a specified period of time
     27*
     28*/
     29class Watchdog : public Thread {
     30public:
     31  Watchdog(const Object *parent, std::function<void()> _expired, Time _timer);
     32  ~Watchdog();
    3333
    34         //reset the timer
    35         void Touch();
    36         void SetTimer(Time _Timer);
    37     private:
    38         void Run();
    39         std::function<void()> expired;
    40         Time timer;
    41     };
     34  // reset the timer
     35  void Touch();
     36  void SetTimer(Time _Timer);
     37
     38private:
     39  void Run();
     40  std::function<void()> expired;
     41  Time timer;
     42};
    4243
    4344} // end namespace core

  • trunk/lib/FlairCore/src/Widget.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair
    24 {
    25 namespace gui
    26 {
     23namespace flair {
     24namespace gui {
    2725
    2826using namespace core;
    2927
    30 Widget::Widget(const Widget* parent,string name,string type): Object(parent,name,type)
    31 {
    32     pimpl_=new Widget_impl(this,parent,name,type);
    33 }
    34 
    35 Widget::~Widget()
    36 {
    37     delete pimpl_;
    38 }
    39 
    40 template <>
    41 bool Widget::GetPersistentXmlProp(std::string prop,double &value)
    42 {
    43     xmlChar *result=NULL;
    44     result=xmlGetProp(pimpl_->file_node,(xmlChar*)prop.c_str());
    45 
    46     if(result!=NULL) {
    47         value=xmlXPathCastStringToNumber(result);
    48         xmlFree(result);
    49         return true;
    50     } else {
    51         return false;
    52     }
    53 }
    54 
    55 template <>
    56 bool Widget::GetPersistentXmlProp(std::string prop,float &value) {
    57     double tmp;
    58     if(GetPersistentXmlProp<double>(prop,tmp)) {
    59         value=tmp;
    60         return true;
    61     } else {
    62         return false;
    63     }
    64 }
    65 
    66 template <>
    67 bool Widget::GetPersistentXmlProp(std::string prop,bool &value) {
    68     double tmp;
    69     if(GetPersistentXmlProp<double>(prop,tmp)) {
    70         value=tmp;
    71         return true;
    72     } else {
    73         return false;
    74     }
    75 }
    76 
    77 template <>
    78 bool Widget::GetPersistentXmlProp(std::string prop,int32_t &value) {
    79     double tmp;
    80     if(GetPersistentXmlProp<double>(prop,tmp)) {
    81         value=tmp;
    82         return true;
    83     } else {
    84         return false;
    85     }
    86 }
    87 
    88 template <>
    89 bool Widget::GetPersistentXmlProp(std::string prop,uint16_t &value) {
    90     double tmp;
    91     if(GetPersistentXmlProp<double>(prop,tmp)) {
    92         value=tmp;
    93         return true;
    94     } else {
    95         return false;
    96     }
    97 }
    98 
    99 template <>
    100 void Widget::SetVolatileXmlProp(string prop,string value,xmlNodePtr node)
    101 {
    102     if(node==NULL) node=pimpl_->send_node;
    103     xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)value.c_str());
    104 }
    105 
    106 template <>
    107 void Widget::SetVolatileXmlProp(string prop,char* value,xmlNodePtr node)
    108 {
    109     if(node==NULL) node=pimpl_->send_node;
    110     xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)value);
    111 }
    112 
    113 template <>
    114 void Widget::SetVolatileXmlProp(string prop,double value,xmlNodePtr node)
    115 {
    116     xmlChar* xmlChar_value=xmlXPathCastNumberToString(value);
    117 
    118     if(node==NULL) node=pimpl_->send_node;
    119     xmlSetProp(node,(xmlChar*)prop.c_str(),xmlChar_value);
    120     xmlFree(xmlChar_value);
    121 }
    122 
    123 template <>
    124 void Widget::SetVolatileXmlProp(string prop,float value,xmlNodePtr node)
    125 {
    126     SetVolatileXmlProp<double>(prop,value,node);
    127 }
    128 
    129 template <>
    130 void Widget::SetVolatileXmlProp(string prop,int32_t value,xmlNodePtr node)
    131 {
    132     SetVolatileXmlProp<double>(prop,value,node);
    133 }
    134 
    135 template <>
    136 void Widget::SetVolatileXmlProp(string prop,uint32_t value,xmlNodePtr node)
    137 {
    138     SetVolatileXmlProp<double>(prop,value,node);
    139 }
    140 
    141 template <>
    142 void Widget::SetVolatileXmlProp(string prop,int16_t value,xmlNodePtr node)
    143 {
    144     SetVolatileXmlProp<double>(prop,value,node);
    145 }
    146 
    147 template <>
    148 void Widget::SetVolatileXmlProp(string prop,uint16_t value,xmlNodePtr node)
    149 {
    150     SetVolatileXmlProp<double>(prop,value,node);
    151 }
    152 
    153 template <>
    154 void Widget::SetVolatileXmlProp(string prop,int8_t value,xmlNodePtr node)
    155 {
    156     SetVolatileXmlProp<double>(prop,value,node);
    157 }
    158 
    159 template <>
    160 void Widget::SetVolatileXmlProp(string prop,uint8_t value,xmlNodePtr node)
    161 {
    162     SetVolatileXmlProp<double>(prop,value,node);
    163 }
    164 
    165 template <>
    166 void Widget::SetVolatileXmlProp(string prop,bool value,xmlNodePtr node)
    167 {
    168     SetVolatileXmlProp<double>(prop,value,node);
    169 }
    170 
    171 template <>
    172 void Widget::SetVolatileXmlProp(string prop,xmlChar* value,xmlNodePtr node) {
    173     if(node==NULL) node=pimpl_->send_node;
    174     xmlSetProp(node,(xmlChar*)prop.c_str(),value);
    175 }
    176 
    177 template <>
    178 void Widget::SetVolatileXmlProp(string prop,DataType const &dataType,xmlNodePtr node) {
    179     if(node==NULL) node=pimpl_->send_node;
    180     xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)dataType.GetDescription().c_str());
    181 /*
    182     switch(dataType)
    183     {
    184         case io_data::Float:
    185             xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)"float");
    186             break;
    187         case io_data::Int8_t:
    188             xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)"int8_t");
    189             break;
    190         case io_data::Int16_t:
    191             xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)"int16_t");
    192             break;
    193     }
    194     */
    195 }
    196 
    197 template <>
    198 void Widget::SetPersistentXmlProp(std::string prop,double value) {
    199     SetVolatileXmlProp(prop,value);
    200     SetVolatileXmlProp(prop,value,pimpl_->file_node);
    201 }
    202 
    203 template <>
    204 void Widget::SetPersistentXmlProp(std::string prop,float value)
    205 {
    206     SetVolatileXmlProp(prop,value);
    207     SetVolatileXmlProp(prop,value,pimpl_->file_node);
    208 }
    209 
    210 template <>
    211 void Widget::SetPersistentXmlProp(std::string prop,int32_t value)
    212 {
    213     SetVolatileXmlProp(prop,value);
    214     SetVolatileXmlProp(prop,value,pimpl_->file_node);
    215 }
    216 
    217 template <>
    218 void Widget::SetPersistentXmlProp(std::string prop,uint16_t value)
    219 {
    220     SetVolatileXmlProp(prop,value);
    221     SetVolatileXmlProp(prop,value,pimpl_->file_node);
    222 }
    223 
    224 template <>
    225 void Widget::SetPersistentXmlProp(std::string prop,bool value)
    226 {
    227     SetVolatileXmlProp(prop,value);
    228     SetVolatileXmlProp(prop,value,pimpl_->file_node);
    229 }
    230 
    231 void Widget::SendXml(void)
    232 {
    233     pimpl_->SendXml();
    234 }
    235 
    236 void Widget::setEnabled(bool status)
    237 {
    238     pimpl_->setEnabled(status);
    239 }
    240 
    241 bool Widget::isEnabled(void) const
    242 {
    243     return pimpl_->isenabled;
    244 
    245 }
     28Widget::Widget(const Widget *parent, string name, string type)
     29    : Object(parent, name, type) {
     30  pimpl_ = new Widget_impl(this, parent, name, type);
     31}
     32
     33Widget::~Widget() { delete pimpl_; }
     34
     35template <> bool Widget::GetPersistentXmlProp(std::string prop, double &value) {
     36  xmlChar *result = NULL;
     37  result = xmlGetProp(pimpl_->file_node, (xmlChar *)prop.c_str());
     38
     39  if (result != NULL) {
     40    value = xmlXPathCastStringToNumber(result);
     41    xmlFree(result);
     42    return true;
     43  } else {
     44    return false;
     45  }
     46}
     47
     48template <> bool Widget::GetPersistentXmlProp(std::string prop, float &value) {
     49  double tmp;
     50  if (GetPersistentXmlProp<double>(prop, tmp)) {
     51    value = tmp;
     52    return true;
     53  } else {
     54    return false;
     55  }
     56}
     57
     58template <> bool Widget::GetPersistentXmlProp(std::string prop, bool &value) {
     59  double tmp;
     60  if (GetPersistentXmlProp<double>(prop, tmp)) {
     61    value = tmp;
     62    return true;
     63  } else {
     64    return false;
     65  }
     66}
     67
     68template <>
     69bool Widget::GetPersistentXmlProp(std::string prop, int32_t &value) {
     70  double tmp;
     71  if (GetPersistentXmlProp<double>(prop, tmp)) {
     72    value = tmp;
     73    return true;
     74  } else {
     75    return false;
     76  }
     77}
     78
     79template <>
     80bool Widget::GetPersistentXmlProp(std::string prop, uint16_t &value) {
     81  double tmp;
     82  if (GetPersistentXmlProp<double>(prop, tmp)) {
     83    value = tmp;
     84    return true;
     85  } else {
     86    return false;
     87  }
     88}
     89
     90template <>
     91void Widget::SetVolatileXmlProp(string prop, string value, xmlNodePtr node) {
     92  if (node == NULL)
     93    node = pimpl_->send_node;
     94  xmlSetProp(node, (xmlChar *)prop.c_str(), (xmlChar *)value.c_str());
     95}
     96
     97template <>
     98void Widget::SetVolatileXmlProp(string prop, char *value, xmlNodePtr node) {
     99  if (node == NULL)
     100    node = pimpl_->send_node;
     101  xmlSetProp(node, (xmlChar *)prop.c_str(), (xmlChar *)value);
     102}
     103
     104template <>
     105void Widget::SetVolatileXmlProp(string prop, double value, xmlNodePtr node) {
     106  xmlChar *xmlChar_value = xmlXPathCastNumberToString(value);
     107
     108  if (node == NULL)
     109    node = pimpl_->send_node;
     110  xmlSetProp(node, (xmlChar *)prop.c_str(), xmlChar_value);
     111  xmlFree(xmlChar_value);
     112}
     113
     114template <>
     115void Widget::SetVolatileXmlProp(string prop, float value, xmlNodePtr node) {
     116  SetVolatileXmlProp<double>(prop, value, node);
     117}
     118
     119template <>
     120void Widget::SetVolatileXmlProp(string prop, int32_t value, xmlNodePtr node) {
     121  SetVolatileXmlProp<double>(prop, value, node);
     122}
     123
     124template <>
     125void Widget::SetVolatileXmlProp(string prop, uint32_t value, xmlNodePtr node) {
     126  SetVolatileXmlProp<double>(prop, value, node);
     127}
     128
     129template <>
     130void Widget::SetVolatileXmlProp(string prop, int16_t value, xmlNodePtr node) {
     131  SetVolatileXmlProp<double>(prop, value, node);
     132}
     133
     134template <>
     135void Widget::SetVolatileXmlProp(string prop, uint16_t value, xmlNodePtr node) {
     136  SetVolatileXmlProp<double>(prop, value, node);
     137}
     138
     139template <>
     140void Widget::SetVolatileXmlProp(string prop, int8_t value, xmlNodePtr node) {
     141  SetVolatileXmlProp<double>(prop, value, node);
     142}
     143
     144template <>
     145void Widget::SetVolatileXmlProp(string prop, uint8_t value, xmlNodePtr node) {
     146  SetVolatileXmlProp<double>(prop, value, node);
     147}
     148
     149template <>
     150void Widget::SetVolatileXmlProp(string prop, bool value, xmlNodePtr node) {
     151  SetVolatileXmlProp<double>(prop, value, node);
     152}
     153
     154template <>
     155void Widget::SetVolatileXmlProp(string prop, xmlChar *value, xmlNodePtr node) {
     156  if (node == NULL)
     157    node = pimpl_->send_node;
     158  xmlSetProp(node, (xmlChar *)prop.c_str(), value);
     159}
     160
     161template <>
     162void Widget::SetVolatileXmlProp(string prop, DataType const &dataType,
     163                                xmlNodePtr node) {
     164  if (node == NULL)
     165    node = pimpl_->send_node;
     166  xmlSetProp(node, (xmlChar *)prop.c_str(),
     167             (xmlChar *)dataType.GetDescription().c_str());
     168  /*
     169      switch(dataType)
     170      {
     171          case io_data::Float:
     172              xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)"float");
     173              break;
     174          case io_data::Int8_t:
     175              xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)"int8_t");
     176              break;
     177          case io_data::Int16_t:
     178              xmlSetProp(node,(xmlChar*)prop.c_str(),(xmlChar*)"int16_t");
     179              break;
     180      }
     181      */
     182}
     183
     184template <> void Widget::SetPersistentXmlProp(std::string prop, double value) {
     185  SetVolatileXmlProp(prop, value);
     186  SetVolatileXmlProp(prop, value, pimpl_->file_node);
     187}
     188
     189template <> void Widget::SetPersistentXmlProp(std::string prop, float value) {
     190  SetVolatileXmlProp(prop, value);
     191  SetVolatileXmlProp(prop, value, pimpl_->file_node);
     192}
     193
     194template <> void Widget::SetPersistentXmlProp(std::string prop, int32_t value) {
     195  SetVolatileXmlProp(prop, value);
     196  SetVolatileXmlProp(prop, value, pimpl_->file_node);
     197}
     198
     199template <>
     200void Widget::SetPersistentXmlProp(std::string prop, uint16_t value) {
     201  SetVolatileXmlProp(prop, value);
     202  SetVolatileXmlProp(prop, value, pimpl_->file_node);
     203}
     204
     205template <> void Widget::SetPersistentXmlProp(std::string prop, bool value) {
     206  SetVolatileXmlProp(prop, value);
     207  SetVolatileXmlProp(prop, value, pimpl_->file_node);
     208}
     209
     210void Widget::SendXml(void) { pimpl_->SendXml(); }
     211
     212void Widget::setEnabled(bool status) { pimpl_->setEnabled(status); }
     213
     214bool Widget::isEnabled(void) const { return pimpl_->isenabled; }
    246215
    247216} // end namespace gui

  • trunk/lib/FlairCore/src/Widget.h

    r2 r15  
    2121class FrameworkManager_impl;
    2222
    23 namespace flair
    24 {
    25 namespace gui
    26 {
     23namespace flair {
     24namespace gui {
    2725
    28     /*! \class Widget
    29     *
    30     * \brief Abstract class for all Framework's widget classes
    31     *
    32     * A widget is an object to display on the ground station. \n
    33     * Communication with ground station is done through xml files; properties of theses files
    34     * are modified through appropriate method. \n
    35     * A xml file is used for default values of the Widget, if it has been specified in the
    36     *  constructor of the FrameworkManager.
    37     */
    38     class Widget: public core::Object
    39     {
    40         friend class core::FrameworkManager;
    41         friend class ::Widget_impl;
    42         friend class ::FrameworkManager_impl;
     26/*! \class Widget
     27*
     28* \brief Abstract class for all Framework's widget classes
     29*
     30* A widget is an object to display on the ground station. \n
     31* Communication with ground station is done through xml files; properties of
     32*theses files
     33* are modified through appropriate method. \n
     34* A xml file is used for default values of the Widget, if it has been specified
     35*in the
     36*  constructor of the FrameworkManager.
     37*/
     38class Widget : public core::Object {
     39  friend class core::FrameworkManager;
     40  friend class ::Widget_impl;
     41  friend class ::FrameworkManager_impl;
    4342
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Construct a Widget, the xml file specified to the FrameworkManager's
    49             * constructor is sued for default values. \n
    50             * Two Widget with same parent must have different names. If a brother Widget already
    51             * has the same name, the name of the new one will be automatically changed. \n
    52             * Type must agree with predifined (hard coded) types
    53             * in ground station code.
    54             *
    55             * \param parent parent
    56             * \param name name
    57             * \param type type
    58             */
    59             Widget(const Widget* parent,std::string name,std::string type);
     43public:
     44  /*!
     45  * \brief Constructor
     46  *
     47  * Construct a Widget, the xml file specified to the FrameworkManager's
     48  * constructor is sued for default values. \n
     49  * Two Widget with same parent must have different names. If a brother Widget
     50  *already
     51  * has the same name, the name of the new one will be automatically changed. \n
     52  * Type must agree with predifined (hard coded) types
     53  * in ground station code.
     54  *
     55  * \param parent parent
     56  * \param name name
     57  * \param type type
     58  */
     59  Widget(const Widget *parent, std::string name, std::string type);
    6060
    61             /*!
    62             * \brief Destructor
    63             *
    64             */
    65             virtual ~Widget();
     61  /*!
     62  * \brief Destructor
     63  *
     64  */
     65  virtual ~Widget();
    6666
    67             /*!
    68             * \brief Set enabled
    69             *
    70             * Enable or disable the Widget on the ground station. \n
    71             * A disabled widget is greyed out on the ground station
    72             * and in unmodifiable.
    73             *
    74             * \param status
    75             */
    76             void setEnabled(bool status);
     67  /*!
     68  * \brief Set enabled
     69  *
     70  * Enable or disable the Widget on the ground station. \n
     71  * A disabled widget is greyed out on the ground station
     72  * and in unmodifiable.
     73  *
     74  * \param status
     75  */
     76  void setEnabled(bool status);
    7777
    78             /*!
    79             * \brief Is enabled?
    80             *
    81             * \return true if widget is enabled
    82             */
    83             bool isEnabled(void) const;
     78  /*!
     79  * \brief Is enabled?
     80  *
     81  * \return true if widget is enabled
     82  */
     83  bool isEnabled(void) const;
    8484
    85         protected:
    86             /*!
    87             * \brief Set a persistent xml property
    88             *
    89             * The property will be saved in the configuration xml and also used to configure the ground station.
    90             *
    91             * \param prop property to set and save
    92             * \param value value to set and save
    93             */
    94             template <typename T>
    95             void SetPersistentXmlProp(std::string prop,T value);
     85protected:
     86  /*!
     87  * \brief Set a persistent xml property
     88  *
     89  * The property will be saved in the configuration xml and also used to
     90  *configure the ground station.
     91  *
     92  * \param prop property to set and save
     93  * \param value value to set and save
     94  */
     95  template <typename T> void SetPersistentXmlProp(std::string prop, T value);
    9696
    97             /*!
    98             * \brief Get a persistent xml property
    99             *
    100             * Get the property from the xml file. If no corresponding property is found in the xml, value remains unchanged. \n
    101             * Thus value can be initialized with a default value before calling this method.
    102             *
    103             * \param prop property to get
    104             * \param value value to store the result
    105             * \return true if value was changed
    106             */
    107             template <typename T>
    108             bool GetPersistentXmlProp(std::string prop,T &value);
     97  /*!
     98  * \brief Get a persistent xml property
     99  *
     100  * Get the property from the xml file. If no corresponding property is found in
     101  *the xml, value remains unchanged. \n
     102  * Thus value can be initialized with a default value before calling this
     103  *method.
     104  *
     105  * \param prop property to get
     106  * \param value value to store the result
     107  * \return true if value was changed
     108  */
     109  template <typename T> bool GetPersistentXmlProp(std::string prop, T &value);
    109110
    110             /*!
    111             * \brief Set a volatile xml property
    112             *
    113             * This property should be used to configure the ground station (one time init). \n
    114             * The property will be destroyed after calling SendXml() as it should no be used anymore.
    115             *
    116             * \param prop property to set
    117             * \param value value to set
    118             * \param node if sepcified, node to set; otherwise use the node of the Widget
    119             */
    120             template <typename T>
    121             void SetVolatileXmlProp(std::string prop,T value,xmlNodePtr node=NULL);
     111  /*!
     112  * \brief Set a volatile xml property
     113  *
     114  * This property should be used to configure the ground station (one time
     115  *init). \n
     116  * The property will be destroyed after calling SendXml() as it should no be
     117  *used anymore.
     118  *
     119  * \param prop property to set
     120  * \param value value to set
     121  * \param node if sepcified, node to set; otherwise use the node of the Widget
     122  */
     123  template <typename T>
     124  void SetVolatileXmlProp(std::string prop, T value, xmlNodePtr node = NULL);
    122125
    123             /*!
    124             * \brief Send xml
    125             *
    126             * Send Widget's xml to ground station. \n
    127             * New changes will be taken into account by ground station. \n
    128             * All volatile properties will be erased after calling ths method, as they should not be used anymore.
    129             */
    130             void SendXml(void);
     126  /*!
     127  * \brief Send xml
     128  *
     129  * Send Widget's xml to ground station. \n
     130  * New changes will be taken into account by ground station. \n
     131  * All volatile properties will be erased after calling ths method, as they
     132  *should not be used anymore.
     133  */
     134  void SendXml(void);
    131135
    132             /*!
    133             * \brief Xml event
    134             *
    135             * This method must be reimplemented to handle a xml event. \n
    136             * It is automatically called when something changed from
    137             * ground station. \n
    138             */
    139             virtual void XmlEvent(void){};
     136  /*!
     137  * \brief Xml event
     138  *
     139  * This method must be reimplemented to handle a xml event. \n
     140  * It is automatically called when something changed from
     141  * ground station. \n
     142  */
     143  virtual void XmlEvent(void){};
    140144
    141          private:
    142             class Widget_impl* pimpl_;
    143     };
     145private:
     146  class Widget_impl *pimpl_;
     147};
    144148
    145149} // end namespace gui

  • trunk/lib/FlairCore/src/Widget_impl.cpp

    r2 r15  
    3434using namespace flair::gui;
    3535
    36 namespace{
     36namespace {
    3737#ifdef __XENO__
    38     RT_HEAP xml_heap;
    39 
    40     void xml2_free(void *mem)
    41     {
    42         //Printf("free %x\n",mem);
    43         if(mem==NULL) return;
    44         int status=rt_heap_free(&xml_heap,mem);
    45 
    46         if(status!=0)
    47         {
    48             printf("libxml2: rt_heap_free error (%s)\n",strerror(-status));
    49         }
    50     }
    51 
    52     void *xml2_alloc(size_t sz)
    53     {
    54         void *ptr;
    55         //printf("alloc %i\n",sz);
    56         int status=rt_heap_alloc(&xml_heap,sz,TM_NONBLOCK ,&ptr);
    57         if(status!=0)
    58         {
    59             printf("libxml2: rt_heap_alloc error (%s)\n",strerror(-status));
    60         }
    61         //Printf("alloc %x %i\n",ptr,sz);
    62 
    63         return ptr;
    64 
    65     }
    66 
    67     void *xml2_realloc(void *emem,size_t sz)
    68     {
    69         //Printf("realloc %x %i -> %i\n",emem,sz,sz);
    70         void *mem_re;
    71 
    72 
    73         if (emem == NULL)
    74         {
    75             return xml2_alloc(sz);
    76         }
    77         else if (sz == 0)
    78         {
    79             xml2_free(emem);
    80         }
    81 
    82         mem_re = xml2_alloc(sz);
    83 
    84         memcpy(mem_re, emem, sz);
    85 
    86         xml2_free(emem);
    87 
    88         return mem_re;
    89     }
    90 
    91     char *xml2_strdup(const char * str)
    92     {
    93        // printf("strdup %s\n",str);
    94         char *s = (char*)xml2_alloc(strlen(str) + 1);
    95         if (s == NULL)
    96            return NULL;
    97 
    98         strcpy(s, str);
    99         return s;
    100 
    101     }
     38RT_HEAP xml_heap;
     39
     40void xml2_free(void *mem) {
     41  // Printf("free %x\n",mem);
     42  if (mem == NULL)
     43    return;
     44  int status = rt_heap_free(&xml_heap, mem);
     45
     46  if (status != 0) {
     47    printf("libxml2: rt_heap_free error (%s)\n", strerror(-status));
     48  }
     49}
     50
     51void *xml2_alloc(size_t sz) {
     52  void *ptr;
     53  // printf("alloc %i\n",sz);
     54  int status = rt_heap_alloc(&xml_heap, sz, TM_NONBLOCK, &ptr);
     55  if (status != 0) {
     56    printf("libxml2: rt_heap_alloc error (%s)\n", strerror(-status));
     57  }
     58  // Printf("alloc %x %i\n",ptr,sz);
     59
     60  return ptr;
     61}
     62
     63void *xml2_realloc(void *emem, size_t sz) {
     64  // Printf("realloc %x %i -> %i\n",emem,sz,sz);
     65  void *mem_re;
     66
     67  if (emem == NULL) {
     68    return xml2_alloc(sz);
     69  } else if (sz == 0) {
     70    xml2_free(emem);
     71  }
     72
     73  mem_re = xml2_alloc(sz);
     74
     75  memcpy(mem_re, emem, sz);
     76
     77  xml2_free(emem);
     78
     79  return mem_re;
     80}
     81
     82char *xml2_strdup(const char *str) {
     83  // printf("strdup %s\n",str);
     84  char *s = (char *)xml2_alloc(strlen(str) + 1);
     85  if (s == NULL)
     86    return NULL;
     87
     88  strcpy(s, str);
     89  return s;
     90}
    10291#endif //__XENO__
    10392}
    10493
    105 Widget_impl::Widget_impl(Widget* self,const Widget* parent,string name,string type)
    106 {
    107     //Printf("Widget %s\n",name.c_str());
    108     isenabled=true;
    109     file_node=NULL;
    110     this->self=self;
    111 
    112     //n'est execute qu'une fois lorsqu'on construit le FrameworkManager
    113     if(parent==NULL)
    114     {
     94Widget_impl::Widget_impl(Widget *self, const Widget *parent, string name,
     95                         string type) {
     96  // Printf("Widget %s\n",name.c_str());
     97  isenabled = true;
     98  file_node = NULL;
     99  this->self = self;
     100
     101  // n'est execute qu'une fois lorsqu'on construit le FrameworkManager
     102  if (parent == NULL) {
    115103#ifdef __XENO__
    116         string tmp_name;
    117         tmp_name=name + "-xml";
    118         int status=rt_heap_create(&xml_heap,tmp_name.c_str(),XML_HEAP,H_FIFO);
    119         if(status!=0)
    120         {
    121             self->Err("rt_heap_create error (%s)\n",strerror(-status));
    122         }
    123 
    124         xmlMemSetup(xml2_free,xml2_alloc, xml2_realloc,xml2_strdup);
     104    string tmp_name;
     105    tmp_name = name + "-xml";
     106    int status = rt_heap_create(&xml_heap, tmp_name.c_str(), XML_HEAP, H_FIFO);
     107    if (status != 0) {
     108      self->Err("rt_heap_create error (%s)\n", strerror(-status));
     109    }
     110
     111    xmlMemSetup(xml2_free, xml2_alloc, xml2_realloc, xml2_strdup);
    125112#endif //__XENO__
    126     }
    127 
    128     //xml init
    129     if(parent==NULL)
    130     {
    131         //create local doc
    132         send_doc = xmlNewDoc((xmlChar*)"1.0");
    133         send_node = xmlNewNode(NULL, (xmlChar*)type.c_str());
    134         xmlDocSetRootElement(send_doc, send_node);
    135         xmlSetProp(send_node,(xmlChar*)"name",(xmlChar*)self->ObjectName().c_str());
    136     }
    137     else
    138     {
    139         parent->pimpl_->AddChild(self);
    140 
    141         //get node corresponding to this widget in the xml file
    142         if(parent->pimpl_->file_node!=NULL)
    143         {
    144             xmlChar* search=xmlCharStrdup(type.c_str());
    145             file_node=GetNodeByProp(parent->pimpl_->file_node->xmlChildrenNode,search,(xmlChar*)"name",(xmlChar*)name.c_str());
    146             xmlFree(search);
    147         }
    148         else
    149         {
    150             self->Err("parent->file_node is NULL\n");
    151         }
    152 
    153         if(file_node==NULL)
    154         {
    155             self->Warn("%s, no match found in xml file\n",type.c_str());
    156             xmlNode *node;
    157             node=xmlNewNode(NULL, (xmlChar*)type.c_str());
    158             xmlSetProp(node,(xmlChar*)"name",(xmlChar*)name.c_str());
    159             file_node=xmlAddChild(parent->pimpl_->file_node,node);
    160             //((Widget*)getFrameworkManager())->pimpl_->PrintXml();
    161         }
    162 
    163         send_doc=parent->pimpl_->CopyDoc();
    164 
    165         //on recupere le dernier node
    166         xmlNodePtr node=xmlDocGetRootElement(send_doc);
    167         while(node->children!=NULL) node=node->children;
    168 
    169         //on ajoute le node du widget
    170         send_node = xmlNewNode(NULL, (xmlChar*)type.c_str());
    171         xmlSetProp(send_node,(xmlChar*)"name",(xmlChar*)name.c_str());
    172         xmlAddChild(node,send_node);
    173     }
    174 
    175     //Printf("Widget ok %s\n",name.c_str());
    176 }
    177 
    178 Widget_impl::~Widget_impl()
    179 {
    180     //Printf("destruction widget %s\n",self->ObjectName().c_str());
    181 
    182     if(self->Parent()!=NULL) ((Widget*)(self->Parent()))->pimpl_->RemoveChild(self);
    183 
    184     //on efface les widgets enfants avant d'envoyer son propre delete au sol
    185     //dans le delete child on modifie le child du parent, donc on se refere toujours au premier
    186     while(childs.size()!=0)
    187     {
    188         //Printf("child %i %s\n",childs.size(),childs.front()->ObjectName().c_str());
    189         if(childs.front()!=NULL) delete childs.front();
    190     }
    191     childs.clear();
    192 
    193     self->SetVolatileXmlProp("Delete",true);
    194 
    195     if(self->Parent()!=NULL) SendXml();
    196 
    197     xmlFreeDoc(send_doc);
    198 
    199     if(self->Parent()==NULL)
    200     {
    201         xmlCleanupParser();
     113  }
     114
     115  // xml init
     116  if (parent == NULL) {
     117    // create local doc
     118    send_doc = xmlNewDoc((xmlChar *)"1.0");
     119    send_node = xmlNewNode(NULL, (xmlChar *)type.c_str());
     120    xmlDocSetRootElement(send_doc, send_node);
     121    xmlSetProp(send_node, (xmlChar *)"name",
     122               (xmlChar *)self->ObjectName().c_str());
     123  } else {
     124    parent->pimpl_->AddChild(self);
     125
     126    // get node corresponding to this widget in the xml file
     127    if (parent->pimpl_->file_node != NULL) {
     128      xmlChar *search = xmlCharStrdup(type.c_str());
     129      file_node =
     130          GetNodeByProp(parent->pimpl_->file_node->xmlChildrenNode, search,
     131                        (xmlChar *)"name", (xmlChar *)name.c_str());
     132      xmlFree(search);
     133    } else {
     134      self->Err("parent->file_node is NULL\n");
     135    }
     136
     137    if (file_node == NULL) {
     138      self->Warn("%s, no match found in xml file\n", type.c_str());
     139      xmlNode *node;
     140      node = xmlNewNode(NULL, (xmlChar *)type.c_str());
     141      xmlSetProp(node, (xmlChar *)"name", (xmlChar *)name.c_str());
     142      file_node = xmlAddChild(parent->pimpl_->file_node, node);
     143      //((Widget*)getFrameworkManager())->pimpl_->PrintXml();
     144    }
     145
     146    send_doc = parent->pimpl_->CopyDoc();
     147
     148    // on recupere le dernier node
     149    xmlNodePtr node = xmlDocGetRootElement(send_doc);
     150    while (node->children != NULL)
     151      node = node->children;
     152
     153    // on ajoute le node du widget
     154    send_node = xmlNewNode(NULL, (xmlChar *)type.c_str());
     155    xmlSetProp(send_node, (xmlChar *)"name", (xmlChar *)name.c_str());
     156    xmlAddChild(node, send_node);
     157  }
     158
     159  // Printf("Widget ok %s\n",name.c_str());
     160}
     161
     162Widget_impl::~Widget_impl() {
     163  // Printf("destruction widget %s\n",self->ObjectName().c_str());
     164
     165  if (self->Parent() != NULL)
     166    ((Widget *)(self->Parent()))->pimpl_->RemoveChild(self);
     167
     168  // on efface les widgets enfants avant d'envoyer son propre delete au sol
     169  // dans le delete child on modifie le child du parent, donc on se refere
     170  // toujours au premier
     171  while (childs.size() != 0) {
     172    // Printf("child %i
     173    // %s\n",childs.size(),childs.front()->ObjectName().c_str());
     174    if (childs.front() != NULL)
     175      delete childs.front();
     176  }
     177  childs.clear();
     178
     179  self->SetVolatileXmlProp("Delete", true);
     180
     181  if (self->Parent() != NULL)
     182    SendXml();
     183
     184  xmlFreeDoc(send_doc);
     185
     186  if (self->Parent() == NULL) {
     187    xmlCleanupParser();
    202188#ifdef __XENO__
    203         int status;
    204         RT_HEAP_INFO info;
    205         status=rt_heap_inquire(&xml_heap,&info);
    206         if(status!=0)
    207         {
    208             self->Err("rt_heap_inquire error (%s)\n",strerror(-status));
    209         }
    210         if(info.usedmem!=0) self->Err("fuite memoire xml heap (%ld)\n",info.usedmem);
    211     //Printf("fin heap xml\n");
    212         status=rt_heap_delete(&xml_heap);
    213         if(status!=0)
    214         {
    215             self->Err("rt_heap_delete error (%s)\n",strerror(-status));
    216         }
     189    int status;
     190    RT_HEAP_INFO info;
     191    status = rt_heap_inquire(&xml_heap, &info);
     192    if (status != 0) {
     193      self->Err("rt_heap_inquire error (%s)\n", strerror(-status));
     194    }
     195    if (info.usedmem != 0)
     196      self->Err("fuite memoire xml heap (%ld)\n", info.usedmem);
     197    // Printf("fin heap xml\n");
     198    status = rt_heap_delete(&xml_heap);
     199    if (status != 0) {
     200      self->Err("rt_heap_delete error (%s)\n", strerror(-status));
     201    }
    217202#endif
    218     }
    219     //Printf("destruction widget %s ok\n",self->ObjectName().c_str());
    220 }
    221 
    222 void Widget_impl::AddChild(const Widget* child)
    223 {
    224     childs.push_back((Widget*)child);
    225 }
    226 
    227 void Widget_impl::RemoveChild(const Widget* child)
    228 {
    229     for(vector<Widget*>::iterator it=childs.begin() ; it < childs.end(); it++ )
    230     {
    231         if(*it==child)
    232         {
    233             childs.erase(it);
    234             break;
    235         }
    236     }
    237 }
    238 
    239 xmlDocPtr Widget_impl::CopyDoc(void)
    240 {
    241     return xmlCopyDoc(send_doc,1);
    242 }
     203  }
     204  // Printf("destruction widget %s ok\n",self->ObjectName().c_str());
     205}
     206
     207void Widget_impl::AddChild(const Widget *child) {
     208  childs.push_back((Widget *)child);
     209}
     210
     211void Widget_impl::RemoveChild(const Widget *child) {
     212  for (vector<Widget *>::iterator it = childs.begin(); it < childs.end();
     213       it++) {
     214    if (*it == child) {
     215      childs.erase(it);
     216      break;
     217    }
     218  }
     219}
     220
     221xmlDocPtr Widget_impl::CopyDoc(void) { return xmlCopyDoc(send_doc, 1); }
    243222
    244223void Widget_impl::ClearXmlProps(void) {
    245     xmlUnlinkNode(send_node);
    246     xmlFreeNode(send_node);
    247 
    248     xmlNodePtr node;
    249     node=xmlDocGetRootElement(send_doc);
    250 
    251     if(node==NULL) {//il ne reste plus rien, on refait le rootelement
    252         send_node = xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE);
    253         xmlSetProp(send_node,(xmlChar*)"name",(xmlChar*)self->ObjectName().c_str());
    254         xmlDocSetRootElement(send_doc, send_node);
    255     } else {
    256         while(node->children!=NULL) node=node->children;
    257         send_node = xmlNewNode(NULL, (xmlChar*)self->ObjectType().c_str());
    258         xmlSetProp(send_node,(xmlChar*)"name",(xmlChar*)self->ObjectName().c_str());
    259         xmlAddChild(node,send_node);
    260     }
     224  xmlUnlinkNode(send_node);
     225  xmlFreeNode(send_node);
     226
     227  xmlNodePtr node;
     228  node = xmlDocGetRootElement(send_doc);
     229
     230  if (node == NULL) { // il ne reste plus rien, on refait le rootelement
     231    send_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE);
     232    xmlSetProp(send_node, (xmlChar *)"name",
     233               (xmlChar *)self->ObjectName().c_str());
     234    xmlDocSetRootElement(send_doc, send_node);
     235  } else {
     236    while (node->children != NULL)
     237      node = node->children;
     238    send_node = xmlNewNode(NULL, (xmlChar *)self->ObjectType().c_str());
     239    xmlSetProp(send_node, (xmlChar *)"name",
     240               (xmlChar *)self->ObjectName().c_str());
     241    xmlAddChild(node, send_node);
     242  }
    261243}
    262244
    263245void Widget_impl::printSendNode() {
    264246
     247  xmlChar *xmlbuff;
     248  int buffersize;
     249  xmlDocDumpFormatMemory(send_doc, &xmlbuff, &buffersize, 1);
     250  Printf("xml:\n%s\n", xmlbuff);
     251  xmlFree(xmlbuff);
     252}
     253
     254void Widget_impl::SendXml(void) {
     255  if (getUiCom() != NULL) {
    265256    xmlChar *xmlbuff;
    266257    int buffersize;
    267     xmlDocDumpFormatMemory(send_doc, &xmlbuff, &buffersize, 1);
    268     Printf("xml:\n%s\n",xmlbuff);
     258    xmlDocDumpFormatMemory(send_doc, &xmlbuff, &buffersize, 0);
     259    getUiCom()->Send((char *)xmlbuff, buffersize);
    269260    xmlFree(xmlbuff);
    270 }
    271 
    272 void Widget_impl::SendXml(void)
    273 {
    274     if(getUiCom()!=NULL) {
    275         xmlChar *xmlbuff;
    276         int buffersize;
    277         xmlDocDumpFormatMemory(send_doc, &xmlbuff, &buffersize, 0);
    278         getUiCom()->Send((char*)xmlbuff,buffersize);
    279         xmlFree(xmlbuff);
    280 
    281         ClearXmlProps();
    282     }
     261
     262    ClearXmlProps();
     263  }
    283264}
    284265
    285266void Widget_impl::setEnabled(bool status) {
    286     self->SetVolatileXmlProp("IsEnabled",status);
    287     SendXml();
    288 }
    289 
    290 
    291 void Widget_impl::ProcessXML(xmlNode *node)
    292 {
    293     //Printf("ProcessXML %s %s\n",xmlGetProp(send_node, (xmlChar*)"name"),send_node->name);
    294     xmlNode *cur_node = NULL;
    295 
    296     for(size_t i=0;i<childs.size();i++)
    297     {
    298         for (cur_node = node; cur_node; cur_node = cur_node->next)
    299         {
    300             if (cur_node->type == XML_ELEMENT_NODE)
    301             {
    302                 //Printf("recherche %s\n", xmlGetProp(cur_node, (xmlChar*)"name"));
    303                 xmlChar* name=NULL;
    304                 name=xmlGetProp(cur_node, (xmlChar*)"name");
    305                 if (!xmlStrcmp((xmlChar *)childs[i]->ObjectName().c_str(), name))
    306                 {
    307                     //Printf("correspond %s\n",childs[i]->ObjectName().c_str());
    308                     xmlChar* new_name=NULL;
    309                     new_name=xmlGetProp(cur_node, (xmlChar*)"new_name");
    310                     if(new_name!=NULL)
    311                     {
    312                         unsigned char* ren_name=new_name;//xmlGetProp(cur_node, (xmlChar*)"new_name");
    313                         self->Warn("%s existe deja, renommage en %s; possiblite de problemes!!\n",childs[i]->ObjectName().c_str(),new_name);
    314                         childs[i]->Object::pimpl_->name=((const char*)ren_name);
    315                         xmlSetProp(childs[i]->pimpl_->send_node,(xmlChar*)"name",xmlGetProp(cur_node, (xmlChar*)"new_name"));
    316                         xmlFree(new_name);
    317                         if(name!=NULL) xmlFree(name);
    318                         break;
    319                     }
    320 
    321                     if(name!=NULL) xmlFree(name);
    322 
    323                     //update file node
    324                     xmlAttr *cur_prop = NULL;
    325                     //xmlAttr *file_prop = NULL;
    326                     for (cur_prop = cur_node->properties; cur_prop; cur_prop = cur_prop->next)
    327                     {
    328                         //Printf("rcv prop %s %s\n",cur_prop->name,cur_prop->children->content);
    329                         xmlSetProp(childs[i]->pimpl_->file_node,cur_prop->name,cur_prop->children->content);
    330                     }
    331 
    332                     childs[i]->XmlEvent();
    333 
    334                     childs[i]->pimpl_->ProcessXML(cur_node->children);
    335                     break;
    336 
    337                 }
    338                 if(name!=NULL) xmlFree(name);
    339             }
     267  self->SetVolatileXmlProp("IsEnabled", status);
     268  SendXml();
     269}
     270
     271void Widget_impl::ProcessXML(xmlNode *node) {
     272  // Printf("ProcessXML %s %s\n",xmlGetProp(send_node,
     273  // (xmlChar*)"name"),send_node->name);
     274  xmlNode *cur_node = NULL;
     275
     276  for (size_t i = 0; i < childs.size(); i++) {
     277    for (cur_node = node; cur_node; cur_node = cur_node->next) {
     278      if (cur_node->type == XML_ELEMENT_NODE) {
     279        // Printf("recherche %s\n", xmlGetProp(cur_node, (xmlChar*)"name"));
     280        xmlChar *name = NULL;
     281        name = xmlGetProp(cur_node, (xmlChar *)"name");
     282        if (!xmlStrcmp((xmlChar *)childs[i]->ObjectName().c_str(), name)) {
     283          // Printf("correspond %s\n",childs[i]->ObjectName().c_str());
     284          xmlChar *new_name = NULL;
     285          new_name = xmlGetProp(cur_node, (xmlChar *)"new_name");
     286          if (new_name != NULL) {
     287            unsigned char *ren_name =
     288                new_name; // xmlGetProp(cur_node, (xmlChar*)"new_name");
     289            self->Warn(
     290                "%s existe deja, renommage en %s; possiblite de problemes!!\n",
     291                childs[i]->ObjectName().c_str(), new_name);
     292            childs[i]->Object::pimpl_->name = ((const char *)ren_name);
     293            xmlSetProp(childs[i]->pimpl_->send_node, (xmlChar *)"name",
     294                       xmlGetProp(cur_node, (xmlChar *)"new_name"));
     295            xmlFree(new_name);
     296            if (name != NULL)
     297              xmlFree(name);
     298            break;
     299          }
     300
     301          if (name != NULL)
     302            xmlFree(name);
     303
     304          // update file node
     305          xmlAttr *cur_prop = NULL;
     306          // xmlAttr *file_prop = NULL;
     307          for (cur_prop = cur_node->properties; cur_prop;
     308               cur_prop = cur_prop->next) {
     309            // Printf("rcv prop %s
     310            // %s\n",cur_prop->name,cur_prop->children->content);
     311            xmlSetProp(childs[i]->pimpl_->file_node, cur_prop->name,
     312                       cur_prop->children->content);
     313          }
     314
     315          childs[i]->XmlEvent();
     316
     317          childs[i]->pimpl_->ProcessXML(cur_node->children);
     318          break;
    340319        }
    341     }
    342 
    343     //printf("fin ProcessXML %s %s\n",xmlGetProp(send_node, (xmlChar*)"name"),send_node->name);
    344 }
    345 
    346 
    347 xmlNodePtr Widget_impl::GetNodeByProp(xmlNodePtr doc,xmlChar *type,xmlChar *prop,xmlChar *value)
    348 {
    349         //printf("cherche keyword: %s %s %s\n", type,prop,value);
    350     xmlNode *cur_node = NULL;
    351         for (cur_node = doc; cur_node; cur_node = cur_node->next)
    352     {
    353         if (cur_node->type == XML_ELEMENT_NODE)
    354         {
    355             //printf("node %s %s\n",xmlGetProp(cur_node, (xmlChar*)"name"),cur_node->name);
    356             xmlChar *test = NULL;
    357             test=xmlGetProp(cur_node, prop);
    358             if(!xmlStrcmp(test,value) && !xmlStrcmp(cur_node->name,type ))
    359             {
    360                 //printf("ok\n");
    361                 if(test!=NULL) xmlFree(test);
    362                 return cur_node;
    363             }
    364             if(test!=NULL) xmlFree(test);
    365         }
    366     }
    367 
    368     return NULL;
    369 }
     320        if (name != NULL)
     321          xmlFree(name);
     322      }
     323    }
     324  }
     325
     326  // printf("fin ProcessXML %s %s\n",xmlGetProp(send_node,
     327  // (xmlChar*)"name"),send_node->name);
     328}
     329
     330xmlNodePtr Widget_impl::GetNodeByProp(xmlNodePtr doc, xmlChar *type,
     331                                      xmlChar *prop, xmlChar *value) {
     332  // printf("cherche keyword: %s %s %s\n", type,prop,value);
     333  xmlNode *cur_node = NULL;
     334  for (cur_node = doc; cur_node; cur_node = cur_node->next) {
     335    if (cur_node->type == XML_ELEMENT_NODE) {
     336      // printf("node %s %s\n",xmlGetProp(cur_node,
     337      // (xmlChar*)"name"),cur_node->name);
     338      xmlChar *test = NULL;
     339      test = xmlGetProp(cur_node, prop);
     340      if (!xmlStrcmp(test, value) && !xmlStrcmp(cur_node->name, type)) {
     341        // printf("ok\n");
     342        if (test != NULL)
     343          xmlFree(test);
     344        return cur_node;
     345      }
     346      if (test != NULL)
     347        xmlFree(test);
     348    }
     349  }
     350
     351  return NULL;
     352}

  • trunk/lib/FlairCore/src/cvimage.cpp

    r2 r15  
    1919using std::string;
    2020
    21 namespace flair { namespace core {
     21namespace flair {
     22namespace core {
    2223
    23 cvimage::cvimage(const Object* parent,uint16_t width,uint16_t height,Type::Format format,string name,bool allocate_data,int n): io_data(parent,name,n),dataType(width,height,format) {
    24     this->allocate_data=allocate_data;
     24cvimage::cvimage(const Object *parent, uint16_t width, uint16_t height,
     25                 Type::Format format, string name, bool allocate_data, int n)
     26    : io_data(parent, name, n), dataType(width, height, format) {
     27  this->allocate_data = allocate_data;
    2528
    26     if(allocate_data) {
    27         switch(format) {
    28             case Type::Format::YUYV:
    29             case Type::Format::UYVY:
    30                 img=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,2);
    31                 break;
    32             case Type::Format::BGR:
    33                 img=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,3);
    34                 break;
    35             case Type::Format::GRAY:
    36                 img=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,1);
    37                 break;
    38             default:
    39                 Err("format no supported");
    40                 break;
    41         }
    42     } else {
    43         if(n>1) Err("number of samples!=1 not possible when not allocating data\n");
    44         n=1;
    45         switch(format) {
    46             case Type::Format::YUYV:
    47             case Type::Format::UYVY:
    48                 img=cvCreateImageHeader(cvSize(width,height),IPL_DEPTH_8U,2);
    49                 break;
    50             case Type::Format::BGR:
    51                 img=cvCreateImageHeader(cvSize(width,height),IPL_DEPTH_8U,3);
    52                 break;
    53             case Type::Format::GRAY:
    54                 img=cvCreateImageHeader(cvSize(width,height),IPL_DEPTH_8U,1);
    55                 break;
    56             default:
    57                 Err("format no supported");
    58                 break;
    59         }
     29  if (allocate_data) {
     30    switch (format) {
     31    case Type::Format::YUYV:
     32    case Type::Format::UYVY:
     33      img = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 2);
     34      break;
     35    case Type::Format::BGR:
     36      img = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
     37      break;
     38    case Type::Format::GRAY:
     39      img = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
     40      break;
     41    default:
     42      Err("format no supported");
     43      break;
    6044    }
     45  } else {
     46    if (n > 1)
     47      Err("number of samples!=1 not possible when not allocating data\n");
     48    n = 1;
     49    switch (format) {
     50    case Type::Format::YUYV:
     51    case Type::Format::UYVY:
     52      img = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 2);
     53      break;
     54    case Type::Format::BGR:
     55      img = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
     56      break;
     57    case Type::Format::GRAY:
     58      img = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 1);
     59      break;
     60    default:
     61      Err("format no supported");
     62      break;
     63    }
     64  }
    6165
    62     SetPtrToCircle((void**)&img);
     66  SetPtrToCircle((void **)&img);
    6367
    64     if(n>1) prev=new cvimage(this,width,height,format,name,n-1);
     68  if (n > 1)
     69    prev = new cvimage(this, width, height, format, name, n - 1);
    6570}
    6671
    6772cvimage::~cvimage() {
    68     //printf("destructeur cvimage\n");
     73  // printf("destructeur cvimage\n");
    6974
    70     cvReleaseImage(&img);
     75  cvReleaseImage(&img);
    7176}
    7277
    73 void cvimage::CopyDatas(char* dst) const {
    74     Warn("non implementé\n");
    75 }
     78void cvimage::CopyDatas(char *dst) const { Warn("non implementé\n"); }
    7679
    7780} // end namespace core

  • trunk/lib/FlairCore/src/cvimage.h

    r2 r15  
    1818#include <stdint.h>
    1919
    20 namespace flair
    21 {
    22 namespace core
    23 {
    24     /*! \class cvimage
    25     *
    26     * \brief Class defining an image of kind IplImage
    27     *
    28     * IplImage is an image struct defined in OpenCV.
    29     *
     20namespace flair {
     21namespace core {
     22/*! \class cvimage
     23*
     24* \brief Class defining an image of kind IplImage
     25*
     26* IplImage is an image struct defined in OpenCV.
     27*
     28*/
     29class cvimage : public io_data {
     30public:
     31  class Type : public DataType {
     32  public:
     33    /*!
     34    \enum Format_t
     35    \brief Picture formats
    3036    */
    31     class cvimage: public io_data {
    32     public:
    33         class Type: public DataType {
    34         public:
    35             /*!
    36             \enum Format_t
    37             \brief Picture formats
    38             */
    39             enum class Format {
    40                 YUYV,/*!< YUYV 16 bits */
    41                 UYVY,/*!< UYVY 16 bits */
    42                 BGR,/*!< BGR 24 bits */
    43                 GRAY,/*!< gray 8 bits */
    44             } ;
    45             Type(uint16_t _width, uint16_t _height, Format _format):width(_width),height(_height),format(_format) {}
     37    enum class Format {
     38      YUYV, /*!< YUYV 16 bits */
     39      UYVY, /*!< UYVY 16 bits */
     40      BGR,  /*!< BGR 24 bits */
     41      GRAY, /*!< gray 8 bits */
     42    };
     43    Type(uint16_t _width, uint16_t _height, Format _format)
     44        : width(_width), height(_height), format(_format) {}
    4645
    47             size_t GetSize() const {
    48                 size_t pixelSize;
    49                 switch(format) {
    50                 case Format::GRAY:
    51                     pixelSize=1;
    52                     break;
    53                 case Format::YUYV:
    54                 case Format::UYVY:
    55                     pixelSize=2;
    56                     break;
    57                 case Format::BGR:
    58                     pixelSize=3;
    59                     break;
    60                 default:
    61                     pixelSize=0; //TODO should throw an exception instead
    62                 }
    63                 return pixelSize*width*height;
    64             }
    65             std::string GetDescription() const {return "cv image";};
     46    size_t GetSize() const {
     47      size_t pixelSize;
     48      switch (format) {
     49      case Format::GRAY:
     50        pixelSize = 1;
     51        break;
     52      case Format::YUYV:
     53      case Format::UYVY:
     54        pixelSize = 2;
     55        break;
     56      case Format::BGR:
     57        pixelSize = 3;
     58        break;
     59      default:
     60        pixelSize = 0; // TODO should throw an exception instead
     61      }
     62      return pixelSize * width * height;
     63    }
     64    std::string GetDescription() const { return "cv image"; };
    6665
    67             Format GetFormat() const {return format;};
    68             uint16_t GetWidth() const {return width;};
    69             uint16_t GetHeight() const {return height;};
     66    Format GetFormat() const { return format; };
     67    uint16_t GetWidth() const { return width; };
     68    uint16_t GetHeight() const { return height; };
    7069
    71         private:
    72             uint16_t width;
    73             uint16_t height;
    74             Format format;
     70  private:
     71    uint16_t width;
     72    uint16_t height;
     73    Format format;
     74  };
    7575
    76         };
     76  /*!
     77  * \brief Constructor
     78  *
     79  * Construct an io_data representing an IplImage.
     80  *
     81  * \param parent parent
     82  * \param width image width
     83  * \param height image height
     84  * \param name name
     85  * \param allocate_data if true, IplImage image data is allocated; otherwise
     86  *img->imagedata must be changed
     87  * \param n number of samples
     88  */
     89  cvimage(const Object *parent, uint16_t width, uint16_t height,
     90          Type::Format format, std::string name = "", bool allocate_data = true,
     91          int n = 1);
    7792
    78         /*!
    79         * \brief Constructor
    80         *
    81         * Construct an io_data representing an IplImage.
    82         *
    83         * \param parent parent
    84         * \param width image width
    85         * \param height image height
    86         * \param name name
    87         * \param allocate_data if true, IplImage image data is allocated; otherwise img->imagedata must be changed
    88         * \param n number of samples
    89         */
    90         cvimage(const Object* parent,uint16_t width,uint16_t height,Type::Format format,std::string name="",bool allocate_data=true,int n=1);
     93  /*!
     94  * \brief Destructor
     95  *
     96  */
     97  ~cvimage();
    9198
    92         /*!
    93         * \brief Destructor
    94         *
    95         */
    96         ~cvimage();
     99  /*!
     100  * \brief IplImage
     101  *
     102  * \return IplImage
     103  */
     104  IplImage *img;
    97105
    98         /*!
    99         * \brief IplImage
    100         *
    101         * \return IplImage
    102         */
    103         IplImage* img;
     106  Type const &GetDataType() const { return dataType; };
    104107
    105         Type const&GetDataType() const {return dataType;};
     108private:
     109  /*!
     110  * \brief Copy datas
     111  *
     112  * Reimplemented from io_data. \n
     113  * See io_data::CopyDatas.
     114  *
     115  * \param dst destination buffer
     116  */
     117  void CopyDatas(char *dst) const;
    106118
    107     private:
    108         /*!
    109         * \brief Copy datas
    110         *
    111         * Reimplemented from io_data. \n
    112         * See io_data::CopyDatas.
    113         *
    114         * \param dst destination buffer
    115         */
    116         void CopyDatas(char* dst) const;
    117 
    118         bool allocate_data;
    119         Type dataType;
    120     };
     119  bool allocate_data;
     120  Type dataType;
     121};
    121122
    122123} // end namespace core

  • trunk/lib/FlairCore/src/cvmatrix.cpp

    r2 r15  
    2424using std::string;
    2525
    26 namespace flair { namespace core {
    27     /*! \class cvmatrixElement
    28     */
    29     class cvmatrixElement: public IODataElement {
    30     public:
    31         cvmatrixElement(const cvmatrix *matrix,string name,uint32_t row, uint32_t col):IODataElement(matrix,name) {
    32             this->matrix=matrix;
    33             this->row=row;
    34             this->col=col;
    35             if(row>=matrix->Rows() || col>=matrix->Cols()) {
    36                 matrix->Err("index (%i,%i) out of bound (%i,%i)\n",row,col,matrix->Rows()-1,matrix->Cols()-1);
    37                 size=0;
    38             } else {
    39                 try {
    40                     ScalarType const &scalarType=dynamic_cast<ScalarType const &>(matrix->GetDataType().GetElementDataType());
    41                     size=scalarType.GetSize();
    42                 } catch (std::bad_cast e) {
    43                     matrix->Err("type not handled\n");
    44                     size=0;
    45                 }
    46             }
    47         }
     26namespace flair {
     27namespace core {
     28/*! \class cvmatrixElement
     29*/
     30class cvmatrixElement : public IODataElement {
     31public:
     32  cvmatrixElement(const cvmatrix *matrix, string name, uint32_t row,
     33                  uint32_t col)
     34      : IODataElement(matrix, name) {
     35    this->matrix = matrix;
     36    this->row = row;
     37    this->col = col;
     38    if (row >= matrix->Rows() || col >= matrix->Cols()) {
     39      matrix->Err("index (%i,%i) out of bound (%i,%i)\n", row, col,
     40                  matrix->Rows() - 1, matrix->Cols() - 1);
     41      size = 0;
     42    } else {
     43      try {
     44        ScalarType const &scalarType = dynamic_cast<ScalarType const &>(
     45            matrix->GetDataType().GetElementDataType());
     46        size = scalarType.GetSize();
     47      } catch (std::bad_cast e) {
     48        matrix->Err("type not handled\n");
     49        size = 0;
     50      }
     51    }
     52  }
    4853
    49         ~cvmatrixElement() {
    50         }
     54  ~cvmatrixElement() {}
    5155
    52         void CopyData(char* dst) const {
    53             if (typeid(matrix->GetDataType().GetElementDataType())==typeid(FloatType)) {
    54                 float value=matrix->Value(row,col);
    55                 memcpy(dst,&value,sizeof(value));
    56             } else if (typeid(matrix->GetDataType().GetElementDataType())==typeid(SignedIntegerType)) {
    57                 switch(matrix->GetDataType().GetElementDataType().GetSize()) {
    58                 case 1: {
    59                         int8_t int8Value=matrix->Value(row,col);
    60                         memcpy(dst,&int8Value,1);
    61                     }
    62                     break;
    63                 case 2: {
    64                         int16_t int16Value=matrix->Value(row,col);
    65                         memcpy(dst,&int16Value,2);
    66                     }
    67                     break;
    68                 }
    69             }
    70         }
     56  void CopyData(char *dst) const {
     57    if (typeid(matrix->GetDataType().GetElementDataType()) ==
     58        typeid(FloatType)) {
     59      float value = matrix->Value(row, col);
     60      memcpy(dst, &value, sizeof(value));
     61    } else if (typeid(matrix->GetDataType().GetElementDataType()) ==
     62               typeid(SignedIntegerType)) {
     63      switch (matrix->GetDataType().GetElementDataType().GetSize()) {
     64      case 1: {
     65        int8_t int8Value = matrix->Value(row, col);
     66        memcpy(dst, &int8Value, 1);
     67      } break;
     68      case 2: {
     69        int16_t int16Value = matrix->Value(row, col);
     70        memcpy(dst, &int16Value, 2);
     71      } break;
     72      }
     73    }
     74  }
    7175
    72         DataType const &GetDataType(void) const {
    73             return matrix->GetDataType().GetElementDataType();
    74         }
     76  DataType const &GetDataType(void) const {
     77    return matrix->GetDataType().GetElementDataType();
     78  }
    7579
    76     private:
    77         const cvmatrix *matrix;
    78         uint32_t row,col;
     80private:
     81  const cvmatrix *matrix;
     82  uint32_t row, col;
     83};
    7984
    80     };
     85cvmatrix::cvmatrix(const Object *parent, uint32_t rows, uint32_t cols,
     86                   ScalarType const &elementDataType, string name, uint32_t n)
     87    : io_data(parent, name, n), dataType(rows, cols, elementDataType) {
     88  pimpl_ = new cvmatrix_impl(this, rows, cols, elementDataType, n);
    8189
    82 cvmatrix::cvmatrix(const Object* parent,uint32_t rows, uint32_t cols, ScalarType const &elementDataType,string name,uint32_t n): io_data(parent,name,n),dataType(rows,cols,elementDataType) {
    83     pimpl_=new cvmatrix_impl(this,rows,cols,elementDataType,n);
    84 
    85     for(uint32_t i=0;i<rows;i++) {
    86         for(uint32_t j=0;j<cols;j++) {
    87             AppendLogDescription(pimpl_->descriptor->ElementName(i,j),elementDataType);
    88             SetValue(i,j,0);
    89         }
     90  for (uint32_t i = 0; i < rows; i++) {
     91    for (uint32_t j = 0; j < cols; j++) {
     92      AppendLogDescription(pimpl_->descriptor->ElementName(i, j),
     93                           elementDataType);
     94      SetValue(i, j, 0);
    9095    }
     96  }
    9197}
    9298
    93 cvmatrix::cvmatrix(const Object* parent,const cvmatrix_descriptor *descriptor, ScalarType const &elementDataType,string name,uint32_t n): io_data(parent,name,n),dataType(descriptor->Rows(),descriptor->Cols(),elementDataType) {
    94     pimpl_=new cvmatrix_impl(this,descriptor,elementDataType,n);
     99cvmatrix::cvmatrix(const Object *parent, const cvmatrix_descriptor *descriptor,
     100                   ScalarType const &elementDataType, string name, uint32_t n)
     101    : io_data(parent, name, n),
     102      dataType(descriptor->Rows(), descriptor->Cols(), elementDataType) {
     103  pimpl_ = new cvmatrix_impl(this, descriptor, elementDataType, n);
    95104
    96     for(uint32_t i=0;i<descriptor->Rows();i++) {
    97         for(uint32_t j=0;j<descriptor->Cols();j++) {
    98             AppendLogDescription(descriptor->ElementName(i,j),elementDataType);
    99             SetValue(i,j,0);
    100         }
     105  for (uint32_t i = 0; i < descriptor->Rows(); i++) {
     106    for (uint32_t j = 0; j < descriptor->Cols(); j++) {
     107      AppendLogDescription(descriptor->ElementName(i, j), elementDataType);
     108      SetValue(i, j, 0);
    101109    }
     110  }
    102111}
    103112
    104 cvmatrix::~cvmatrix() {
    105     delete pimpl_;
     113cvmatrix::~cvmatrix() { delete pimpl_; }
     114
     115IODataElement *cvmatrix::Element(uint32_t row, uint32_t col) const {
     116  return new cvmatrixElement(this, Name(row, col), row, col);
    106117}
    107118
    108 IODataElement* cvmatrix::Element(uint32_t row, uint32_t col) const {
    109     return new cvmatrixElement(this,Name(row,col),row,col);
    110 }
    111 
    112 IODataElement* cvmatrix::Element(uint32_t index) const {
    113     if(Rows()==1) {
    114         return new cvmatrixElement(this,Name(0,index),0,index);
    115     } else if(Cols()==1) {
    116         return new cvmatrixElement(this,Name(index,0),index,0);
    117     } else {
    118         Err("matrix is not 1D\n");
    119         return nullptr;
    120     }
     119IODataElement *cvmatrix::Element(uint32_t index) const {
     120  if (Rows() == 1) {
     121    return new cvmatrixElement(this, Name(0, index), 0, index);
     122  } else if (Cols() == 1) {
     123    return new cvmatrixElement(this, Name(index, 0), index, 0);
     124  } else {
     125    Err("matrix is not 1D\n");
     126    return nullptr;
     127  }
    121128}
    122129
    123130float cvmatrix::Value(uint32_t row, uint32_t col) const {
    124     float value;
     131  float value;
    125132
    126     if(row>=(uint32_t)pimpl_->mat->rows || col>=(uint32_t)pimpl_->mat->cols) {
    127         Warn("index (%i,%i) out of bound (%i,%i)\n",row,col,pimpl_->mat->rows-1,pimpl_->mat->cols-1);
    128         return 0;
    129     }
     133  if (row >= (uint32_t)pimpl_->mat->rows ||
     134      col >= (uint32_t)pimpl_->mat->cols) {
     135    Warn("index (%i,%i) out of bound (%i,%i)\n", row, col,
     136         pimpl_->mat->rows - 1, pimpl_->mat->cols - 1);
     137    return 0;
     138  }
    130139
    131     GetMutex();
    132     value=cvGetReal2D(pimpl_->mat,row,col);
    133     ReleaseMutex();
     140  GetMutex();
     141  value = cvGetReal2D(pimpl_->mat, row, col);
     142  ReleaseMutex();
    134143
    135     return value;
     144  return value;
    136145}
    137146
    138147float cvmatrix::ValueNoMutex(uint32_t row, uint32_t col) const {
    139     if(row>=(uint32_t)pimpl_->mat->rows || col>=(uint32_t)pimpl_->mat->cols) {
    140         Warn("index (%i,%i) out of bound (%i,%i)\n",row,col,pimpl_->mat->rows-1,pimpl_->mat->cols-1);
    141         return 0;
    142     }
     148  if (row >= (uint32_t)pimpl_->mat->rows ||
     149      col >= (uint32_t)pimpl_->mat->cols) {
     150    Warn("index (%i,%i) out of bound (%i,%i)\n", row, col,
     151         pimpl_->mat->rows - 1, pimpl_->mat->cols - 1);
     152    return 0;
     153  }
    143154
    144     return cvGetReal2D(pimpl_->mat,row,col);
     155  return cvGetReal2D(pimpl_->mat, row, col);
    145156}
    146157
    147 void cvmatrix::SetValue(uint32_t row, uint32_t col,float value) {
    148     if(row>=(uint32_t)pimpl_->mat->rows || col>=(uint32_t)pimpl_->mat->cols) {
    149         Warn("index (%i,%i) out of bound (%i,%i)\n",row,col,pimpl_->mat->rows-1,pimpl_->mat->cols-1);
    150     } else {
    151         GetMutex();
    152         cvSetReal2D(pimpl_->mat,row,col,value);
    153         ReleaseMutex();
    154     }
     158void cvmatrix::SetValue(uint32_t row, uint32_t col, float value) {
     159  if (row >= (uint32_t)pimpl_->mat->rows ||
     160      col >= (uint32_t)pimpl_->mat->cols) {
     161    Warn("index (%i,%i) out of bound (%i,%i)\n", row, col,
     162         pimpl_->mat->rows - 1, pimpl_->mat->cols - 1);
     163  } else {
     164    GetMutex();
     165    cvSetReal2D(pimpl_->mat, row, col, value);
     166    ReleaseMutex();
     167  }
    155168}
    156169
    157 void cvmatrix::SetValueNoMutex(uint32_t row, uint32_t col,float value) {
    158     if(row>=(uint32_t)pimpl_->mat->rows || col>=(uint32_t)pimpl_->mat->cols) {
    159         Warn("index (%i,%i) out of bound (%i,%i)\n",row,col,pimpl_->mat->rows-1,pimpl_->mat->cols-1);
    160     } else {
    161         cvSetReal2D(pimpl_->mat,row,col,value);
    162     }
     170void cvmatrix::SetValueNoMutex(uint32_t row, uint32_t col, float value) {
     171  if (row >= (uint32_t)pimpl_->mat->rows ||
     172      col >= (uint32_t)pimpl_->mat->cols) {
     173    Warn("index (%i,%i) out of bound (%i,%i)\n", row, col,
     174         pimpl_->mat->rows - 1, pimpl_->mat->cols - 1);
     175  } else {
     176    cvSetReal2D(pimpl_->mat, row, col, value);
     177  }
    163178}
    164179
    165 CvMat* cvmatrix::getCvMat(void) const {
    166     return pimpl_->mat;
     180CvMat *cvmatrix::getCvMat(void) const { return pimpl_->mat; }
     181
     182void cvmatrix::CopyDatas(char *dst) const {
     183  GetMutex();
     184  // printf("%f %x %i\n",cvGetReal2D(pimpl_->mat,0,0),dst,Size());
     185  memcpy(dst, pimpl_->mat->data.ptr, dataType.GetSize());
     186  ReleaseMutex();
    167187}
    168188
    169 void cvmatrix::CopyDatas(char* dst) const {
    170     GetMutex();
    171     //printf("%f %x %i\n",cvGetReal2D(pimpl_->mat,0,0),dst,Size());
    172     memcpy(dst,pimpl_->mat->data.ptr,dataType.GetSize());
    173     ReleaseMutex();
    174 }
     189uint32_t cvmatrix::Rows(void) const { return pimpl_->mat->rows; }
    175190
    176 uint32_t cvmatrix::Rows(void) const {
    177     return pimpl_->mat->rows;
    178 }
    179 
    180 uint32_t cvmatrix::Cols(void) const {
    181     return pimpl_->mat->cols;
    182 }
     191uint32_t cvmatrix::Cols(void) const { return pimpl_->mat->cols; }
    183192
    184193string cvmatrix::Name(uint32_t row, uint32_t col) const {
    185     return pimpl_->descriptor->ElementName(row,col);
     194  return pimpl_->descriptor->ElementName(row, col);
    186195}
    187 
    188196
    189197} // end namespace core

  • trunk/lib/FlairCore/src/cvmatrix.h

    r2 r15  
    2121struct CvMat;
    2222
    23 namespace flair { namespace core {
    24 
    25     /*! \class cvmatrix
    26     *
    27     * \brief Class defining a matrix of kind CvMat
    28     *
    29     * CvMat is a matrix struct defined in OpenCV.
    30     *
    31     */
    32     class cvmatrix: public io_data {
    33     public:
    34         class Type: public DataType {
    35         public:
    36             Type(size_t _nbRows,size_t _nbCols,ScalarType const &_elementDataType):nbRows(_nbRows),nbCols(_nbCols),elementDataType(_elementDataType) {}
    37             size_t GetSize() const {
    38                 return nbRows*nbCols*elementDataType.GetSize();
    39             }
    40             std::string GetDescription() const {return "matrix";}
    41             size_t GetNbRows() const {return nbRows;}
    42             size_t GetNbCols() const {return nbCols;}
    43             ScalarType const &GetElementDataType() const {return elementDataType;}
    44 
    45         private:
    46             size_t nbRows,nbCols;
    47             ScalarType const &elementDataType;
    48         };
    49 
    50         /*!
    51         * \brief Constructor
    52         *
    53         * Construct an io_data representing a CvMat. \n
    54         * It uses a cvmatrix_descriptor to get size and elements' names. \n
    55         * Names are used for graphs and logs.
    56         *
    57         * \param parent parent
    58         * \param descriptor matrix description
    59         * \param type type of matrix elements
    60         * \param name name
    61         * \param n number of samples
    62         */
    63         cvmatrix(const Object* parent,const cvmatrix_descriptor *descriptor, ScalarType const &elementDataType,std::string name="",uint32_t n=1);
    64 
    65         /*!
    66         * \brief Constructor
    67         *
    68         * Construct an io_data representing a CvMat. \n
    69         * Elements are unamed.
    70         *
    71         * \param parent parent
    72         * \param rows matrix rows
    73         * \param cols matrix cols
    74         * \param type type of matrix elements
    75         * \param name name
    76         * \param n number of samples
    77         */
    78         cvmatrix(const Object* parent,uint32_t rows, uint32_t cols, ScalarType const &elementDataType,std::string name="",uint32_t n=1);
    79 
    80         /*!
    81         * \brief Destructor
    82         *
    83         */
    84         ~cvmatrix();
    85 
    86         /*!
    87         * \brief Element value
    88         *
    89         * Element is accessed by locking and unlocking the io_data Mutex.
    90         *
    91         * \param row element row
    92         * \param col element col
    93         *
    94         * \return element value
    95         */
    96         float Value(uint32_t row, uint32_t col) const;
    97 
    98         /*!
    99         * \brief Element value
    100         *
    101         * Element is not accessed by locking and unlocking the io_data Mutex. \n
    102         * Thus, this function should be called with Mutex locked. \n
    103         * This function is usefull when multiple successive access are done to the
    104         * elments of the matrix. It avoids unnecessary locking and unlocking.
    105         *
    106         * \param row element row
    107         * \param col element col
    108         *
    109         * \return element value
    110         */
    111         float ValueNoMutex(uint32_t row, uint32_t col) const;
    112 
    113         /*!
    114         * \brief Set element value
    115         *
    116         * Element is accessed by locking and unlocking the io_data Mutex.
    117         *
    118         * \param row element row
    119         * \param col element col
    120         * \param value element value
    121         */
    122         void SetValue(uint32_t row, uint32_t col,float value);
    123 
    124         /*!
    125         * \brief Set element value
    126         *
    127         * Element is not accessed by locking and unlocking the io_data Mutex. \n
    128         * Thus, this function should be called with Mutex locked. \n
    129         * This function is usefull when multiple successive access are done to the
    130         * elments of the matrix. It avoids unnecessary locking and unlocking.
    131         *
    132         * \param row element row
    133         * \param col element col
    134         * \param value element value
    135         */
    136         void SetValueNoMutex(uint32_t row, uint32_t col,float value);
    137 
    138         /*!
    139         * \brief get CvMat
    140         *
    141         * The io_data Mutex must be used by the user.
    142         */
    143         CvMat* getCvMat(void) const;
    144 
    145         /*!
    146         * \brief Element name
    147         *
    148         * If cvmatrix was created without cvmatrix_descriptor, element name is empty.
    149         *
    150         * \param row element row
    151         * \param col element col
    152         *
    153         * \return element name
    154         */
    155         std::string Name(uint32_t row, uint32_t col) const;
    156 
    157         /*!
    158         * \brief Element
    159         *
    160         * Get a pointer to a specific element. This pointer can be used for plotting.
    161         *
    162         * \param row element row
    163         * \param col element col
    164         *
    165         * \return pointer to the element
    166         */
    167         IODataElement* Element(uint32_t row, uint32_t col) const;
    168 
    169         /*!
    170         * \brief Element
    171         *
    172         * Get a pointer to a specific element. This pointer can be used for plotting. \n
    173         * This function can be used for a 1D matrix.
    174         *
    175         * \param index element index
    176         *
    177         * \return pointer to the element
    178         */
    179         IODataElement* Element(uint32_t index) const;
    180 
    181         /*!
    182         * \brief Number of rows
    183         *
    184         * \return rows
    185         */
    186         uint32_t Rows(void) const;
    187 
    188         /*!
    189         * \brief Number of colomns
    190         *
    191         * \return colomns
    192         */
    193         uint32_t Cols(void) const;
    194 
    195         Type const &GetDataType() const {return dataType;};
    196 
    197     private:
    198         /*!
    199         * \brief Copy datas
    200         *
    201         * Reimplemented from io_data. \n
    202         * See io_data::CopyDatas.
    203         *
    204         * \param dst destination buffer
    205         */
    206         void CopyDatas(char* dst) const;
    207 
    208         class cvmatrix_impl* pimpl_;
    209         Type dataType;
    210     };
     23namespace flair {
     24namespace core {
     25
     26/*! \class cvmatrix
     27*
     28* \brief Class defining a matrix of kind CvMat
     29*
     30* CvMat is a matrix struct defined in OpenCV.
     31*
     32*/
     33class cvmatrix : public io_data {
     34public:
     35  class Type : public DataType {
     36  public:
     37    Type(size_t _nbRows, size_t _nbCols, ScalarType const &_elementDataType)
     38        : nbRows(_nbRows), nbCols(_nbCols), elementDataType(_elementDataType) {}
     39    size_t GetSize() const {
     40      return nbRows * nbCols * elementDataType.GetSize();
     41    }
     42    std::string GetDescription() const { return "matrix"; }
     43    size_t GetNbRows() const { return nbRows; }
     44    size_t GetNbCols() const { return nbCols; }
     45    ScalarType const &GetElementDataType() const { return elementDataType; }
     46
     47  private:
     48    size_t nbRows, nbCols;
     49    ScalarType const &elementDataType;
     50  };
     51
     52  /*!
     53  * \brief Constructor
     54  *
     55  * Construct an io_data representing a CvMat. \n
     56  * It uses a cvmatrix_descriptor to get size and elements' names. \n
     57  * Names are used for graphs and logs.
     58  *
     59  * \param parent parent
     60  * \param descriptor matrix description
     61  * \param type type of matrix elements
     62  * \param name name
     63  * \param n number of samples
     64  */
     65  cvmatrix(const Object *parent, const cvmatrix_descriptor *descriptor,
     66           ScalarType const &elementDataType, std::string name = "",
     67           uint32_t n = 1);
     68
     69  /*!
     70  * \brief Constructor
     71  *
     72  * Construct an io_data representing a CvMat. \n
     73  * Elements are unamed.
     74  *
     75  * \param parent parent
     76  * \param rows matrix rows
     77  * \param cols matrix cols
     78  * \param type type of matrix elements
     79  * \param name name
     80  * \param n number of samples
     81  */
     82  cvmatrix(const Object *parent, uint32_t rows, uint32_t cols,
     83           ScalarType const &elementDataType, std::string name = "",
     84           uint32_t n = 1);
     85
     86  /*!
     87  * \brief Destructor
     88  *
     89  */
     90  ~cvmatrix();
     91
     92  /*!
     93  * \brief Element value
     94  *
     95  * Element is accessed by locking and unlocking the io_data Mutex.
     96  *
     97  * \param row element row
     98  * \param col element col
     99  *
     100  * \return element value
     101  */
     102  float Value(uint32_t row, uint32_t col) const;
     103
     104  /*!
     105  * \brief Element value
     106  *
     107  * Element is not accessed by locking and unlocking the io_data Mutex. \n
     108  * Thus, this function should be called with Mutex locked. \n
     109  * This function is usefull when multiple successive access are done to the
     110  * elments of the matrix. It avoids unnecessary locking and unlocking.
     111  *
     112  * \param row element row
     113  * \param col element col
     114  *
     115  * \return element value
     116  */
     117  float ValueNoMutex(uint32_t row, uint32_t col) const;
     118
     119  /*!
     120  * \brief Set element value
     121  *
     122  * Element is accessed by locking and unlocking the io_data Mutex.
     123  *
     124  * \param row element row
     125  * \param col element col
     126  * \param value element value
     127  */
     128  void SetValue(uint32_t row, uint32_t col, float value);
     129
     130  /*!
     131  * \brief Set element value
     132  *
     133  * Element is not accessed by locking and unlocking the io_data Mutex. \n
     134  * Thus, this function should be called with Mutex locked. \n
     135  * This function is usefull when multiple successive access are done to the
     136  * elments of the matrix. It avoids unnecessary locking and unlocking.
     137  *
     138  * \param row element row
     139  * \param col element col
     140  * \param value element value
     141  */
     142  void SetValueNoMutex(uint32_t row, uint32_t col, float value);
     143
     144  /*!
     145  * \brief get CvMat
     146  *
     147  * The io_data Mutex must be used by the user.
     148  */
     149  CvMat *getCvMat(void) const;
     150
     151  /*!
     152  * \brief Element name
     153  *
     154  * If cvmatrix was created without cvmatrix_descriptor, element name is empty.
     155  *
     156  * \param row element row
     157  * \param col element col
     158  *
     159  * \return element name
     160  */
     161  std::string Name(uint32_t row, uint32_t col) const;
     162
     163  /*!
     164  * \brief Element
     165  *
     166  * Get a pointer to a specific element. This pointer can be used for plotting.
     167  *
     168  * \param row element row
     169  * \param col element col
     170  *
     171  * \return pointer to the element
     172  */
     173  IODataElement *Element(uint32_t row, uint32_t col) const;
     174
     175  /*!
     176  * \brief Element
     177  *
     178  * Get a pointer to a specific element. This pointer can be used for plotting.
     179  *\n
     180  * This function can be used for a 1D matrix.
     181  *
     182  * \param index element index
     183  *
     184  * \return pointer to the element
     185  */
     186  IODataElement *Element(uint32_t index) const;
     187
     188  /*!
     189  * \brief Number of rows
     190  *
     191  * \return rows
     192  */
     193  uint32_t Rows(void) const;
     194
     195  /*!
     196  * \brief Number of colomns
     197  *
     198  * \return colomns
     199  */
     200  uint32_t Cols(void) const;
     201
     202  Type const &GetDataType() const { return dataType; };
     203
     204private:
     205  /*!
     206  * \brief Copy datas
     207  *
     208  * Reimplemented from io_data. \n
     209  * See io_data::CopyDatas.
     210  *
     211  * \param dst destination buffer
     212  */
     213  void CopyDatas(char *dst) const;
     214
     215  class cvmatrix_impl *pimpl_;
     216  Type dataType;
     217};
    211218
    212219} // end namespace core

  • trunk/lib/FlairCore/src/cvmatrix_descriptor.cpp

    r2 r15  
    2020using std::string;
    2121
    22 namespace flair { namespace core {
     22namespace flair {
     23namespace core {
    2324
    2425cvmatrix_descriptor::cvmatrix_descriptor(uint32_t rows, uint32_t cols) {
    25     this->rows=rows;
    26     this->cols=cols;
     26  this->rows = rows;
     27  this->cols = cols;
    2728
    28     if(rows==0 || cols==0) getFrameworkManager()->Err("rows and cols must be >0\n");
     29  if (rows == 0 || cols == 0)
     30    getFrameworkManager()->Err("rows and cols must be >0\n");
    2931
    30     element_names=(string**)malloc(rows*cols*sizeof(string*));
    31     for(uint32_t i=0; i<rows*cols; i++) {
    32         element_names[i]=new string();
    33     }
     32  element_names = (string **)malloc(rows * cols * sizeof(string *));
     33  for (uint32_t i = 0; i < rows * cols; i++) {
     34    element_names[i] = new string();
     35  }
    3436}
    3537
    3638cvmatrix_descriptor::~cvmatrix_descriptor() {
    37     for(uint32_t i=0; i<rows*cols; i++) {
    38         delete element_names[i];
    39     }
    40     free(element_names);
     39  for (uint32_t i = 0; i < rows * cols; i++) {
     40    delete element_names[i];
     41  }
     42  free(element_names);
    4143}
    4244
    43 void cvmatrix_descriptor::SetElementName(uint32_t row, uint32_t col,string name) {
    44     if(row>=rows || col>=cols) {
    45         getFrameworkManager()->Err("index out of bound %s (%i,%i), range (%i,%i)\n",name.c_str(),row,col,rows-1,cols-1);
    46         return;
    47     }
    48     *element_names[row*cols+col]=name;
     45void cvmatrix_descriptor::SetElementName(uint32_t row, uint32_t col,
     46                                         string name) {
     47  if (row >= rows || col >= cols) {
     48    getFrameworkManager()->Err("index out of bound %s (%i,%i), range (%i,%i)\n",
     49                               name.c_str(), row, col, rows - 1, cols - 1);
     50    return;
     51  }
     52  *element_names[row * cols + col] = name;
    4953}
    5054
    5155string cvmatrix_descriptor::ElementName(uint32_t row, uint32_t col) const {
    52     if(row>=rows || col>=cols) {
    53         getFrameworkManager()->Err("index out of bound (%i,%i), range (%i,%i)\n",row,col,rows-1,cols-1);
    54         return *element_names[0];//safe value...
    55     }
    56     return *element_names[row*cols+col];
     56  if (row >= rows || col >= cols) {
     57    getFrameworkManager()->Err("index out of bound (%i,%i), range (%i,%i)\n",
     58                               row, col, rows - 1, cols - 1);
     59    return *element_names[0]; // safe value...
     60  }
     61  return *element_names[row * cols + col];
    5762}
    5863
    59 uint32_t cvmatrix_descriptor::Rows() const {
    60     return rows;
    61 }
     64uint32_t cvmatrix_descriptor::Rows() const { return rows; }
    6265
    63 uint32_t cvmatrix_descriptor::Cols() const {
    64     return cols;
    65 }
     66uint32_t cvmatrix_descriptor::Cols() const { return cols; }
    6667
    6768} // end namespace core

  • trunk/lib/FlairCore/src/cvmatrix_descriptor.h

    r2 r15  
    1616#include <string>
    1717
    18 namespace flair { namespace core
    19 {
     18namespace flair {
     19namespace core {
    2020
    21     /*! \class cvmatrix_descriptor
    22     *
    23     * \brief Class describing cvmatrix elements, for log and graphs purpose
    24     *
    25     * This class allows to give a name to matrix elements. These names
    26     * will be used in graphs and logs.
    27     */
    28     class cvmatrix_descriptor {
    29         public:
    30             /*!
    31             * \brief Constructor
    32             *
    33             * Construct a matrix descriptor.
    34             *
    35             * \param rows matrix rows
    36             * \param cols matrix cols
    37             */
    38             cvmatrix_descriptor(uint32_t rows, uint32_t cols);
     21/*! \class cvmatrix_descriptor
     22*
     23* \brief Class describing cvmatrix elements, for log and graphs purpose
     24*
     25* This class allows to give a name to matrix elements. These names
     26* will be used in graphs and logs.
     27*/
     28class cvmatrix_descriptor {
     29public:
     30  /*!
     31  * \brief Constructor
     32  *
     33  * Construct a matrix descriptor.
     34  *
     35  * \param rows matrix rows
     36  * \param cols matrix cols
     37  */
     38  cvmatrix_descriptor(uint32_t rows, uint32_t cols);
    3939
    40             /*!
    41             * \brief Destructor
    42             *
    43             */
    44             ~cvmatrix_descriptor();
     40  /*!
     41  * \brief Destructor
     42  *
     43  */
     44  ~cvmatrix_descriptor();
    4545
    46             /*!
    47             * \brief Set element name
    48             *
    49             * \param row element row
    50             * \param col element col
    51             * \param name element name
    52             */
    53             void SetElementName(uint32_t row, uint32_t col,std::string name);
     46  /*!
     47  * \brief Set element name
     48  *
     49  * \param row element row
     50  * \param col element col
     51  * \param name element name
     52  */
     53  void SetElementName(uint32_t row, uint32_t col, std::string name);
    5454
    55             /*!
    56             * \brief Element name
    57             *
    58             * \param row element row
    59             * \param col element col
    60             *
    61             * \return element name
    62             */
    63             std::string ElementName(uint32_t row, uint32_t col) const;
     55  /*!
     56  * \brief Element name
     57  *
     58  * \param row element row
     59  * \param col element col
     60  *
     61  * \return element name
     62  */
     63  std::string ElementName(uint32_t row, uint32_t col) const;
    6464
    65             /*!
    66             * \brief Number of rows
    67             *
    68             * \return rows
    69             */
    70             uint32_t Rows(void) const;
     65  /*!
     66  * \brief Number of rows
     67  *
     68  * \return rows
     69  */
     70  uint32_t Rows(void) const;
    7171
    72             /*!
    73             * \brief Number of colomns
    74             *
    75             * \return colomns
    76             */
    77             uint32_t Cols(void) const;
     72  /*!
     73  * \brief Number of colomns
     74  *
     75  * \return colomns
     76  */
     77  uint32_t Cols(void) const;
    7878
    79         private:
    80             uint32_t rows,cols;
    81             std::string **element_names;
    82 
    83     };
     79private:
     80  uint32_t rows, cols;
     81  std::string **element_names;
     82};
    8483
    8584} // end namespace core

  • trunk/lib/FlairCore/src/cvmatrix_impl.cpp

    r2 r15  
    2424using namespace flair::core;
    2525
    26 cvmatrix_impl::cvmatrix_impl(cvmatrix* self,int rows, int cols,flair::core::ScalarType const &_elementDataType,int n):elementDataType(_elementDataType) {
    27     descriptor=new cvmatrix_descriptor(rows,cols);
    28     Init(self,n);
     26cvmatrix_impl::cvmatrix_impl(cvmatrix *self, int rows, int cols,
     27                             flair::core::ScalarType const &_elementDataType,
     28                             int n)
     29    : elementDataType(_elementDataType) {
     30  descriptor = new cvmatrix_descriptor(rows, cols);
     31  Init(self, n);
    2932}
    3033
    31 cvmatrix_impl::cvmatrix_impl(cvmatrix* self,const cvmatrix_descriptor *descriptor,flair::core::ScalarType const &_elementDataType,int  n):elementDataType(_elementDataType) {
    32     this->descriptor=descriptor;
    33     Init(self,n);
     34cvmatrix_impl::cvmatrix_impl(cvmatrix *self,
     35                             const cvmatrix_descriptor *descriptor,
     36                             flair::core::ScalarType const &_elementDataType,
     37                             int n)
     38    : elementDataType(_elementDataType) {
     39  this->descriptor = descriptor;
     40  Init(self, n);
    3441}
    3542
    36 void cvmatrix_impl::Init(cvmatrix* self,int n) {
    37     this->self=self;
     43void cvmatrix_impl::Init(cvmatrix *self, int n) {
     44  this->self = self;
    3845
    39     mat=nullptr;
    40     try {
    41         ScalarType const &scalarType=dynamic_cast<ScalarType const &>(elementDataType);
    42         if (typeid(scalarType)==typeid(FloatType)) {
    43             mat=cvCreateMat(descriptor->Rows(),descriptor->Cols(),CV_32FC1);
    44         } else if (typeid(scalarType)==typeid(SignedIntegerType)) {
    45             switch(elementDataType.GetSize()) {
    46             case 1:
    47                 mat=cvCreateMat(descriptor->Rows(),descriptor->Cols(),CV_8SC1);
    48                 break;
    49             case 2:
    50                 mat=cvCreateMat(descriptor->Rows(),descriptor->Cols(),CV_16SC1);
    51                 break;
    52             default:
    53                 self->Err("unsupported integer scalar type\n");
    54             }
    55         } else {
    56             self->Err("unsupported scalar type\n");
    57         }
    58     } catch(std::bad_cast e) {
    59         self->Err("type is not a scalar\n");
     46  mat = nullptr;
     47  try {
     48    ScalarType const &scalarType =
     49        dynamic_cast<ScalarType const &>(elementDataType);
     50    if (typeid(scalarType) == typeid(FloatType)) {
     51      mat = cvCreateMat(descriptor->Rows(), descriptor->Cols(), CV_32FC1);
     52    } else if (typeid(scalarType) == typeid(SignedIntegerType)) {
     53      switch (elementDataType.GetSize()) {
     54      case 1:
     55        mat = cvCreateMat(descriptor->Rows(), descriptor->Cols(), CV_8SC1);
     56        break;
     57      case 2:
     58        mat = cvCreateMat(descriptor->Rows(), descriptor->Cols(), CV_16SC1);
     59        break;
     60      default:
     61        self->Err("unsupported integer scalar type\n");
     62      }
     63    } else {
     64      self->Err("unsupported scalar type\n");
    6065    }
     66  } catch (std::bad_cast e) {
     67    self->Err("type is not a scalar\n");
     68  }
    6169
    62     if(mat==nullptr) self->Err("allocating matrix failed\n");
    63     if(n>1) self->Warn("n>1 not supported\n");
     70  if (mat == nullptr)
     71    self->Err("allocating matrix failed\n");
     72  if (n > 1)
     73    self->Warn("n>1 not supported\n");
    6474}
    6575
    6676cvmatrix_impl::~cvmatrix_impl() {
    67     cvReleaseMat(&mat);
    68     delete descriptor;
     77  cvReleaseMat(&mat);
     78  delete descriptor;
    6979}

  • trunk/lib/FlairCore/src/io_data.cpp

    r2 r15  
    2121using std::string;
    2222
    23 namespace flair { namespace core {
     23namespace flair {
     24namespace core {
    2425
    2526DummyType dummyType;
    … …  
    2829SignedIntegerType Int16Type(16);
    2930
    30 io_data::io_data(const Object* parent,string name,int n): Mutex(parent,name) {
    31     pimpl_=new io_data_impl(this,n);
     31io_data::io_data(const Object *parent, string name, int n)
     32    : Mutex(parent, name) {
     33  pimpl_ = new io_data_impl(this, n);
    3234}
    3335
    34 io_data::~io_data() {
    35     delete pimpl_;
    36 }
     36io_data::~io_data() { delete pimpl_; }
    3737
    38 void io_data::AppendLogDescription(string description,DataType const &datatype) {
    39     pimpl_->AppendLogDescription(description,datatype);
     38void io_data::AppendLogDescription(string description,
     39                                   DataType const &datatype) {
     40  pimpl_->AppendLogDescription(description, datatype);
    4041}
    4142
    4243void io_data::SetDataTime(Time time) {
    43     GetMutex();
    44     pimpl_->time=time;
    45     ReleaseMutex();
     44  GetMutex();
     45  pimpl_->time = time;
     46  ReleaseMutex();
    4647}
    4748
    4849Time io_data::DataTime(void) const {
    49     Time tmp;
    50     GetMutex();
    51     tmp=pimpl_->time;
    52     ReleaseMutex();
    53     return tmp;
     50  Time tmp;
     51  GetMutex();
     52  tmp = pimpl_->time;
     53  ReleaseMutex();
     54  return tmp;
    5455}
    5556
    56 const io_data* io_data::Prev(int n) const {
    57     if(n>0)
    58         return prev->Prev(n-1);
    59     else
    60         return this;
     57const io_data *io_data::Prev(int n) const {
     58  if (n > 0)
     59    return prev->Prev(n - 1);
     60  else
     61    return this;
    6162}
    6263
    63 void io_data::SetPtrToCircle(void **ptr) {
    64     pimpl_->circle_ptr=ptr;
    65 }
     64void io_data::SetPtrToCircle(void **ptr) { pimpl_->circle_ptr = ptr; }
    6665
    6766} // end namespace core

  • trunk/lib/FlairCore/src/io_data.h

    r2 r15  
    1919class io_data_impl;
    2020
    21 namespace flair { namespace core {
     21namespace flair {
     22namespace core {
    2223
    23     class Object;
     24class Object;
    2425
    25     class DataType {
    26     public:
    27         virtual std::string GetDescription() const =0;
    28         //size in bytes
    29         virtual size_t GetSize() const =0;
    30     };
     26class DataType {
     27public:
     28  virtual std::string GetDescription() const = 0;
     29  // size in bytes
     30  virtual size_t GetSize() const = 0;
     31};
    3132
    32     class DummyType: public DataType {
    33     public:
    34         size_t GetSize() const {return 0;}
    35         std::string GetDescription() const {return "dummy";};
    36     };
    37     extern DummyType dummyType;
     33class DummyType : public DataType {
     34public:
     35  size_t GetSize() const { return 0; }
     36  std::string GetDescription() const { return "dummy"; };
     37};
     38extern DummyType dummyType;
    3839
    39     class ScalarType: public DataType {
    40     public:
    41         ScalarType(size_t _size):size(_size) {}
    42         size_t GetSize() const {return size;}
    43         virtual std::string GetDescription() const {return "scalar";};
    44     private:
    45         size_t size;
    46     };
     40class ScalarType : public DataType {
     41public:
     42  ScalarType(size_t _size) : size(_size) {}
     43  size_t GetSize() const { return size; }
     44  virtual std::string GetDescription() const { return "scalar"; };
    4745
    48     class SignedIntegerType: public ScalarType {
    49     public:
    50         SignedIntegerType(size_t sizeInBits):ScalarType(sizeInBits/8){}
    51         std::string GetDescription() const {return "int"+std::to_string(GetSize()*8)+"_t";};
    52     };
    53     extern SignedIntegerType Int8Type;
    54     extern SignedIntegerType Int16Type;
     46private:
     47  size_t size;
     48};
    5549
    56     class FloatType: public ScalarType {
    57     public:
    58         FloatType():ScalarType(4){}
    59         std::string GetDescription() const {return "float";};
    60     };
    61     extern FloatType floatType;
     50class SignedIntegerType : public ScalarType {
     51public:
     52  SignedIntegerType(size_t sizeInBits) : ScalarType(sizeInBits / 8) {}
     53  std::string GetDescription() const {
     54    return "int" + std::to_string(GetSize() * 8) + "_t";
     55  };
     56};
     57extern SignedIntegerType Int8Type;
     58extern SignedIntegerType Int16Type;
    6259
     60class FloatType : public ScalarType {
     61public:
     62  FloatType() : ScalarType(4) {}
     63  std::string GetDescription() const { return "float"; };
     64};
     65extern FloatType floatType;
    6366
    64     /*! \class io_data
    65     *
    66     * \brief Abstract class for data types.
    67     *
    68     * Use this class to define a custom data type. Data types ares used for logging and graphs. \n
    69     * The reimplemented class must call SetSize() in its constructor. \n
    70     * io_data can be constructed with n samples (see io_data::io_data).
    71     * In this case, old samples can be accessed throug io_data::Prev.
    72     */
    73     class io_data: public Mutex {
    74         friend class IODevice;
    75         friend class ::IODevice_impl;
    76         friend class ::io_data_impl;
     67/*! \class io_data
     68*
     69* \brief Abstract class for data types.
     70*
     71* Use this class to define a custom data type. Data types ares used for logging
     72*and graphs. \n
     73* The reimplemented class must call SetSize() in its constructor. \n
     74* io_data can be constructed with n samples (see io_data::io_data).
     75* In this case, old samples can be accessed throug io_data::Prev.
     76*/
     77class io_data : public Mutex {
     78  friend class IODevice;
     79  friend class ::IODevice_impl;
     80  friend class ::io_data_impl;
    7781
    78         public:
    79             /*!
    80             * \brief Constructor
    81             *
    82             * Construct an io_data. \n
    83             *
    84             * \param parent parent
    85             * \param name name
    86             * \param n number of samples
    87             */
    88             io_data(const Object* parent,std::string name,int n);
     82public:
     83  /*!
     84  * \brief Constructor
     85  *
     86  * Construct an io_data. \n
     87  *
     88  * \param parent parent
     89  * \param name name
     90  * \param n number of samples
     91  */
     92  io_data(const Object *parent, std::string name, int n);
    8993
    90             /*!
    91             * \brief Destructor
    92             *
    93             */
    94             virtual ~io_data();
     94  /*!
     95  * \brief Destructor
     96  *
     97  */
     98  virtual ~io_data();
    9599
    96             /*!
    97             * \brief Set data time
    98             *
    99             * \param time time
    100             */
    101             void SetDataTime(Time time);
     100  /*!
     101  * \brief Set data time
     102  *
     103  * \param time time
     104  */
     105  void SetDataTime(Time time);
    102106
    103             /*!
    104             * \brief Data time
    105             *
    106             * \return data time
    107             */
    108             Time DataTime(void) const;
     107  /*!
     108  * \brief Data time
     109  *
     110  * \return data time
     111  */
     112  Time DataTime(void) const;
    109113
    110             /*!
    111             * \brief Previous data
    112             *
    113             * Access previous data. io_data must have been constructed with
    114             * n>1, io_data::SetPtrToCircle must have been set and
    115             * io_data::prev must have been allocated.
    116             *
    117             * \param n previous data number
    118             *
    119             * \return previous data
    120             */
    121             const io_data* Prev(int n) const;
     114  /*!
     115  * \brief Previous data
     116  *
     117  * Access previous data. io_data must have been constructed with
     118  * n>1, io_data::SetPtrToCircle must have been set and
     119  * io_data::prev must have been allocated.
     120  *
     121  * \param n previous data number
     122  *
     123  * \return previous data
     124  */
     125  const io_data *Prev(int n) const;
    122126
    123             virtual DataType const&GetDataType() const =0;
     127  virtual DataType const &GetDataType() const = 0;
    124128
    125         protected:
    126             /*!
    127             * \brief Specify the description of the reimplemented class data's
    128             *
    129             *  This method must be called in the constructor of the reimplemented class, once by element. \n
    130             *  Each element description must be called in the same order as CopyDatas put the datas in the buffer. \n
    131             *  The description will be used for the log descriptor file.
    132             *
    133             * \param description description of the element
    134             * \param datatype type of the element
    135             */
    136             void AppendLogDescription(std::string description, DataType const &datatype);
     129protected:
     130  /*!
     131  * \brief Specify the description of the reimplemented class data's
     132  *
     133  *  This method must be called in the constructor of the reimplemented class,
     134  *once by element. \n
     135  *  Each element description must be called in the same order as CopyDatas put
     136  *the datas in the buffer. \n
     137  *  The description will be used for the log descriptor file.
     138  *
     139  * \param description description of the element
     140  * \param datatype type of the element
     141  */
     142  void AppendLogDescription(std::string description, DataType const &datatype);
    137143
    138             /*!
    139             * \brief Set the datas to circle
    140             *
    141             * \param ptr pointer to the data to circle
    142             */
    143             void SetPtrToCircle(void **ptr);
     144  /*!
     145  * \brief Set the datas to circle
     146  *
     147  * \param ptr pointer to the data to circle
     148  */
     149  void SetPtrToCircle(void **ptr);
    144150
    145             /*!
    146             * \brief Pointer to previous data
    147             *
    148             * Reimplemented class must allocate this pointer if n>1. \n
    149             * Pointer must be allocated with the same kind of reimplemented class.
    150             */
    151             io_data* prev;
     151  /*!
     152  * \brief Pointer to previous data
     153  *
     154  * Reimplemented class must allocate this pointer if n>1. \n
     155  * Pointer must be allocated with the same kind of reimplemented class.
     156  */
     157  io_data *prev;
    152158
    153         private:
    154             /*!
    155             * \brief Copy datas
    156             *
    157             * This method is automatically called by IODevice::ProcessUpdate to log io_data datas. \n
    158             * This method must be reimplemented, in order to copy the datas to the logs.
    159             * Copied datas must be of size io_data::Size.
    160             *
    161             * \param dst destination buffer
    162             */
    163             virtual void CopyDatas(char* dst) const =0;
     159private:
     160  /*!
     161  * \brief Copy datas
     162  *
     163  * This method is automatically called by IODevice::ProcessUpdate to log
     164  *io_data datas. \n
     165  * This method must be reimplemented, in order to copy the datas to the logs.
     166  * Copied datas must be of size io_data::Size.
     167  *
     168  * \param dst destination buffer
     169  */
     170  virtual void CopyDatas(char *dst) const = 0;
    164171
    165             io_data_impl *pimpl_;
    166     };
     172  io_data_impl *pimpl_;
     173};
    167174
    168175} // end namespace core

  • trunk/lib/FlairCore/src/io_data_impl.cpp

    r2 r15  
    2323using namespace flair::core;
    2424
    25 io_data_impl::io_data_impl(io_data* self,int n) {
    26     this->self=self;
    27     this->n=n;
    28     if(n<1) self->Err("n doit être >0\n");
    29     size=0;
    30     is_consistent=false;
    31     circle_ptr=NULL;
     25io_data_impl::io_data_impl(io_data *self, int n) {
     26  this->self = self;
     27  this->n = n;
     28  if (n < 1)
     29    self->Err("n doit être >0\n");
     30  size = 0;
     31  is_consistent = false;
     32  circle_ptr = NULL;
    3233}
    3334
    3435io_data_impl::~io_data_impl() {}
    3536
    36 void io_data_impl::AppendLogDescription(string description,DataType const &datatype) {
    37     description+=" ("+datatype.GetDescription()+")";
    38     descriptors.push_back(description);
     37void io_data_impl::AppendLogDescription(string description,
     38                                        DataType const &datatype) {
     39  description += " (" + datatype.GetDescription() + ")";
     40  descriptors.push_back(description);
    3941}
    4042
    41 void io_data_impl::WriteLogDescriptor(std::fstream& desc_file,int *index) {
    42     for(size_t i=0;i<descriptors.size();i++) {
    43         desc_file << (*index)++ << ": " << self->ObjectName() << " - " << descriptors.at(i) << "\n";
    44     }
     43void io_data_impl::WriteLogDescriptor(std::fstream &desc_file, int *index) {
     44  for (size_t i = 0; i < descriptors.size(); i++) {
     45    desc_file << (*index)++ << ": " << self->ObjectName() << " - "
     46              << descriptors.at(i) << "\n";
     47  }
    4548}
    4649
    4750void io_data_impl::PrintLogDescriptor(void) {
    48     for(size_t i=0;i<descriptors.size();i++) {
    49         Printf("    %s\n",descriptors.at(i).c_str());
    50     }
     51  for (size_t i = 0; i < descriptors.size(); i++) {
     52    Printf("    %s\n", descriptors.at(i).c_str());
     53  }
    5154}
    5255
    5356void io_data_impl::Circle(void) {
    54     if(n>1) {
    55         self->GetMutex();
     57  if (n > 1) {
     58    self->GetMutex();
    5659
    57         void* tmp=*self->Prev(n-1)->pimpl_->circle_ptr;
     60    void *tmp = *self->Prev(n - 1)->pimpl_->circle_ptr;
    5861
    59         for(int i=0;i<n-1;i++) {
    60             //printf("%i\n",i);
    61             *(self->Prev(n-1-i)->pimpl_->circle_ptr)=*(self->Prev(n-2-i)->pimpl_->circle_ptr);
    62             self->Prev(n-1-i)->pimpl_->is_consistent=self->Prev(n-2-i)->pimpl_->is_consistent;
    63             self->Prev(n-1-i)->pimpl_->time=self->Prev(n-2-i)->pimpl_->time;
    64         }
    65         *circle_ptr=tmp;
    66         is_consistent=false;
    67         self->ReleaseMutex();
    68 
     62    for (int i = 0; i < n - 1; i++) {
     63      // printf("%i\n",i);
     64      *(self->Prev(n - 1 - i)->pimpl_->circle_ptr) =
     65          *(self->Prev(n - 2 - i)->pimpl_->circle_ptr);
     66      self->Prev(n - 1 - i)->pimpl_->is_consistent =
     67          self->Prev(n - 2 - i)->pimpl_->is_consistent;
     68      self->Prev(n - 1 - i)->pimpl_->time = self->Prev(n - 2 - i)->pimpl_->time;
    6969    }
     70    *circle_ptr = tmp;
     71    is_consistent = false;
     72    self->ReleaseMutex();
     73  }
    7074}
    7175
    72 bool io_data_impl::IsConsistent(void) {
    73     return is_consistent;
    74 }
     76bool io_data_impl::IsConsistent(void) { return is_consistent; }
    7577
    76 void io_data_impl::SetConsistent(bool status) {
    77     is_consistent=status;
    78 }
     78void io_data_impl::SetConsistent(bool status) { is_consistent = status; }

  • trunk/lib/FlairCore/src/ui_com.cpp

    r2 r15  
    3939using namespace flair::gui;
    4040
    41 ui_com::ui_com(const Object *parent,UDTSOCKET sock): Thread(parent,"send",2)
    42 {
    43     //buffer envoi
    44     send_buffer=(char*)malloc(3);
    45     send_size=3;//id(1)+period(2)
    46 
    47     //mutex
    48     send_mutex=NULL;
    49     send_mutex=new Mutex(this,ObjectName());
    50 
    51     socket_fd=sock;
    52     connection_lost=false;
     41ui_com::ui_com(const Object *parent, UDTSOCKET sock)
     42    : Thread(parent, "send", 2) {
     43  // buffer envoi
     44  send_buffer = (char *)malloc(3);
     45  send_size = 3; // id(1)+period(2)
     46
     47  // mutex
     48  send_mutex = NULL;
     49  send_mutex = new Mutex(this, ObjectName());
     50
     51  socket_fd = sock;
     52  connection_lost = false;
    5353
    5454#ifdef __XENO__
    55     int status;
    56     string tmp_name;
    57 
    58     is_running=true;
    59 
    60     //pipe
    61     tmp_name= getFrameworkManager()->ObjectName() + "-" + ObjectName()+ "-pipe";
    62     //xenomai limitation
    63     if(tmp_name.size()>31) Err("rt_pipe_create error (%s is too long)\n",tmp_name.c_str());
     55  int status;
     56  string tmp_name;
     57
     58  is_running = true;
     59
     60  // pipe
     61  tmp_name = getFrameworkManager()->ObjectName() + "-" + ObjectName() + "-pipe";
     62  // xenomai limitation
     63  if (tmp_name.size() > 31)
     64    Err("rt_pipe_create error (%s is too long)\n", tmp_name.c_str());
    6465#ifdef RT_PIPE_SIZE
    65     status=rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO,RT_PIPE_SIZE);
     66  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
    6667#else
    67     status=rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO,0);
    68 #endif
    69 
    70     if(status!=0)
    71     {
    72         Err("rt_pipe_create error (%s)\n",strerror(-status));
    73         //return -1;
    74     }
    75 
    76     //start user side thread
     68  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, 0);
     69#endif
     70
     71  if (status != 0) {
     72    Err("rt_pipe_create error (%s)\n", strerror(-status));
     73    // return -1;
     74  }
     75
     76// start user side thread
    7777#ifdef NRT_STACK_SIZE
    78     // Initialize thread creation attributes
    79     pthread_attr_t attr;
    80     if(pthread_attr_init(&attr) != 0)
    81     {
    82         Err("pthread_attr_init error\n");
    83     }
    84 
    85     if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0)
    86     {
    87        Err("pthread_attr_setstacksize error\n");
    88     }
    89 
    90     if(pthread_create(&thread,  &attr, user_thread, (void*)this) < 0)
    91 #else //NRT_STACK_SIZE
    92     if(pthread_create(&thread, NULL, user_thread, (void*)this) < 0)
    93 #endif //NRT_STACK_SIZE
    94     {
    95         Err("pthread_create error\n");
    96       //return -1;
    97     }
     78  // Initialize thread creation attributes
     79  pthread_attr_t attr;
     80  if (pthread_attr_init(&attr) != 0) {
     81    Err("pthread_attr_init error\n");
     82  }
     83
     84  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
     85    Err("pthread_attr_setstacksize error\n");
     86  }
     87
     88  if (pthread_create(&thread, &attr, user_thread, (void *)this) < 0)
     89#else  // NRT_STACK_SIZE
     90  if (pthread_create(&thread, NULL, user_thread, (void *)this) < 0)
     91#endif // NRT_STACK_SIZE
     92  {
     93    Err("pthread_create error\n");
     94    // return -1;
     95  }
    9896#ifdef NRT_STACK_SIZE
    99     if(pthread_attr_destroy(&attr) != 0)
    100     {
    101         Err("pthread_attr_destroy error\n");
    102     }
    103 #endif
    104 
    105 #endif//__XENO__
    106     int timeout=100;
    107     if(UDT::setsockopt(socket_fd, 0, UDT_RCVTIMEO, &timeout, sizeof(int))!=0) Err("UDT::setsockopt error (UDT_RCVTIMEO)\n");
    108 
    109     bool blocking = true;
    110     if(UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool))!=0) Err("UDT::setsockopt error (UDT_SNDSYN)\n");
    111 
    112     if(UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool))!=0) Err("UDT::setsockopt error (UDT_RCVSYN)\n");
    113 //#endif //__XENO__
    114 
    115     Start();
    116 }
    117 
    118 
    119 ui_com::~ui_com()
    120 {
    121     //printf("destruction ui_com\n");
     97  if (pthread_attr_destroy(&attr) != 0) {
     98    Err("pthread_attr_destroy error\n");
     99  }
     100#endif
     101
     102#endif //__XENO__
     103  int timeout = 100;
     104  if (UDT::setsockopt(socket_fd, 0, UDT_RCVTIMEO, &timeout, sizeof(int)) != 0)
     105    Err("UDT::setsockopt error (UDT_RCVTIMEO)\n");
     106
     107  bool blocking = true;
     108  if (UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
     109    Err("UDT::setsockopt error (UDT_SNDSYN)\n");
     110
     111  if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
     112    Err("UDT::setsockopt error (UDT_RCVSYN)\n");
     113  //#endif //__XENO__
     114
     115  Start();
     116}
     117
     118ui_com::~ui_com() {
     119// printf("destruction ui_com\n");
    122120
    123121#ifdef __XENO__
    124     is_running=false;
    125 
    126     pthread_join(thread, NULL);
    127 
    128     int status=rt_pipe_delete(&pipe);
    129     if(status!=0) Err("rt_pipe_delete error (%s)\n",strerror(-status));
    130 #endif
    131 
    132     SafeStop();
    133 
    134     if(IsSuspended()==true) Resume();
    135 
    136     Join();
    137 
    138     if(send_buffer!=NULL) free(send_buffer);
    139     send_buffer=NULL;
    140 
    141     //printf("destruction ui_com ok\n");
    142 }
    143 
    144 void ui_com::Send(char* buf,ssize_t size) {
    145     if(connection_lost==true) return;
    146 
    147     char* tosend=buf;
    148     ssize_t nb_write;
    149 
    150     if(buf[0]==XML_HEADER) {
    151         //cut xml header
    152         tosend=strstr(buf,"<root");
    153         size-=tosend-buf;
    154     }
     122  is_running = false;
     123
     124  pthread_join(thread, NULL);
     125
     126  int status = rt_pipe_delete(&pipe);
     127  if (status != 0)
     128    Err("rt_pipe_delete error (%s)\n", strerror(-status));
     129#endif
     130
     131  SafeStop();
     132
     133  if (IsSuspended() == true)
     134    Resume();
     135
     136  Join();
     137
     138  if (send_buffer != NULL)
     139    free(send_buffer);
     140  send_buffer = NULL;
     141
     142  // printf("destruction ui_com ok\n");
     143}
     144
     145void ui_com::Send(char *buf, ssize_t size) {
     146  if (connection_lost == true)
     147    return;
     148
     149  char *tosend = buf;
     150  ssize_t nb_write;
     151
     152  if (buf[0] == XML_HEADER) {
     153    // cut xml header
     154    tosend = strstr(buf, "<root");
     155    size -= tosend - buf;
     156  }
    155157
    156158#ifdef __XENO__
    157     nb_write=rt_pipe_write(&pipe,tosend,size,P_NORMAL);
    158 
    159     if(nb_write<0)
    160     {
    161         Err("rt_pipe_write error (%s)\n",strerror(-nb_write));
    162     }
    163     else if (nb_write != size)
    164     {
    165         Err("rt_pipe_write error %i/%i\n",nb_write,size);
    166     }
     159  nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
     160
     161  if (nb_write < 0) {
     162    Err("rt_pipe_write error (%s)\n", strerror(-nb_write));
     163  } else if (nb_write != size) {
     164    Err("rt_pipe_write error %i/%i\n", nb_write, size);
     165  }
    167166#else //__XENO__
    168167
    169168#ifdef COMPRESS_FRAMES
    170     bool sendItCompressed=false;
    171     if(buf[0]==XML_HEADER) {
    172         sendItCompressed=true;
    173     }
    174 
    175     if(sendItCompressed) {
    176         char *out;
    177         ssize_t out_size;
    178         if(compressBuffer(tosend, size,&out,&out_size, 9)==Z_OK) {
    179             size=out_size;
    180             nb_write = UDT::sendmsg(socket_fd,out,size, -1, true);
    181             free(out);
     169  bool sendItCompressed = false;
     170  if (buf[0] == XML_HEADER) {
     171    sendItCompressed = true;
     172  }
     173
     174  if (sendItCompressed) {
     175    char *out;
     176    ssize_t out_size;
     177    if (compressBuffer(tosend, size, &out, &out_size, 9) == Z_OK) {
     178      size = out_size;
     179      nb_write = UDT::sendmsg(socket_fd, out, size, -1, true);
     180      free(out);
     181    } else {
     182      Warn("Compress error, sending it uncompressed\n");
     183      sendItCompressed = false;
     184    }
     185  }
     186
     187  if (!sendItCompressed) {
     188    nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
     189  }
     190
     191#else // COMPRESS_FRAMES
     192  nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
     193#endif // COMPRESS_FRAMES
     194  // Printf("write %i %i\n",nb_write,size);
     195  if (nb_write < 0) {
     196    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
     197    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
     198        UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
     199      connection_lost = true;
     200    }
     201  } else if (nb_write != size) {
     202    Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
     203        UDT::getlasterror().getErrorCode(), nb_write, size);
     204  }
     205#endif //__XENO__
     206}
     207
     208ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
     209  ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
     210
     211  if (bytesRead < 0) {
     212    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST) {
     213      Err("UDT::recvmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
     214      connection_lost = true;
     215    }
     216  }
     217
     218  return bytesRead;
     219}
     220
     221void ui_com::Run(void) {
     222  // check endianness
     223  char header;
     224  if (IsBigEndian()) {
     225    header = DATAS_BIG_ENDIAN;
     226    Printf("System is big endian\n");
     227  } else {
     228    header = DATAS_LITTLE_ENDIAN;
     229    Printf("System is little endian\n");
     230  }
     231
     232#ifdef __XENO__
     233  WarnUponSwitches(true);
     234  printf("\n"); // a revoir pourquoi??
     235// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
     236#endif
     237  // on attend d'avoir des choses à faire
     238  Suspend();
     239
     240  while (!ToBeStopped()) {
     241    size_t resume_id;
     242    Time min = 0xffffffffffffffffULL;
     243
     244    // on recpuere l'id de la prochaine execution
     245    send_mutex->GetMutex();
     246    resume_id = resume_time.size();
     247    for (size_t i = 0; i < resume_time.size(); i++) {
     248      if (resume_time.at(i) < min && data_to_send.at(i)->IsEnabled() == true) {
     249        min = resume_time.at(i);
     250        resume_id = i;
     251      }
     252    }
     253
     254    // attente
     255    if (resume_id < resume_time.size()) {
     256      Time time = resume_time.at(resume_id);
     257      uint16_t resume_period = data_to_send.at(resume_id)->SendPeriod();
     258      send_mutex->ReleaseMutex();
     259      // on dort jusqu'a la prochaine execution
     260      SleepUntil(time);
     261
     262      // envoi des donnees
     263      int offset = 3;
     264      send_buffer[0] = header;
     265      send_mutex->GetMutex();
     266
     267      for (size_t i = 0; i < data_to_send.size(); i++) {
     268        if (data_to_send.at(i)->SendPeriod() == resume_period &&
     269            data_to_send.at(i)->IsEnabled() == true) {
     270          data_to_send.at(i)->CopyDatas(send_buffer + offset);
     271          offset += data_to_send.at(i)->SendSize();
     272        }
     273      }
     274      if (offset != 3) {
     275        memcpy(&send_buffer[1], &resume_period, sizeof(uint16_t));
     276        // printf("send %i %i %i %x
     277        // %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
     278        // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
     279        // printf("\n");
     280        Send(send_buffer, offset);
     281      }
     282
     283      // on planifie la prochaine execution
     284      for (size_t i = 0; i < data_to_send.size(); i++) {
     285        if (data_to_send.at(i)->SendPeriod() == resume_period) {
     286          resume_time.at(i) += data_to_send.at(i)->SendPeriod() * 1000000;
     287        }
     288      }
     289      send_mutex->ReleaseMutex();
     290      // Printf("%i %lld\n",resume_period,GetTime()/1000000);
     291    } else {
     292      send_mutex->ReleaseMutex();
     293      // rien a faire, suspend
     294      // Printf("rien a faire suspend\n");
     295      Suspend();
     296      // Printf("wake\n");
     297      // on planifie la prochaine execution
     298      Time time = GetTime();
     299      send_mutex->GetMutex();
     300      for (size_t i = 0; i < data_to_send.size(); i++) {
     301        resume_time.at(i) =
     302            time + (Time)data_to_send.at(i)->SendPeriod() * 1000000;
     303      }
     304      send_mutex->ReleaseMutex();
     305    }
     306  }
     307}
     308#ifdef __XENO__
     309void *ui_com::user_thread(void *arg) {
     310  int pipe_fd = -1;
     311  string devname;
     312  char *buf = NULL;
     313  ui_com *caller = (ui_com *)arg;
     314  int rv;
     315  fd_set set;
     316  struct timeval timeout;
     317  ssize_t nb_read, nb_write;
     318
     319  buf = (char *)malloc(NRT_PIPE_SIZE);
     320  if (buf == NULL) {
     321    caller->Err("malloc error\n");
     322  }
     323
     324  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
     325            caller->ObjectName() + "-pipe";
     326  while (pipe_fd < 0) {
     327    pipe_fd = open(devname.c_str(), O_RDWR);
     328    if (pipe_fd < 0 && errno != ENOENT)
     329      caller->Err("open pipe_fd error (%s)\n", strerror(-errno));
     330    usleep(1000);
     331  }
     332
     333  while (1) {
     334    FD_ZERO(&set);         // clear the set
     335    FD_SET(pipe_fd, &set); // add our file descriptor to the set
     336
     337    timeout.tv_sec = 0;
     338    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
     339
     340    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
     341
     342    if (rv == -1) {
     343      if (caller->is_running == false &&
     344          UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
     345        break; // timeout
     346      if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
     347        caller->Err("epoll_wait, %s, code %i\n",
     348                    UDT::getlasterror().getErrorMessage(),
     349                    UDT::getlasterror().getErrorCode());
     350    } else if (rv == 0) {
     351      // printf("timeout\n"); // a timeout occured
     352      if (caller->is_running == false)
     353        break;
     354
     355    } else {
     356      nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
     357      buf[nb_read] = 0;
     358// printf("envoi\n%s\n",buf);
     359
     360#ifdef COMPRESS_FRAMES
     361      char *out;
     362      ssize_t out_size;
     363
     364      if (buf[0] == XML_HEADER) {
     365        if (compressBuffer(buf, nb_read, &out, &out_size, 9) == Z_OK) {
     366          nb_read = out_size;
     367          nb_write = UDT::sendmsg(caller->socket_fd, out, out_size, -1, true);
     368          free(out);
    182369        } else {
    183             Warn("Compress error, sending it uncompressed\n");
    184             sendItCompressed=false;
     370          caller->Warn("Compress error, sending it uncompressed\n");
     371          nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
    185372        }
    186     }
    187 
    188     if(!sendItCompressed) {
    189         nb_write = UDT::sendmsg(socket_fd,tosend,size, -1, true);
    190     }
    191 
    192 #else //COMPRESS_FRAMES
    193     nb_write = UDT::sendmsg(socket_fd,tosend,size, -1, true);
    194 #endif //COMPRESS_FRAMES
    195 //Printf("write %i %i\n",nb_write,size);
    196     if(nb_write<0) {
    197         Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    198         if(UDT::getlasterror().getErrorCode()==CUDTException::ECONNLOST || UDT::getlasterror().getErrorCode()==CUDTException::EINVSOCK) {
    199             connection_lost=true;
     373      } else {
     374        nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
     375      }
     376#else
     377      nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
     378#endif
     379
     380      if (nb_write < 0) {
     381        caller->Err("UDT::sendmsg error (%s)\n",
     382                    UDT::getlasterror().getErrorMessage());
     383        if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
     384            UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
     385          caller->connection_lost = true;
    200386        }
    201     } else if (nb_write != size) {
    202         Err("%s, code %i (%ld/%ld)\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode(),nb_write,size);
    203     }
    204 #endif //__XENO__
    205 }
    206 
    207 ssize_t ui_com::Receive(char* buf,ssize_t buf_size) {
    208     ssize_t bytesRead= UDT::recvmsg(socket_fd,buf,buf_size);
    209 
    210     if(bytesRead<0) {
    211         if(UDT::getlasterror().getErrorCode()==CUDTException::ECONNLOST) {
    212             Err("UDT::recvmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    213             connection_lost=true;
    214         }
    215     }
    216 
    217     return bytesRead;
    218 }
    219 
    220 void ui_com::Run(void)
    221 {
    222     //check endianness
    223     char header;
    224     if(IsBigEndian())
    225     {
    226         header=DATAS_BIG_ENDIAN;
    227         Printf("System is big endian\n");
    228     }
    229     else
    230     {
    231         header=DATAS_LITTLE_ENDIAN;
    232         Printf("System is little endian\n");
    233     }
    234 
    235 #ifdef __XENO__
    236     WarnUponSwitches(true);
    237     printf("\n");//a revoir pourquoi??
    238     //sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
    239 #endif
    240     //on attend d'avoir des choses à faire
    241     Suspend();
    242 
    243     while(!ToBeStopped())
    244     {
    245         size_t resume_id;
    246         Time min=0xffffffffffffffffULL;
    247 
    248         //on recpuere l'id de la prochaine execution
    249         send_mutex->GetMutex();
    250         resume_id=resume_time.size();
    251         for(size_t i=0;i<resume_time.size();i++)
    252         {
    253             if(resume_time.at(i)<min && data_to_send.at(i)->IsEnabled()==true)
    254             {
    255                 min=resume_time.at(i);
    256                 resume_id=i;
    257             }
    258         }
    259 
    260         //attente
    261         if(resume_id<resume_time.size())
    262         {
    263             Time time=resume_time.at(resume_id);
    264             uint16_t resume_period=data_to_send.at(resume_id)->SendPeriod();
    265             send_mutex->ReleaseMutex();
    266             //on dort jusqu'a la prochaine execution
    267             SleepUntil(time);
    268 
    269             //envoi des donnees
    270             int offset=3;
    271             send_buffer[0]=header;
    272             send_mutex->GetMutex();
    273 
    274             for(size_t i=0;i<data_to_send.size();i++) {
    275                 if(data_to_send.at(i)->SendPeriod()==resume_period && data_to_send.at(i)->IsEnabled()==true) {
    276                     data_to_send.at(i)->CopyDatas(send_buffer+offset);
    277                     offset+=data_to_send.at(i)->SendSize();
    278                 }
    279             }
    280             if(offset!=3) {
    281                 memcpy(&send_buffer[1],&resume_period,sizeof(uint16_t));
    282                 //printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
    283                 //for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
    284                 //printf("\n");
    285                 Send(send_buffer,offset);
    286             }
    287 
    288             //on planifie la prochaine execution
    289             for(size_t i=0;i<data_to_send.size();i++)
    290             {
    291                 if(data_to_send.at(i)->SendPeriod()==resume_period)
    292                 {
    293                     resume_time.at(i)+=data_to_send.at(i)->SendPeriod()*1000000;
    294                 }
    295             }
    296             send_mutex->ReleaseMutex();
    297             //Printf("%i %lld\n",resume_period,GetTime()/1000000);
    298         }
    299         else
    300         {
    301             send_mutex->ReleaseMutex();
    302             //rien a faire, suspend
    303             //Printf("rien a faire suspend\n");
    304             Suspend();
    305             //Printf("wake\n");
    306             //on planifie la prochaine execution
    307             Time time=GetTime();
    308             send_mutex->GetMutex();
    309             for(size_t i=0;i<data_to_send.size();i++)
    310             {
    311                 resume_time.at(i)=time+(Time)data_to_send.at(i)->SendPeriod()*1000000;
    312             }
    313             send_mutex->ReleaseMutex();
    314         }
    315     }
    316 }
    317 #ifdef __XENO__
    318 void* ui_com::user_thread(void * arg)
    319 {
    320     int pipe_fd=-1;
    321     string devname;
    322     char* buf=NULL;
    323     ui_com *caller = (ui_com*)arg;
    324     int rv;
    325     fd_set set;
    326     struct timeval timeout;
    327     ssize_t nb_read,nb_write;
    328 
    329     buf=(char*)malloc(NRT_PIPE_SIZE);
    330     if(buf==NULL)
    331     {
    332         caller->Err("malloc error\n");
    333     }
    334 
    335     devname= NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" + caller->ObjectName()+ "-pipe";
    336     while(pipe_fd<0)
    337     {
    338         pipe_fd = open(devname.c_str(), O_RDWR);
    339         if (pipe_fd < 0 && errno!=ENOENT) caller->Err("open pipe_fd error (%s)\n",strerror(-errno));
    340         usleep(1000);
    341     }
    342 
    343     while(1)
    344     {
    345         FD_ZERO(&set); // clear the set
    346         FD_SET(pipe_fd, &set); // add our file descriptor to the set
    347 
    348         timeout.tv_sec = 0;
    349         timeout.tv_usec = SELECT_TIMEOUT_MS*1000;
    350 
    351         rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
    352 
    353         if(rv == -1)
    354         {
    355             if(caller->is_running==false && UDT::getlasterror().getErrorCode()==CUDTException::ETIMEOUT) break;//timeout
    356             if(UDT::getlasterror().getErrorCode()!=CUDTException::ETIMEOUT) caller->Err("epoll_wait, %s, code %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode());
    357         }
    358         else if(rv == 0)
    359         {
    360             //printf("timeout\n"); // a timeout occured
    361             if(caller->is_running==false) break;
    362 
    363         }
    364         else
    365         {
    366             nb_read=read(pipe_fd,buf,NRT_PIPE_SIZE);
    367             buf[nb_read]=0;
    368             //printf("envoi\n%s\n",buf);
    369 
    370 #ifdef COMPRESS_FRAMES
    371             char *out;
    372             ssize_t out_size;
    373 
    374             if(buf[0]==XML_HEADER) {
    375                 if(compressBuffer(buf, nb_read,&out,&out_size, 9)==Z_OK) {
    376                     nb_read=out_size;
    377                     nb_write = UDT::sendmsg(caller->socket_fd,out,out_size, -1, true);
    378                     free(out);
    379                 } else {
    380                     caller->Warn("Compress error, sending it uncompressed\n");
    381                     nb_write = UDT::sendmsg(caller->socket_fd, buf,nb_read, -1, true);
    382                 }
    383             } else {
    384                 nb_write = UDT::sendmsg(caller->socket_fd, buf,nb_read, -1, true);
    385             }
    386 #else
    387             nb_write = UDT::sendmsg(caller->socket_fd, buf,nb_read, -1, true);
    388 #endif
    389 
    390             if(nb_write<0)
    391             {
    392                 caller->Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage());
    393                 if(UDT::getlasterror().getErrorCode()==CUDTException::ECONNLOST || UDT::getlasterror().getErrorCode()==CUDTException::EINVSOCK)
    394                 {
    395                     caller->connection_lost=true;
    396                 }
    397             }
    398             else if (nb_write != nb_read)
    399             {
    400                 caller->Err("UDT::sendmsg error %i/%i\n",nb_write,nb_read);
    401             }
    402         }
    403     }
    404 
    405     close(pipe_fd);
    406     if(buf!=NULL) free(buf);
    407     pthread_exit(0);
    408 }
    409 #endif
    410 
    411 int ui_com::compressBuffer(char *in, ssize_t in_size,char **out,ssize_t *out_size, int level)
    412 {
    413     int ret, flush;
    414     unsigned have;
    415     z_stream strm;
    416 
    417     /* allocate deflate state */
    418     strm.zalloc = Z_NULL;
    419     strm.zfree = Z_NULL;
    420     strm.opaque = Z_NULL;
    421     ret = deflateInit(&strm, level);
    422     if (ret != Z_OK)
    423         return ret;
    424 
    425     *out=(char*)malloc(COMPRESS_CHUNK);
    426     if(!(*out))
    427         return Z_BUF_ERROR;
    428 
    429     strm.next_in = (unsigned char*)in;
    430     strm.avail_out =  COMPRESS_CHUNK;
    431     strm.next_out = (unsigned char*)*out;
    432     strm.avail_in=in_size;
    433     flush=Z_FINISH;
    434 
    435     ret = deflate(&strm, flush);    /* no bad return value */
    436     if (ret == Z_STREAM_ERROR) {
    437         free(*out);
    438         return ret;
    439     }
    440 
    441     have = COMPRESS_CHUNK - strm.avail_out;
    442     *out_size=have;
    443 //printf("%i -> %i\n",in_size,have);
    444     /* clean up and return */
    445     (void)deflateEnd(&strm);
    446 
    447     if(strm.avail_out!=0) {
    448         return Z_OK;
    449     } else {
    450         return Z_STREAM_ERROR;
    451     }
    452 }
    453 
    454 int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,unsigned char **out,ssize_t *out_size)
    455 {
    456     int ret;
    457     //unsigned have;
    458     z_stream strm;
    459 
    460     /* allocate inflate state */
    461     strm.zalloc = Z_NULL;
    462     strm.zfree = Z_NULL;
    463     strm.opaque = Z_NULL;
    464     strm.avail_in = 0;
    465     strm.next_in = Z_NULL;
    466     ret = inflateInit(&strm);
    467     if (ret != Z_OK)
    468         return ret;
    469 
    470     *out=(unsigned char*)malloc(COMPRESS_CHUNK);
    471     if(!(*out))
    472         return Z_BUF_ERROR;
    473 
    474     strm.avail_in = in_size;
    475     strm.next_in = in;
    476     strm.avail_out = COMPRESS_CHUNK;
    477     strm.next_out = *out;
    478 
    479     ret = inflate(&strm, Z_NO_FLUSH);
    480     assert(ret != Z_STREAM_ERROR);  /* state not clobbered */
    481     switch (ret) {
    482     case Z_NEED_DICT:
    483         ret = Z_DATA_ERROR;     /* and fall through */
    484     case Z_DATA_ERROR:
    485     case Z_MEM_ERROR:
    486         (void)inflateEnd(&strm);
    487         return ret;
    488     }
    489     //have = COMPRESS_CHUNK - strm.avail_out;
    490 
    491     /* clean up and return */
     387      } else if (nb_write != nb_read) {
     388        caller->Err("UDT::sendmsg error %i/%i\n", nb_write, nb_read);
     389      }
     390    }
     391  }
     392
     393  close(pipe_fd);
     394  if (buf != NULL)
     395    free(buf);
     396  pthread_exit(0);
     397}
     398#endif
     399
     400int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
     401                           ssize_t *out_size, int level) {
     402  int ret, flush;
     403  unsigned have;
     404  z_stream strm;
     405
     406  /* allocate deflate state */
     407  strm.zalloc = Z_NULL;
     408  strm.zfree = Z_NULL;
     409  strm.opaque = Z_NULL;
     410  ret = deflateInit(&strm, level);
     411  if (ret != Z_OK)
     412    return ret;
     413
     414  *out = (char *)malloc(COMPRESS_CHUNK);
     415  if (!(*out))
     416    return Z_BUF_ERROR;
     417
     418  strm.next_in = (unsigned char *)in;
     419  strm.avail_out = COMPRESS_CHUNK;
     420  strm.next_out = (unsigned char *)*out;
     421  strm.avail_in = in_size;
     422  flush = Z_FINISH;
     423
     424  ret = deflate(&strm, flush); /* no bad return value */
     425  if (ret == Z_STREAM_ERROR) {
     426    free(*out);
     427    return ret;
     428  }
     429
     430  have = COMPRESS_CHUNK - strm.avail_out;
     431  *out_size = have;
     432  // printf("%i -> %i\n",in_size,have);
     433  /* clean up and return */
     434  (void)deflateEnd(&strm);
     435
     436  if (strm.avail_out != 0) {
     437    return Z_OK;
     438  } else {
     439    return Z_STREAM_ERROR;
     440  }
     441}
     442
     443int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
     444                             unsigned char **out, ssize_t *out_size) {
     445  int ret;
     446  // unsigned have;
     447  z_stream strm;
     448
     449  /* allocate inflate state */
     450  strm.zalloc = Z_NULL;
     451  strm.zfree = Z_NULL;
     452  strm.opaque = Z_NULL;
     453  strm.avail_in = 0;
     454  strm.next_in = Z_NULL;
     455  ret = inflateInit(&strm);
     456  if (ret != Z_OK)
     457    return ret;
     458
     459  *out = (unsigned char *)malloc(COMPRESS_CHUNK);
     460  if (!(*out))
     461    return Z_BUF_ERROR;
     462
     463  strm.avail_in = in_size;
     464  strm.next_in = in;
     465  strm.avail_out = COMPRESS_CHUNK;
     466  strm.next_out = *out;
     467
     468  ret = inflate(&strm, Z_NO_FLUSH);
     469  assert(ret != Z_STREAM_ERROR); /* state not clobbered */
     470  switch (ret) {
     471  case Z_NEED_DICT:
     472    ret = Z_DATA_ERROR; /* and fall through */
     473  case Z_DATA_ERROR:
     474  case Z_MEM_ERROR:
    492475    (void)inflateEnd(&strm);
    493     return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
    494 }
    495 
    496 bool ui_com::ConnectionLost()
    497 {
    498     return connection_lost;
    499 }
    500 
    501 void ui_com::AddSendData(const SendData *obj)
    502 {
    503     send_mutex->GetMutex();
    504 
    505     resume_time.push_back(GetTime()+(Time)obj->SendPeriod()*1000000);
    506 
    507     //on resume en meme temps tout ceux qui ont la meme periode
    508     for(size_t i=0;i<data_to_send.size();i++)
    509     {
    510         if(data_to_send.at(i)->SendPeriod()==obj->SendPeriod())
    511         {
    512             resume_time.at(resume_time.size()-1)=resume_time.at(i);
    513             break;
    514         }
    515     }
    516 
    517     data_to_send.push_back(obj);
    518 
    519     send_mutex->ReleaseMutex();
    520 }
    521 
    522 void ui_com::UpdateSendData(const SendData *obj)
    523 {
    524     //le mutex est deja pris par l'appellant
    525     size_t id,i;
    526 
    527     //on recupere l'id
    528     for(i=0;i<data_to_send.size();i++)
    529     {
    530         if(data_to_send.at(i)==obj)
    531         {
    532             id=i;
    533             break;
    534         }
    535     }
    536 
    537     //on resume en meme temps tout ceux qui ont la meme periode
    538     for(i=0;i<data_to_send.size();i++)
    539     {
    540         if(i==id) continue;
    541         if(data_to_send.at(i)->IsEnabled()==true && data_to_send.at(i)->SendPeriod()==obj->SendPeriod())
    542         {
    543             resume_time.at(id)=resume_time.at(i);
    544             break;
    545         }
    546     }
    547 
    548     //si aucun match, on planifie l'execution
    549     if(i==data_to_send.size()) resume_time.at(id)=GetTime()+(Time)obj->SendPeriod()*1000000;
    550 
    551     if(IsSuspended()==true) Resume();
    552 
    553     return;
    554 }
    555 
    556 void ui_com::UpdateDataToSendSize(void)
    557 {
    558     send_mutex->GetMutex();
    559     send_size=3;//id(1)+period(2)
    560     for(size_t i=0;i<data_to_send.size();i++)
    561     {
    562         if(data_to_send[i]!=NULL) send_size+=data_to_send[i]->SendSize();
    563     }
    564 
    565     //send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
    566     if(send_buffer!=NULL) free(send_buffer);
    567     send_buffer=(char*)malloc(send_size*sizeof(char));
    568     send_mutex->ReleaseMutex();
    569 }
    570 
    571 void ui_com::RemoveSendData(const SendData *obj)
    572 {
    573     //printf("remove_data_to_send %i\n",data_to_send.size());
    574 
    575     send_mutex->GetMutex();
    576     //on recupere l'id
    577     for(size_t i=0;i<data_to_send.size();i++)
    578     {
    579         if(data_to_send.at(i)==obj)
    580         {
    581             data_to_send.erase (data_to_send.begin()+i);
    582             resume_time.erase (resume_time.begin()+i);
    583             //printf("remove_data_to_send %i ok\n",data_to_send.size());
    584             break;
    585         }
    586     }
    587     send_mutex->ReleaseMutex();
    588 
    589     return;
    590 }
    591 
    592 void ui_com::Block(void)
    593 {
    594     send_mutex->GetMutex();
    595 }
    596 
    597 void ui_com::UnBlock(void)
    598 {
    599     send_mutex->ReleaseMutex();
    600 }
     476    return ret;
     477  }
     478  // have = COMPRESS_CHUNK - strm.avail_out;
     479
     480  /* clean up and return */
     481  (void)inflateEnd(&strm);
     482  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
     483}
     484
     485bool ui_com::ConnectionLost() { return connection_lost; }
     486
     487void ui_com::AddSendData(const SendData *obj) {
     488  send_mutex->GetMutex();
     489
     490  resume_time.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);
     491
     492  // on resume en meme temps tout ceux qui ont la meme periode
     493  for (size_t i = 0; i < data_to_send.size(); i++) {
     494    if (data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
     495      resume_time.at(resume_time.size() - 1) = resume_time.at(i);
     496      break;
     497    }
     498  }
     499
     500  data_to_send.push_back(obj);
     501
     502  send_mutex->ReleaseMutex();
     503}
     504
     505void ui_com::UpdateSendData(const SendData *obj) {
     506  // le mutex est deja pris par l'appellant
     507  size_t id, i;
     508
     509  // on recupere l'id
     510  for (i = 0; i < data_to_send.size(); i++) {
     511    if (data_to_send.at(i) == obj) {
     512      id = i;
     513      break;
     514    }
     515  }
     516
     517  // on resume en meme temps tout ceux qui ont la meme periode
     518  for (i = 0; i < data_to_send.size(); i++) {
     519    if (i == id)
     520      continue;
     521    if (data_to_send.at(i)->IsEnabled() == true &&
     522        data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
     523      resume_time.at(id) = resume_time.at(i);
     524      break;
     525    }
     526  }
     527
     528  // si aucun match, on planifie l'execution
     529  if (i == data_to_send.size())
     530    resume_time.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;
     531
     532  if (IsSuspended() == true)
     533    Resume();
     534
     535  return;
     536}
     537
     538void ui_com::UpdateDataToSendSize(void) {
     539  send_mutex->GetMutex();
     540  send_size = 3; // id(1)+period(2)
     541  for (size_t i = 0; i < data_to_send.size(); i++) {
     542    if (data_to_send[i] != NULL)
     543      send_size += data_to_send[i]->SendSize();
     544  }
     545
     546  // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
     547  if (send_buffer != NULL)
     548    free(send_buffer);
     549  send_buffer = (char *)malloc(send_size * sizeof(char));
     550  send_mutex->ReleaseMutex();
     551}
     552
     553void ui_com::RemoveSendData(const SendData *obj) {
     554  // printf("remove_data_to_send %i\n",data_to_send.size());
     555
     556  send_mutex->GetMutex();
     557  // on recupere l'id
     558  for (size_t i = 0; i < data_to_send.size(); i++) {
     559    if (data_to_send.at(i) == obj) {
     560      data_to_send.erase(data_to_send.begin() + i);
     561      resume_time.erase(resume_time.begin() + i);
     562      // printf("remove_data_to_send %i ok\n",data_to_send.size());
     563      break;
     564    }
     565  }
     566  send_mutex->ReleaseMutex();
     567
     568  return;
     569}
     570
     571void ui_com::Block(void) { send_mutex->GetMutex(); }
     572
     573void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }

  • trunk/lib/FlairCore/src/unexported/ConditionVariable_impl.h

    r2 r15  
    2323
    2424namespace flair {
    25     namespace core {
    26         class ConditionVariable;
    27     }
     25namespace core {
     26class ConditionVariable;
     27}
    2828}
    2929
    … …  
    3232*
    3333*/
    34 class ConditionVariable_impl
    35 {
     34class ConditionVariable_impl {
    3635
    37     public:
    38         ConditionVariable_impl(flair::core::ConditionVariable* self);
    39         ~ConditionVariable_impl();
    40         void CondWait(void);
    41         bool CondWaitUntil(flair::core::Time date);
    42         void CondSignal(void);
     36public:
     37  ConditionVariable_impl(flair::core::ConditionVariable *self);
     38  ~ConditionVariable_impl();
     39  void CondWait(void);
     40  bool CondWaitUntil(flair::core::Time date);
     41  void CondSignal(void);
    4342
    44     private:
    45         flair::core::ConditionVariable* self;
     43private:
     44  flair::core::ConditionVariable *self;
    4645#ifdef __XENO__
    47         RT_COND m_ResumeCond;
     46  RT_COND m_ResumeCond;
    4847#else
    49         pthread_cond_t m_ResumeCond;
     48  pthread_cond_t m_ResumeCond;
    5049#endif
    5150};

  • trunk/lib/FlairCore/src/unexported/DataPlot_impl.h

    r2 r15  
    1616#include <vector>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class IODataElement;
    23     }
     18namespace flair {
     19namespace core {
     20class IODataElement;
     21}
    2422}
    2523
    … …  
    2826*
    2927*/
    30 class DataPlot_impl
    31 {
    32     public:
    33         DataPlot_impl();
    34         ~DataPlot_impl();
     28class DataPlot_impl {
     29public:
     30  DataPlot_impl();
     31  ~DataPlot_impl();
    3532
    36         std::vector<const flair::core::IODataElement*> tosend;
     33  std::vector<const flair::core::IODataElement *> tosend;
    3734};
    3835

  • trunk/lib/FlairCore/src/unexported/FrameworkManager_impl.h

    r2 r15  
    2424#endif
    2525
    26 namespace flair
    27 {
    28     namespace core
    29     {
    30         class FrameworkManager;
    31         class IODevice;
    32         class Watchdog;
    33     }
    34     namespace gui
    35     {
    36         class TabWidget;
    37         class PushButton;
    38         class Layout;
    39     }
     26namespace flair {
     27namespace core {
     28class FrameworkManager;
     29class IODevice;
     30class Watchdog;
     31}
     32namespace gui {
     33class TabWidget;
     34class PushButton;
     35class Layout;
     36}
    4037}
    4138
    4239class ui_com;
    4340
    44 class FrameworkManager_impl: public flair::core::Thread
    45 {
    46     public:
    47         FrameworkManager_impl(flair::core::FrameworkManager *self,std::string name);
    48         ~FrameworkManager_impl();
    49         void SetupConnection(std::string address,uint16_t port,size_t rcv_buf_size=10000);
    50         void SetupUserInterface(std::string xml_file);
    51         void SetupLogger(std::string log_path);
    52         void AddDeviceToLog(flair::core::IODevice *device);
    53         void StartLog();
    54         void StopLog();
    55         char* GetBuffer(size_t sz);
    56         void ReleaseBuffer(char* buf);
    57         void WriteLog(const char* buf,size_t size);
     41class FrameworkManager_impl : public flair::core::Thread {
     42public:
     43  FrameworkManager_impl(flair::core::FrameworkManager *self, std::string name);
     44  ~FrameworkManager_impl();
     45  void SetupConnection(std::string address, uint16_t port,
     46                       size_t rcv_buf_size = 10000);
     47  void SetupUserInterface(std::string xml_file);
     48  void SetupLogger(std::string log_path);
     49  void AddDeviceToLog(flair::core::IODevice *device);
     50  void StartLog();
     51  void StopLog();
     52  char *GetBuffer(size_t sz);
     53  void ReleaseBuffer(char *buf);
     54  void WriteLog(const char *buf, size_t size);
    5855
    59         /*!
    60         * \brief Affiche le xml
    61         *
    62         * Pour debug.
    63         */
    64         void PrintXml(void) const;
     56  /*!
     57  * \brief Affiche le xml
     58  *
     59  * Pour debug.
     60  */
     61  void PrintXml(void) const;
    6562
    66         bool is_logging;
    67         bool disable_errors;
    68         bool connection_lost;
    69         static ui_com *com;
    70         static FrameworkManager_impl *_this;
    71         std::string log_path;
     63  bool is_logging;
     64  bool disable_errors;
     65  bool connection_lost;
     66  static ui_com *com;
     67  static FrameworkManager_impl *_this;
     68  std::string log_path;
    7269
    73         flair::gui::TabWidget* tabwidget;
    74         flair::gui::PushButton *save_button;//,*load_button;
     70  flair::gui::TabWidget *tabwidget;
     71  flair::gui::PushButton *save_button; //,*load_button;
    7572
    76         xmlDocPtr file_doc;
     73  xmlDocPtr file_doc;
    7774
    78         typedef struct {
    79             const flair::core::IODevice* device;
    80             size_t size;
    81             flair::core::Time time;
    82         } log_header_t;
     75  typedef struct {
     76    const flair::core::IODevice *device;
     77    size_t size;
     78    flair::core::Time time;
     79  } log_header_t;
    8380
    84     private:
    85         flair::core::FrameworkManager *self;
    86         UDTSOCKET file_sock,com_sock;
    87         UDTSOCKET GetSocket(std::string address,uint16_t port);
    88         void Run();
    89         void SendFile(std::string path,std::string name);
    90         void FinishSending(void);
    91         std::string FileName(flair::core::IODevice* device);
    92         void SaveXmlChange(char* buf);
    93         void SaveXml(void);
    94         size_t rcv_buf_size;
    95         char *rcv_buf;
     81private:
     82  flair::core::FrameworkManager *self;
     83  UDTSOCKET file_sock, com_sock;
     84  UDTSOCKET GetSocket(std::string address, uint16_t port);
     85  void Run();
     86  void SendFile(std::string path, std::string name);
     87  void FinishSending(void);
     88  std::string FileName(flair::core::IODevice *device);
     89  void SaveXmlChange(char *buf);
     90  void SaveXml(void);
     91  size_t rcv_buf_size;
     92  char *rcv_buf;
    9693#ifdef __XENO__
    97         int CreatePipe(RT_PIPE* fd,std::string name);
    98         int DeletePipe(RT_PIPE* fd);
    99         RT_PIPE cmd_pipe;
    100         RT_PIPE data_pipe;
    101         RT_HEAP log_heap;
     94  int CreatePipe(RT_PIPE *fd, std::string name);
     95  int DeletePipe(RT_PIPE *fd);
     96  RT_PIPE cmd_pipe;
     97  RT_PIPE data_pipe;
     98  RT_HEAP log_heap;
    10299#else
    103         int CreatePipe(int (*fd)[2],std::string name);
    104         int DeletePipe(int (*fd)[2]);
    105         int cmd_pipe[2];
    106         int data_pipe[2];
     100  int CreatePipe(int (*fd)[2], std::string name);
     101  int DeletePipe(int (*fd)[2]);
     102  int cmd_pipe[2];
     103  int data_pipe[2];
    107104#endif
    108         //logger
    109         bool continuer;//a enlever, avoir un seul bool pour toutes les taches
    110         static void* write_log_user(void * arg);
    111         pthread_t log_th;
    112         std::string xml_file;
    113         bool logger_defined;
    114         bool ui_defined;
    115         flair::gui::Layout* top_layout;
     105  // logger
     106  bool continuer; // a enlever, avoir un seul bool pour toutes les taches
     107  static void *write_log_user(void *arg);
     108  pthread_t log_th;
     109  std::string xml_file;
     110  bool logger_defined;
     111  bool ui_defined;
     112  flair::gui::Layout *top_layout;
    116113
    117         typedef struct {
    118             flair::core::IODevice* device;
    119             size_t size;
    120             hdfile_t *dbtFile;
    121             bool running;
    122         } log_desc_t;
     114  typedef struct {
     115    flair::core::IODevice *device;
     116    size_t size;
     117    hdfile_t *dbtFile;
     118    bool running;
     119  } log_desc_t;
    123120
    124         std::vector<log_desc_t> logs;
    125         std::vector<std::string> xml_changes;
    126         flair::core::Watchdog* gcs_watchdog;
    127         void ConnectionLost(void);
     121  std::vector<log_desc_t> logs;
     122  std::vector<std::string> xml_changes;
     123  flair::core::Watchdog *gcs_watchdog;
     124  void ConnectionLost(void);
    128125};
    129126
    130 namespace
    131 {
    132     inline ui_com* getUiCom(void) {
    133         return FrameworkManager_impl::com;
    134     }
     127namespace {
     128inline ui_com *getUiCom(void) { return FrameworkManager_impl::com; }
    135129
    136     inline  FrameworkManager_impl* getFrameworkManagerImpl(void) {
    137         return FrameworkManager_impl::_this;
    138     }
     130inline FrameworkManager_impl *getFrameworkManagerImpl(void) {
     131  return FrameworkManager_impl::_this;
     132}
    139133}
    140134

  • trunk/lib/FlairCore/src/unexported/IODevice_impl.h

    r2 r15  
    1717#include <Object.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class io_data;
    24         class IODevice;
    25         class Thread;
    26         class Mutex;
    27         class FrameworkManager;
    28         class SharedMem;
    29     }
     19namespace flair {
     20namespace core {
     21class io_data;
     22class IODevice;
     23class Thread;
     24class Mutex;
     25class FrameworkManager;
     26class SharedMem;
     27}
    3028}
    3129
    3230class FrameworkManager_impl;
    3331
    34 class IODevice_impl
    35 {
    36     public:
    37         IODevice_impl(const flair::core::IODevice* self);
    38         ~IODevice_impl();
    39         void ResumeThread(void);
    40         size_t LogSize(void) const;
    41         void AppendLog(char** ptr);
    42         void AddDataToLog(const flair::core::io_data* data);
    43         void WriteLogsDescriptors(std::fstream& desc_file,int *index);
    44         int SetToWake(const flair::core::Thread* thread);
    45         void WriteLog(flair::core::Time time);
    46         void AddDeviceToLog(const flair::core::IODevice* device);
    47         bool SetToBeLogged(void);//return true if possible
    48         void OutputToShMem(bool enabled);
    49         void WriteToShMem(void);
    50         void PrintLogsDescriptors(void);
     32class IODevice_impl {
     33public:
     34  IODevice_impl(const flair::core::IODevice *self);
     35  ~IODevice_impl();
     36  void ResumeThread(void);
     37  size_t LogSize(void) const;
     38  void AppendLog(char **ptr);
     39  void AddDataToLog(const flair::core::io_data *data);
     40  void WriteLogsDescriptors(std::fstream &desc_file, int *index);
     41  int SetToWake(const flair::core::Thread *thread);
     42  void WriteLog(flair::core::Time time);
     43  void AddDeviceToLog(const flair::core::IODevice *device);
     44  bool SetToBeLogged(void); // return true if possible
     45  void OutputToShMem(bool enabled);
     46  void WriteToShMem(void);
     47  void PrintLogsDescriptors(void);
    5148
    52     private:
    53         std::vector<const flair::core::IODevice*> devicesToLog;
    54         const flair::core::IODevice* self;
    55         const flair::core::FrameworkManager* framework;
    56         FrameworkManager_impl* framework_impl;
    57         std::vector<const flair::core::io_data*> datasToLog;
    58         flair::core::Thread* thread_to_wake;
    59         flair::core::Mutex* wake_mutex;
    60         bool tobelogged;
    61         bool outputtoshm;
    62         flair::core::SharedMem *shmem;
     49private:
     50  std::vector<const flair::core::IODevice *> devicesToLog;
     51  const flair::core::IODevice *self;
     52  const flair::core::FrameworkManager *framework;
     53  FrameworkManager_impl *framework_impl;
     54  std::vector<const flair::core::io_data *> datasToLog;
     55  flair::core::Thread *thread_to_wake;
     56  flair::core::Mutex *wake_mutex;
     57  bool tobelogged;
     58  bool outputtoshm;
     59  flair::core::SharedMem *shmem;
    6360};
    6461

  • trunk/lib/FlairCore/src/unexported/Mutex_impl.h

    r2 r15  
    2020#endif
    2121
    22 namespace flair
    23 {
    24     namespace core
    25     {
    26         class Mutex;
    27     }
     22namespace flair {
     23namespace core {
     24class Mutex;
     25}
    2826}
    2927
    30 class Mutex_impl
    31 {
    32     public:
    33         Mutex_impl(flair::core::Mutex *self);
    34         ~Mutex_impl();
    35         void GetMutex(void);
    36         void ReleaseMutex(void);
     28class Mutex_impl {
     29public:
     30  Mutex_impl(flair::core::Mutex *self);
     31  ~Mutex_impl();
     32  void GetMutex(void);
     33  void ReleaseMutex(void);
    3734//        bool IsLocked(void);
    3835#ifdef __XENO__
    39         RT_MUTEX mutex;
     36  RT_MUTEX mutex;
    4037#else
    41         pthread_mutex_t mutex;
    42         //bool flag_locked;
     38  pthread_mutex_t mutex;
     39// bool flag_locked;
    4340#endif
    4441
    45     private:
    46         flair::core::Mutex *self;
    47 
     42private:
     43  flair::core::Mutex *self;
    4844};
    4945

  • trunk/lib/FlairCore/src/unexported/Object_impl.h

    r2 r15  
    1919#define OBJECT_IMPL_H
    2020
    21 class Object_impl
    22 {
    23     public:
    24         Object_impl(const flair::core::Object* self,const flair::core::Object* parent=NULL,std::string name="",std::string type="");
    25         ~Object_impl();
    26         std::string name,type;
    27         std::vector<const flair::core::Object*> childs;
    28         std::vector<const flair::core::Object*> type_childs;
    29         void AddChild(const flair::core::Object* child);
    30         void RemoveChild(const flair::core::Object* child);
    31         bool ErrorOccured(bool recursive);
    32         bool error_occured;
    33         const flair::core::Object* parent;
     21class Object_impl {
     22public:
     23  Object_impl(const flair::core::Object *self,
     24              const flair::core::Object *parent = NULL, std::string name = "",
     25              std::string type = "");
     26  ~Object_impl();
     27  std::string name, type;
     28  std::vector<const flair::core::Object *> childs;
     29  std::vector<const flair::core::Object *> type_childs;
     30  void AddChild(const flair::core::Object *child);
     31  void RemoveChild(const flair::core::Object *child);
     32  bool ErrorOccured(bool recursive);
     33  bool error_occured;
     34  const flair::core::Object *parent;
    3435
    35     private:
    36         const flair::core::Object* self;
     36private:
     37  const flair::core::Object *self;
    3738};
    3839

  • trunk/lib/FlairCore/src/unexported/OneAxisRotation_impl.h

    r2 r15  
    1414#define ONEAXISROTATION_IMPL_H
    1515
    16 namespace flair
    17 {
    18     namespace core
    19     {
    20         class Vector3D;
    21         class Euler;
    22         class Quaternion;
    23         class RotationMatrix;
    24     }
    25     namespace gui
    26     {
    27         class GroupBox;
    28         class ComboBox;
    29         class DoubleSpinBox;
    30     }
     16namespace flair {
     17namespace core {
     18class Vector3D;
     19class Euler;
     20class Quaternion;
     21class RotationMatrix;
     22}
     23namespace gui {
     24class GroupBox;
     25class ComboBox;
     26class DoubleSpinBox;
     27}
    3128}
    3229
    … …  
    3532*
    3633*/
    37 class OneAxisRotation_impl
    38 {
    39     public:
    40         OneAxisRotation_impl(flair::gui::GroupBox* box);
    41         ~OneAxisRotation_impl();
    42         void ComputeRotation(flair::core::Vector3D& point) const;
    43         void ComputeRotation(flair::core::Quaternion& quat) const;
    44         void ComputeRotation(flair::core::RotationMatrix& matrix) const;
    45         void ComputeRotation(flair::core::Euler& euler) const;
     34class OneAxisRotation_impl {
     35public:
     36  OneAxisRotation_impl(flair::gui::GroupBox *box);
     37  ~OneAxisRotation_impl();
     38  void ComputeRotation(flair::core::Vector3D &point) const;
     39  void ComputeRotation(flair::core::Quaternion &quat) const;
     40  void ComputeRotation(flair::core::RotationMatrix &matrix) const;
     41  void ComputeRotation(flair::core::Euler &euler) const;
    4642
    47     private:
    48         flair::gui::ComboBox *rot_axe;
    49         flair::gui::DoubleSpinBox* rot_value;
     43private:
     44  flair::gui::ComboBox *rot_axe;
     45  flair::gui::DoubleSpinBox *rot_value;
    5046};
    5147

  • trunk/lib/FlairCore/src/unexported/SendData_impl.h

    r2 r15  
    2323*
    2424*/
    25 class SendData_impl
    26 {
    27     public:
    28                 SendData_impl();
    29                 ~SendData_impl();
    30         bool is_enabled;
    31         size_t send_size;
    32         uint16_t send_period;
     25class SendData_impl {
     26public:
     27  SendData_impl();
     28  ~SendData_impl();
     29  bool is_enabled;
     30  size_t send_size;
     31  uint16_t send_period;
    3332};
    3433

  • trunk/lib/FlairCore/src/unexported/SharedMem_impl.h

    r2 r15  
    2323#endif
    2424
    25 namespace flair
    26 {
    27     namespace core
    28     {
    29         class SharedMem;
    30     }
     25namespace flair {
     26namespace core {
     27class SharedMem;
     28}
    3129}
    3230
    … …  
    3634*/
    3735
    38 class SharedMem_impl
    39 {
    40     public:
    41         SharedMem_impl(const flair::core::SharedMem* self,std::string name,size_t size);
    42         ~SharedMem_impl();
     36class SharedMem_impl {
     37public:
     38  SharedMem_impl(const flair::core::SharedMem *self, std::string name,
     39                 size_t size);
     40  ~SharedMem_impl();
    4341
    44         void Write(const char* buf,size_t size);
    45         void Read(char* buf,size_t size);
     42  void Write(const char *buf, size_t size);
     43  void Read(char *buf, size_t size);
    4644
    47     private:
    48         const flair::core::SharedMem* self;
    49         size_t size;
    50         char *mem_segment;
     45private:
     46  const flair::core::SharedMem *self;
     47  size_t size;
     48  char *mem_segment;
    5149#ifdef __XENO__
    52         RT_HEAP heap;
    53         RT_MUTEX mutex;
    54         bool heap_binded;
    55         bool mutex_binded;
     50  RT_HEAP heap;
     51  RT_MUTEX mutex;
     52  bool heap_binded;
     53  bool mutex_binded;
    5654#else
    57         int fd;
    58         sem_t *sem;
    59         std::string sem_name,shm_name;
     55  int fd;
     56  sem_t *sem;
     57  std::string sem_name, shm_name;
    6058#endif
    6159};

  • trunk/lib/FlairCore/src/unexported/Socket_impl.h

    r2 r15  
    2121#endif
    2222
    23 namespace flair
    24 {
    25     namespace core
    26     {
    27         class Socket;
    28     }
     23namespace flair {
     24namespace core {
     25class Socket;
     26}
    2927}
    3028
    31 class Socket_impl
    32 {
    33     public:
    34         Socket_impl(const flair::core::Socket* self, std::string name,std::string address,bool broadcast=false);
    35         Socket_impl(const flair::core::Socket* self, std::string name,uint16_t port);
    36         ~Socket_impl();
     29class Socket_impl {
     30public:
     31  Socket_impl(const flair::core::Socket *self, std::string name,
     32              std::string address, bool broadcast = false);
     33  Socket_impl(const flair::core::Socket *self, std::string name, uint16_t port);
     34  ~Socket_impl();
    3735
    38         void SendMessage(std::string message);
    39         void SendMessage(const char* src,size_t src_len);
    40         ssize_t RecvMessage(char* msg,size_t msg_len,flair::core::Time timeout,char* src=NULL,size_t* src_len=NULL);
     36  void SendMessage(std::string message);
     37  void SendMessage(const char *src, size_t src_len);
     38  ssize_t RecvMessage(char *msg, size_t msg_len, flair::core::Time timeout,
     39                      char *src = NULL, size_t *src_len = NULL);
    4140
    42     private:
    43         int fd;
    44         uint16_t port;
    45         std::string address;
    46         bool broadcast;
    47         void Init(void);
    48         const flair::core::Socket* self;
    49         struct sockaddr_in sock_in;
     41private:
     42  int fd;
     43  uint16_t port;
     44  std::string address;
     45  bool broadcast;
     46  void Init(void);
     47  const flair::core::Socket *self;
     48  struct sockaddr_in sock_in;
    5049#ifdef __XENO__
    51         bool is_running;
    52         static void* user(void * arg);
    53         pthread_t user_thread;
    54         RT_PIPE pipe;
     50  bool is_running;
     51  static void *user(void *arg);
     52  pthread_t user_thread;
     53  RT_PIPE pipe;
    5554#endif
    5655};

  • trunk/lib/FlairCore/src/unexported/Thread_impl.h

    r2 r15  
    2222#endif
    2323
    24 namespace flair
    25 {
    26     namespace core
    27     {
    28         class Thread;
    29         class IODevice;
    30         class ConditionVariable;
    31     }
     24namespace flair {
     25namespace core {
     26class Thread;
     27class IODevice;
     28class ConditionVariable;
     29}
    3230}
    3331
    34 class Thread_impl
    35 {
    36     public:
    37         Thread_impl(flair::core::Thread* self,uint8_t priority);
    38         ~Thread_impl();
    39         void Start(void);
    40         void Join(void);
    41         void SafeStop(void);
    42         bool ToBeStopped(void);
    43         void SetPeriodUS(uint32_t period);
    44         uint32_t GetPeriodUS(void) const;
    45         void SetPeriodMS(uint32_t period);
    46         uint32_t GetPeriodMS(void) const;
    47         void WaitPeriod(void);
    48         void Suspend(void);
    49         bool SuspendUntil (flair::core::Time date);
    50         void Resume(void);
    51         bool IsSuspended(void);
    52         int WaitUpdate(const flair::core::IODevice* device);
    53         bool period_set;
     32class Thread_impl {
     33public:
     34  Thread_impl(flair::core::Thread *self, uint8_t priority);
     35  ~Thread_impl();
     36  void Start(void);
     37  void Join(void);
     38  void SafeStop(void);
     39  bool ToBeStopped(void);
     40  void SetPeriodUS(uint32_t period);
     41  uint32_t GetPeriodUS(void) const;
     42  void SetPeriodMS(uint32_t period);
     43  uint32_t GetPeriodMS(void) const;
     44  void WaitPeriod(void);
     45  void Suspend(void);
     46  bool SuspendUntil(flair::core::Time date);
     47  void Resume(void);
     48  bool IsSuspended(void);
     49  int WaitUpdate(const flair::core::IODevice *device);
     50  bool period_set;
    5451
    55     private:
    56         flair::core::Thread* self;
    57         flair::core::ConditionVariable* cond;
    58         uint8_t priority;
    59         flair::core::Time max_jitter,min_jitter,mean_jitter;
    60         flair::core::Time last;
    61         uint64_t cpt;
    62         flair::core::Time period;
    63         bool isRunning;
    64         bool tobestopped;
    65         bool is_suspended;
    66         void PrintStats(void);
    67         void ComputeJitter(flair::core::Time time);
     52private:
     53  flair::core::Thread *self;
     54  flair::core::ConditionVariable *cond;
     55  uint8_t priority;
     56  flair::core::Time max_jitter, min_jitter, mean_jitter;
     57  flair::core::Time last;
     58  uint64_t cpt;
     59  flair::core::Time period;
     60  bool isRunning;
     61  bool tobestopped;
     62  bool is_suspended;
     63  void PrintStats(void);
     64  void ComputeJitter(flair::core::Time time);
    6865#ifdef __XENO__
    69         RT_TASK task_rt;
    70         static void main_rt(void * arg);
     66  RT_TASK task_rt;
     67  static void main_rt(void *arg);
    7168#else
    72         pthread_t task_nrt;
    73         static void* main_nrt(void * arg);
    74         flair::core::Time next_time;
     69  pthread_t task_nrt;
     70  static void *main_nrt(void *arg);
     71  flair::core::Time next_time;
    7572#endif
    7673};

  • trunk/lib/FlairCore/src/unexported/Widget_impl.h

    r2 r15  
    1717#include <io_data.h>
    1818
    19 namespace flair
    20 {
    21     namespace gui
    22     {
    23         class Widget;
    24     }
     19namespace flair {
     20namespace gui {
     21class Widget;
     22}
    2523}
    2624
    … …  
    2826* \brief Classe representant un Widget
    2927*
    30 *  C'est une classe de base. Tout comme l'Object elle permet de gérer les liens de parenté
    31 *  et de détruire automatiquement les enfants. Elle permet en plus de gérer une communication
     28*  C'est une classe de base. Tout comme l'Object elle permet de gérer les liens
     29*de parenté
     30*  et de détruire automatiquement les enfants. Elle permet en plus de gérer une
     31*communication
    3232*  avec la station sol, et donc d'y afficher un QWidget. \n
    33 *  La comunication avec la station sol se fait par l'échange de fichiers xml. Les propriétés xml du Widget sont
     33*  La comunication avec la station sol se fait par l'échange de fichiers xml.
     34*Les propriétés xml du Widget sont
    3435*  modifiables par les fonctions appropriées. \n
    35 *  Un fichier xml de réglages par défaut du Widget est utilisé s'il a été spécifié à la construction du FrameworkManager.
     36*  Un fichier xml de réglages par défaut du Widget est utilisé s'il a été
     37*spécifié à la construction du FrameworkManager.
    3638*/
    37 class Widget_impl
    38 {
    39     friend class flair::core::FrameworkManager;
    40     friend class FrameworkManager_impl;
     39class Widget_impl {
     40  friend class flair::core::FrameworkManager;
     41  friend class FrameworkManager_impl;
    4142
    42     public:
    43         /*!
    44         * \brief Constructeur
    45         *
    46         * Construit un Widget, qui est automatiquement enfant du parent. Le fichier xml
    47         * spécifié au constructeur du FrameworkManager est utilisé pour les réglages par
    48         * défaut. \n
    49         * Sauf pour le FrameworkManager, ce constructeur doit être apellé depuis une tache temps réel
    50         * lorsque l'on utilise la librairie framework_rt.
    51         *
    52         * \param parent parent
    53         * \param name nom
    54         * \param type type
    55         */
    56         Widget_impl(flair::gui::Widget* self,const flair::gui::Widget* parent,std::string name,std::string type);
     43public:
     44  /*!
     45  * \brief Constructeur
     46  *
     47  * Construit un Widget, qui est automatiquement enfant du parent. Le fichier
     48  *xml
     49  * spécifié au constructeur du FrameworkManager est utilisé pour les réglages
     50  *par
     51  * défaut. \n
     52  * Sauf pour le FrameworkManager, ce constructeur doit être apellé depuis une
     53  *tache temps réel
     54  * lorsque l'on utilise la librairie framework_rt.
     55  *
     56  * \param parent parent
     57  * \param name nom
     58  * \param type type
     59  */
     60  Widget_impl(flair::gui::Widget *self, const flair::gui::Widget *parent,
     61              std::string name, std::string type);
    5762
    58         /*!
    59         * \brief Déstructeur
    60         *
    61         * Détruit automatiquement les enfants.
    62         * La destruction implique la destruction du QWidget associé sur la station sol.\n
    63         * Sauf pour le FrameworkManager, ce déstructeur doit être apellé depuis une tache temps réel
    64         * lorsque l'on utilise la librairie framework_rt.
    65         *
    66         */
    67         ~Widget_impl();
     63  /*!
     64  * \brief Déstructeur
     65  *
     66  * Détruit automatiquement les enfants.
     67  * La destruction implique la destruction du QWidget associé sur la station
     68  *sol.\n
     69  * Sauf pour le FrameworkManager, ce déstructeur doit être apellé depuis une
     70  *tache temps réel
     71  * lorsque l'on utilise la librairie framework_rt.
     72  *
     73  */
     74  ~Widget_impl();
    6875
    69         /*!
    70         * \brief Activer
    71         *
    72         * Active le QWidget associé sur la station sol. \n
    73         * Un QWdiget désactivé apparait grisé et n'est pas modifiable.
    74         *
    75         * \param status
    76         */
    77         void setEnabled(bool status);
     76  /*!
     77  * \brief Activer
     78  *
     79  * Active le QWidget associé sur la station sol. \n
     80  * Un QWdiget désactivé apparait grisé et n'est pas modifiable.
     81  *
     82  * \param status
     83  */
     84  void setEnabled(bool status);
    7885
    79         /*!
    80         * \brief Envoi le xml
    81         *
    82         * Envoi le xml à la station sol pour prendre en compte les changements.
    83         */
    84         void SendXml(void);
     86  /*!
     87  * \brief Envoi le xml
     88  *
     89  * Envoi le xml à la station sol pour prendre en compte les changements.
     90  */
     91  void SendXml(void);
    8592
    86         xmlNodePtr file_node;
    87         xmlNodePtr send_node;
    88         bool isenabled;
     93  xmlNodePtr file_node;
     94  xmlNodePtr send_node;
     95  bool isenabled;
    8996
    90      private:
    91         flair::gui::Widget* self;
     97private:
     98  flair::gui::Widget *self;
    9299
    93         std::vector<flair::gui::Widget*> childs;
     100  std::vector<flair::gui::Widget *> childs;
    94101
    95         void AddChild(const flair::gui::Widget* child);
    96         void RemoveChild(const flair::gui::Widget* child);
     102  void AddChild(const flair::gui::Widget *child);
     103  void RemoveChild(const flair::gui::Widget *child);
    97104
    98          /*!
    99         * \brief Efface les proriétés xml
    100         *
    101         *  Permet d'effacer toutes les propriétés XML fixées par SetVolatileXmlProp.
    102         *  A utliser lorsque l'on a plus besoin d'utiliser ces propriétés. Utile
    103         * pour réduire la taille des fichiers XML écangés avec la station sol.
    104         */
    105         void ClearXmlProps(void);
     105  /*!
     106 * \brief Efface les proriétés xml
     107 *
     108 *  Permet d'effacer toutes les propriétés XML fixées par SetVolatileXmlProp.
     109 *  A utliser lorsque l'on a plus besoin d'utiliser ces propriétés. Utile
     110 * pour réduire la taille des fichiers XML écangés avec la station sol.
     111 */
     112  void ClearXmlProps(void);
    106113
    107         //xml
    108         void ProcessXML(xmlNode *node);
    109         xmlDocPtr CopyDoc(void);
    110         static xmlNodePtr GetNodeByProp(xmlNodePtr doc,xmlChar *type,xmlChar *prop,xmlChar *value);
    111         void printSendNode();
    112         xmlDocPtr send_doc;
     114  // xml
     115  void ProcessXML(xmlNode *node);
     116  xmlDocPtr CopyDoc(void);
     117  static xmlNodePtr GetNodeByProp(xmlNodePtr doc, xmlChar *type, xmlChar *prop,
     118                                  xmlChar *value);
     119  void printSendNode();
     120  xmlDocPtr send_doc;
    113121};
    114122

  • trunk/lib/FlairCore/src/unexported/communication.h

    r2 r15  
    66#define COMMUNICATION_H
    77
    8 
    9 //messages file socket
     8// messages file socket
    109#define FILE_INFO_LITTLE_ENDIAN 0x01
    1110#define FILE_INFO_BIG_ENDIAN 0x02
    … …  
    1918#define DATAS_BIG_ENDIAN 0xfe
    2019
    21 
    2220#endif // COMMUNICATION_H

  • trunk/lib/FlairCore/src/unexported/config.h

    r2 r15  
    1919#define CONFIG_H
    2020
    21 //stack size of nrt threads, comment it to use default value
    22 #define NRT_STACK_SIZE 1024*1024*1
     21// stack size of nrt threads, comment it to use default value
     22#define NRT_STACK_SIZE 1024 * 1024 * 1
    2323
    24 //stack size of rt threads, comment it to use default value
    25 #define RT_STACK_SIZE 1024*100
     24// stack size of rt threads, comment it to use default value
     25#define RT_STACK_SIZE 1024 * 100
    2626
    27 //rt pipe size, comment it to use system heap
    28 #define RT_PIPE_SIZE 1024*1024
     27// rt pipe size, comment it to use system heap
     28#define RT_PIPE_SIZE 1024 * 1024
    2929
    30 //nrt pipe size
    31 #define NRT_PIPE_SIZE 1024*100
     30// nrt pipe size
     31#define NRT_PIPE_SIZE 1024 * 100
    3232
    33 //rt log heap size
    34 #define LOG_HEAP 1024*100
     33// rt log heap size
     34#define LOG_HEAP 1024 * 100
    3535
    36 //xml heap size
    37 #define XML_HEAP 5*1024*1024
     36// xml heap size
     37#define XML_HEAP 5 * 1024 * 1024
    3838
    39 //nrt pipe path
     39// nrt pipe path
    4040#define NRT_PIPE_PATH "/proc/xenomai/registry/native/pipes/"
    4141
    42 //min priority for Threads
     42// min priority for Threads
    4343#define MIN_THREAD_PRIORITY 20
    4444
    45 //max priority for Threads
     45// max priority for Threads
    4646#define MAX_THREAD_PRIORITY 99
    4747
    48 //priority of the FrameworkManager task (manages udt connection)
     48// priority of the FrameworkManager task (manages udt connection)
    4949#define FRAMEWORK_TASK_PRIORITY 1
    5050
    51 //timeout in ms for select
     51// timeout in ms for select
    5252#define SELECT_TIMEOUT_MS 200
    5353
    54 //type of xml root element
     54// type of xml root element
    5555#define XML_ROOT_TYPE "root"
    5656
    57 //name of xml root element
     57// name of xml root element
    5858//#define XML_ROOT_ELEMENT "Manager"
    5959
    60 //name of main tabwidget
     60// name of main tabwidget
    6161#define XML_MAIN_TABWIDGET "Main_TabWidget"
    6262
    63 //name of app tabwidget
     63// name of app tabwidget
    6464#define XML_APP_TABWIDGET "App_TabWidget"
    6565
    66 //use compression for messages with ground station
     66// use compression for messages with ground station
    6767#define COMPRESS_FRAMES
    6868
    69 //size of buffer shunck
     69// size of buffer shunck
    7070#define COMPRESS_CHUNK 1024
    7171

  • trunk/lib/FlairCore/src/unexported/cvmatrix_impl.h

    r2 r15  
    1414#define CVMATRIX_IMPL_H
    1515
    16 
    1716#include <io_data.h>
    1817#include <cvmatrix.h>
    … …  
    2423*
    2524*/
    26 class cvmatrix_impl
    27 {
    28     public:
    29         cvmatrix_impl(flair::core::cvmatrix* self,int rows,int cols,flair::core::ScalarType const &elementDataType,int n);
    30         cvmatrix_impl(flair::core::cvmatrix* self,const flair::core::cvmatrix_descriptor *descriptor, flair::core::ScalarType const &elementDataType,int n);
    31         ~cvmatrix_impl();
     25class cvmatrix_impl {
     26public:
     27  cvmatrix_impl(flair::core::cvmatrix *self, int rows, int cols,
     28                flair::core::ScalarType const &elementDataType, int n);
     29  cvmatrix_impl(flair::core::cvmatrix *self,
     30                const flair::core::cvmatrix_descriptor *descriptor,
     31                flair::core::ScalarType const &elementDataType, int n);
     32  ~cvmatrix_impl();
    3233
    33         CvMat* mat;
    34         flair::core::ScalarType const &elementDataType;
    35         // const since if element description is modified it would be a mess in the log
    36         const flair::core::cvmatrix_descriptor *descriptor;
     34  CvMat *mat;
     35  flair::core::ScalarType const &elementDataType;
     36  // const since if element description is modified it would be a mess in the
     37  // log
     38  const flair::core::cvmatrix_descriptor *descriptor;
    3739
    38     private:
    39         flair::core::cvmatrix* self;
    40         void Init(flair::core::cvmatrix* self,int n);
     40private:
     41  flair::core::cvmatrix *self;
     42  void Init(flair::core::cvmatrix *self, int n);
    4143};
    4244

  • trunk/lib/FlairCore/src/unexported/io_data_impl.h

    r2 r15  
    1919* \brief Abstract class for data types.
    2020*
    21 * Use this class to define a custom data type. Data types ares used for logging and graphs. \n
     21* Use this class to define a custom data type. Data types ares used for logging
     22*and graphs. \n
    2223* The reimplemented class must call SetSize() in its constructor. \n
    2324* io_data can be constructed with n samples (see io_data::io_data).
    2425* In this case, old samples can be accessed throug io_data::Prev.
    2526*/
    26 class io_data_impl
    27 {
    28     public:
    29                 io_data_impl(flair::core::io_data* self,int n);
    30                 ~io_data_impl();
    31                 void Circle(void);
    32                 bool IsConsistent(void);
    33                 void SetConsistent(bool status);
    34                 void WriteLogDescriptor(std::fstream& desc_file,int *index);
    35                 void PrintLogDescriptor(void);
    36         void AppendLogDescription(std::string description,flair::core::DataType const &datatype);
    37         size_t size;
    38         flair::core::Time time;
    39         void** circle_ptr;
     27class io_data_impl {
     28public:
     29  io_data_impl(flair::core::io_data *self, int n);
     30  ~io_data_impl();
     31  void Circle(void);
     32  bool IsConsistent(void);
     33  void SetConsistent(bool status);
     34  void WriteLogDescriptor(std::fstream &desc_file, int *index);
     35  void PrintLogDescriptor(void);
     36  void AppendLogDescription(std::string description,
     37                            flair::core::DataType const &datatype);
     38  size_t size;
     39  flair::core::Time time;
     40  void **circle_ptr;
    4041
    41     private:
    42         flair::core::io_data* self;
    43         int n;
    44         bool is_consistent;
    45         std::vector<std::string> descriptors;
     42private:
     43  flair::core::io_data *self;
     44  int n;
     45  bool is_consistent;
     46  std::vector<std::string> descriptors;
    4647};
    4748

  • trunk/lib/FlairCore/src/unexported/ui_com.h

    r2 r15  
    2525#endif
    2626
    27 namespace flair
    28 {
    29     namespace core
    30     {
    31         class Mutex;
    32         class Object;
    33     }
    34     namespace gui
    35     {
    36         class SendData;
    37     }
     27namespace flair {
     28namespace core {
     29class Mutex;
     30class Object;
     31}
     32namespace gui {
     33class SendData;
     34}
    3835}
    3936
    40 class ui_com: public flair::core::Thread
    41 {
    42     public:
    43         ui_com(const flair::core::Object *parent,UDTSOCKET sock);
    44         ~ui_com();
    45         void Send(char* buf,ssize_t size);
    46         ssize_t Receive(char* buf,ssize_t buf_size);
    47         void AddSendData(const flair::gui::SendData *obj);
    48         void UpdateSendData(const flair::gui::SendData *obj);
    49         void RemoveSendData(const flair::gui::SendData *obj);
    50         void UpdateDataToSendSize(void);
    51         void Block(void);
    52         void UnBlock(void);
    53         bool ConnectionLost(void);
     37class ui_com : public flair::core::Thread {
     38public:
     39  ui_com(const flair::core::Object *parent, UDTSOCKET sock);
     40  ~ui_com();
     41  void Send(char *buf, ssize_t size);
     42  ssize_t Receive(char *buf, ssize_t buf_size);
     43  void AddSendData(const flair::gui::SendData *obj);
     44  void UpdateSendData(const flair::gui::SendData *obj);
     45  void RemoveSendData(const flair::gui::SendData *obj);
     46  void UpdateDataToSendSize(void);
     47  void Block(void);
     48  void UnBlock(void);
     49  bool ConnectionLost(void);
    5450
    55     private:
    56         ssize_t send_size;
    57         char *send_buffer;
    58         std::vector<const flair::gui::SendData*> data_to_send;
    59         std::vector<flair::core::Time> resume_time;
    60         flair::core::Mutex *send_mutex;
    61         UDTSOCKET socket_fd;
    62         bool connection_lost;
    63         void Run(void);
    64         void SendDatas(void);
    65         static int compressBuffer(char *in, ssize_t in_size,char **out,ssize_t *out_size, int level);
    66         static int uncompressBuffer(unsigned char *in, ssize_t in_size,unsigned char **out,ssize_t *out_size);
     51private:
     52  ssize_t send_size;
     53  char *send_buffer;
     54  std::vector<const flair::gui::SendData *> data_to_send;
     55  std::vector<flair::core::Time> resume_time;
     56  flair::core::Mutex *send_mutex;
     57  UDTSOCKET socket_fd;
     58  bool connection_lost;
     59  void Run(void);
     60  void SendDatas(void);
     61  static int compressBuffer(char *in, ssize_t in_size, char **out,
     62                            ssize_t *out_size, int level);
     63  static int uncompressBuffer(unsigned char *in, ssize_t in_size,
     64                              unsigned char **out, ssize_t *out_size);
    6765#ifdef __XENO__
    68         bool is_running;
    69         static void* user_thread(void * arg);
    70         pthread_t thread;
    71         RT_PIPE pipe;
     66  bool is_running;
     67  static void *user_thread(void *arg);
     68  pthread_t thread;
     69  RT_PIPE pipe;
    7270#endif
    73 
    7471};
    7572

  • 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();
    104 
    105     if(pas->CurrentIndex()==1)
    106     {
    107         trim_yaw=-trim_yaw;
    108         u_yaw=-u_yaw;
    109     }
    110 
    111 if(nb_mot==2)
    112 {
    113     //(top) front left
    114     value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)]=Set(trim_pitch+trim_roll+trim_yaw,
    115                  u_thrust+u_pitch+u_roll+u_yaw);
    116 
    117     //(top) front right
    118     value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)]=Set(trim_pitch-trim_roll-trim_yaw,
    119                  u_thrust+u_pitch-u_roll-u_yaw);
    120 }
    121     if(nb_mot==4 || nb_mot==8)
    122     {
    123         //(top) front left
    124         value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)]=Set(trim_pitch+trim_roll+trim_yaw,
    125                      u_thrust+u_pitch+u_roll+u_yaw);
    126 
    127         //(top) front right
    128         value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)]=Set(trim_pitch-trim_roll-trim_yaw,
    129                      u_thrust+u_pitch-u_roll-u_yaw);
    130 
    131         //(top) rear left
    132         value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)]=Set(-trim_pitch+trim_roll-trim_yaw,
    133                      u_thrust-u_pitch+u_roll-u_yaw);
    134 
    135         //(top) rear right
    136         value[self->MultiplexValue(X4X8Multiplex::TopRearRight)]=Set(-trim_pitch-trim_roll+trim_yaw,
    137                      u_thrust-u_pitch-u_roll+u_yaw);
    138     }
    139 
    140     if(nb_mot==8)
    141     {
    142         //bottom front left
    143         value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)]=Set(trim_pitch+trim_roll-trim_yaw,
    144                      u_thrust+u_pitch+u_roll-u_yaw);
    145 
    146         //bottom front right
    147         value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)]=Set(trim_pitch-trim_roll+trim_yaw,
    148                      u_thrust+u_pitch-u_roll+u_yaw);
    149 
    150         //bottom rear left
    151         value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)]=Set(-trim_pitch+trim_roll+trim_yaw,
    152                      u_thrust-u_pitch+u_roll+u_yaw);
    153 
    154         //bottom rear right
    155         value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)]=Set(-trim_pitch-trim_roll-trim_yaw,
    156                      u_thrust-u_pitch-u_roll-u_yaw);
    157 
    158     }
    159 
    160     //on prend une fois pour toute le mutex et on fait des accès directs
    161     output->GetMutex();
    162     for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,value[i]);
    163     output->ReleaseMutex();
    164 
    165     output->SetDataTime(data->DataTime());
    166 
    167     self->ProcessUpdate(output);
    168 }
    169 
    170 float X4X8Multiplex_impl::Set(float trim,float u)
    171 {
    172     float value=trim;
    173 
    174     if(u>0)
    175     {
    176         value+=sqrtf(u);
    177     }
    178 
    179     return value;
    180 }
    181 
    182 string X4X8Multiplex_impl::MotorName(int index)
    183 {
    184     switch(nb_mot)
    185     {
    186         case 4:
    187         {
    188             switch(index)
    189             {
    190                 case 0:
    191                     return "front left";
    192                 case 1:
    193                     return "front rigth";
    194                 case 2:
    195                     return "rear left";
    196                 case 3:
    197                     return "rear rigth";
    198                 default:
    199                     return "unammed motor";
    200             }
    201         }
    202         case 8:
    203         {
    204             switch(index)
    205             {
    206                 case 0:
    207                     return "top front left";
    208                 case 1:
    209                     return "top front rigth";
    210                 case 2:
    211                     return "top rear left";
    212                 case 3:
    213                     return "top rear rigth";
    214                 case 4:
    215                     return "bottom front left";
    216                 case 5:
    217                     return "bottom front rigth";
    218                 case 6:
    219                     return "bottom rear left";
    220                 case 7:
    221                     return "bottom rear rigth";
    222                 default:
    223                     return "unammed motor";
    224             }
    225         }
    226         default:
    227         {
    228             return "unammed motor";
    229         }
    230     }
    231 }
     212  }
     213  default: { return "unammed motor"; }
     214  }
     215}

  • trunk/lib/FlairFilter/src/unexported/Ahrs_impl.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace core {
    20         class AhrsData;
    21     }
    22     namespace gui {
    23         class Tab;
    24         class DataPlot1D;
    25     }
    26     namespace filter {
    27         class Ahrs;
    28     }
     19namespace core {
     20class AhrsData;
     21}
     22namespace gui {
     23class Tab;
     24class DataPlot1D;
     25}
     26namespace filter {
     27class Ahrs;
     28}
    2929}
    3030
    … …  
    3737
    3838class Ahrs_impl {
    39     public:
    40         Ahrs_impl(flair::filter::Ahrs* self);
    41         ~Ahrs_impl();
    42         void UseDefaultPlot(void);
    43         void AddPlot(const flair::core::AhrsData *ahrsData,flair::gui::DataPlot::Color_t color);
    44         flair::gui::DataPlot1D *rollPlot,*pitchPlot,*yawPlot;
    45         flair::gui::DataPlot1D *wXPlot,*wYPlot,*wZPlot;
    46         flair::gui::DataPlot1D *q0Plot,*q1Plot,*q2Plot,*q3Plot;
    47         flair::core::AhrsData *ahrsData;
     39public:
     40  Ahrs_impl(flair::filter::Ahrs *self);
     41  ~Ahrs_impl();
     42  void UseDefaultPlot(void);
     43  void AddPlot(const flair::core::AhrsData *ahrsData,
     44               flair::gui::DataPlot::Color_t color);
     45  flair::gui::DataPlot1D *rollPlot, *pitchPlot, *yawPlot;
     46  flair::gui::DataPlot1D *wXPlot, *wYPlot, *wZPlot;
     47  flair::gui::DataPlot1D *q0Plot, *q1Plot, *q2Plot, *q3Plot;
     48  flair::core::AhrsData *ahrsData;
    4849
    49     private:
    50         flair::gui::Tab *eulerTab,*quaternionTab;
    51         flair::filter::Ahrs* self;
     50private:
     51  flair::gui::Tab *eulerTab, *quaternionTab;
     52  flair::filter::Ahrs *self;
    5253};
    5354

  • trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h

    r10 r15  
    1717#include <Butterworth.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class cvmatrix;
    24     }
    25     namespace gui
    26     {
    27         class LayoutPosition;
    28         class SpinBox;
    29         class DoubleSpinBox;
    30     }
    31     namespace filter
    32     {
    33         class ButterworthLowPass;
    34     }
     19namespace flair {
     20namespace core {
     21class cvmatrix;
     22}
     23namespace gui {
     24class LayoutPosition;
     25class SpinBox;
     26class DoubleSpinBox;
     27}
     28namespace filter {
     29class ButterworthLowPass;
     30}
    3531}
    3632
    3733// Storage for Layout
    38 //de-templatized for pimpl idom
    39 //comes from iir from Bernd Porr
    40 class LayoutStorage
    41 {
    42     public:
    43         LayoutStorage(int MaxPoles)
    44         {
    45             this->MaxPoles=MaxPoles;
    46             m_pairs=(Iir::PoleZeroPair*)malloc((MaxPoles+1)/2*sizeof(Iir::PoleZeroPair));
    47         }
    48         ~LayoutStorage()
    49         {
    50             free(m_pairs);
    51         }
    52         operator Iir::LayoutBase ()
    53         {
    54             return Iir::LayoutBase (MaxPoles, m_pairs);
    55         }
     34// de-templatized for pimpl idom
     35// comes from iir from Bernd Porr
     36class LayoutStorage {
     37public:
     38  LayoutStorage(int MaxPoles) {
     39    this->MaxPoles = MaxPoles;
     40    m_pairs = (Iir::PoleZeroPair *)malloc((MaxPoles + 1) / 2 *
     41                                          sizeof(Iir::PoleZeroPair));
     42  }
     43  ~LayoutStorage() { free(m_pairs); }
     44  operator Iir::LayoutBase() { return Iir::LayoutBase(MaxPoles, m_pairs); }
    5645
    57     private:
    58         Iir::PoleZeroPair *m_pairs;
    59         int MaxPoles;
     46private:
     47  Iir::PoleZeroPair *m_pairs;
     48  int MaxPoles;
    6049};
    6150
    6251// Storage for Cascade
    63 //de-templatized for pimpl idom
    64 //comes from iir from Bernd Porr
    65 class CascadeStages
    66 {
    67     public:
    68         CascadeStages(int MaxStages)
    69         {
    70             this->MaxStages=MaxStages;
    71             m_stages=(Iir::Cascade::Stage*)malloc(MaxStages*sizeof(Iir::Cascade::Stage));
    72             m_states=(Iir::DirectFormII*)malloc(MaxStages*sizeof(Iir::DirectFormII));
    73         }
    74         ~CascadeStages()
    75         {
    76             free(m_stages);
    77             free(m_states);
    78         }
    79         void reset ()
    80         {
    81             Iir::DirectFormII* state = m_states;
    82             for (int i = MaxStages; --i >= 0; ++state)
    83             state->reset();
    84         }
     52// de-templatized for pimpl idom
     53// comes from iir from Bernd Porr
     54class CascadeStages {
     55public:
     56  CascadeStages(int MaxStages) {
     57    this->MaxStages = MaxStages;
     58    m_stages =
     59        (Iir::Cascade::Stage *)malloc(MaxStages * sizeof(Iir::Cascade::Stage));
     60    m_states =
     61        (Iir::DirectFormII *)malloc(MaxStages * sizeof(Iir::DirectFormII));
     62  }
     63  ~CascadeStages() {
     64    free(m_stages);
     65    free(m_states);
     66  }
     67  void reset() {
     68    Iir::DirectFormII *state = m_states;
     69    for (int i = MaxStages; --i >= 0; ++state)
     70      state->reset();
     71  }
    8572
    86         template <typename Sample>
    87         inline Sample filter(const Sample in)
    88         {
    89             double out = in;
    90             Iir::DirectFormII* state = m_states;
    91             Iir::Biquad const* stage = m_stages;
    92             for (int i = MaxStages; --i >= 0; ++state, ++stage)
    93             out = state->process1 (out, *stage);
    94             return static_cast<Sample> (out);
    95         }
     73  template <typename Sample> inline Sample filter(const Sample in) {
     74    double out = in;
     75    Iir::DirectFormII *state = m_states;
     76    Iir::Biquad const *stage = m_stages;
     77    for (int i = MaxStages; --i >= 0; ++state, ++stage)
     78      out = state->process1(out, *stage);
     79    return static_cast<Sample>(out);
     80  }
    9681
    97         Iir::Cascade::Storage getCascadeStorage()
    98         {
    99             return Iir::Cascade::Storage (MaxStages, m_stages);
    100         }
     82  Iir::Cascade::Storage getCascadeStorage() {
     83    return Iir::Cascade::Storage(MaxStages, m_stages);
     84  }
    10185
    102     private:
    103         int MaxStages;
    104         Iir::Cascade::Stage *m_stages;
    105         Iir::DirectFormII *m_states;
     86private:
     87  int MaxStages;
     88  Iir::Cascade::Stage *m_stages;
     89  Iir::DirectFormII *m_states;
    10690};
    10791
    108 //de-templatized for pimpl idom
    109 //comes from iir from Bernd Porr
    110 class PoleFilter : Iir::Butterworth::LowPassBase,public CascadeStages
    111 {
    112     public:
    113         PoleFilter (int MaxPoles):CascadeStages((MaxPoles + 1) / 2)
    114         {
    115             this->MaxPoles=MaxPoles;
    116             m_analogStorage=new LayoutStorage(MaxPoles);
    117             m_digitalStorage=new LayoutStorage(MaxPoles);
    118             // This glues together the factored base classes
    119             // with the templatized storage classes.
    120             Iir::Butterworth::LowPassBase::setCascadeStorage (this->getCascadeStorage());
    121             Iir::Butterworth::LowPassBase::setPrototypeStorage (*m_analogStorage,*m_digitalStorage);
    122         }
    123         ~PoleFilter()
    124         {
    125             delete m_analogStorage;
    126             delete m_digitalStorage;
    127         }
    128         void setup (double sampleRate,double cutoffFrequency)
    129         {
    130             Iir::Butterworth::LowPassBase::setup (MaxPoles, sampleRate,cutoffFrequency);
    131         }
     92// de-templatized for pimpl idom
     93// comes from iir from Bernd Porr
     94class PoleFilter : Iir::Butterworth::LowPassBase, public CascadeStages {
     95public:
     96  PoleFilter(int MaxPoles) : CascadeStages((MaxPoles + 1) / 2) {
     97    this->MaxPoles = MaxPoles;
     98    m_analogStorage = new LayoutStorage(MaxPoles);
     99    m_digitalStorage = new LayoutStorage(MaxPoles);
     100    // This glues together the factored base classes
     101    // with the templatized storage classes.
     102    Iir::Butterworth::LowPassBase::setCascadeStorage(this->getCascadeStorage());
     103    Iir::Butterworth::LowPassBase::setPrototypeStorage(*m_analogStorage,
     104                                                       *m_digitalStorage);
     105  }
     106  ~PoleFilter() {
     107    delete m_analogStorage;
     108    delete m_digitalStorage;
     109  }
     110  void setup(double sampleRate, double cutoffFrequency) {
     111    Iir::Butterworth::LowPassBase::setup(MaxPoles, sampleRate, cutoffFrequency);
     112  }
    132113
    133     private:
    134         int MaxPoles;
    135         LayoutStorage* m_analogStorage;
    136         LayoutStorage* m_digitalStorage;
     114private:
     115  int MaxPoles;
     116  LayoutStorage *m_analogStorage;
     117  LayoutStorage *m_digitalStorage;
    137118};
    138119
     120class ButterworthLowPass_impl {
     121public:
     122  ButterworthLowPass_impl(flair::filter::ButterworthLowPass *self,
     123                          const flair::gui::LayoutPosition *position,
     124                          std::string name, int order);
     125  ~ButterworthLowPass_impl();
     126  void UpdateFrom(const flair::core::io_data *data);
     127  flair::core::cvmatrix *output;
    139128
    140 class ButterworthLowPass_impl
    141 {
    142     public:
    143         ButterworthLowPass_impl(flair::filter::ButterworthLowPass* self,const flair::gui::LayoutPosition* position,std::string name,int order);
    144         ~ButterworthLowPass_impl();
    145         void UpdateFrom(const flair::core::io_data *data);
    146         flair::core::cvmatrix *output;
    147 
    148     private:
    149         flair::gui::DoubleSpinBox *cutoff,*T;
    150         PoleFilter* f;
    151         bool first_update;
    152         flair::core::Time previous_time;
     129private:
     130  flair::gui::DoubleSpinBox *cutoff, *T;
     131  PoleFilter *f;
     132  bool first_update;
     133  flair::core::Time previous_time;
    153134};
    154135

  • trunk/lib/FlairFilter/src/unexported/EulerDerivative_impl.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         class DoubleSpinBox;
    28     }
    29     namespace filter
    30     {
    31         class EulerDerivative;
    32     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class LayoutPosition;
     24class DoubleSpinBox;
     25}
     26namespace filter {
     27class EulerDerivative;
     28}
    3329}
    3430
    … …  
    3733*/
    3834
    39 class EulerDerivative_impl
    40 {
    41     public:
    42         EulerDerivative_impl(flair::filter::EulerDerivative* self,const flair::gui::LayoutPosition* position,std::string name,const flair::core::cvmatrix* init_value=NULL);
    43         ~EulerDerivative_impl();
    44         void UpdateFrom(const flair::core::io_data *data);
    45         flair::core::cvmatrix *output;
     35class EulerDerivative_impl {
     36public:
     37  EulerDerivative_impl(flair::filter::EulerDerivative *self,
     38                       const flair::gui::LayoutPosition *position,
     39                       std::string name,
     40                       const flair::core::cvmatrix *init_value = NULL);
     41  ~EulerDerivative_impl();
     42  void UpdateFrom(const flair::core::io_data *data);
     43  flair::core::cvmatrix *output;
    4644
    47     private:
    48         flair::gui::DoubleSpinBox* T;
    49         flair::core::Time previous_time;
    50         bool first_update;
    51         flair::core::cvmatrix* prev_value;
    52 
     45private:
     46  flair::gui::DoubleSpinBox *T;
     47  flair::core::Time previous_time;
     48  bool first_update;
     49  flair::core::cvmatrix *prev_value;
    5350};
    5451

  • trunk/lib/FlairFilter/src/unexported/JoyReference_impl.h

    r10 r15  
    2424
    2525namespace flair {
    26     namespace core {
    27         class cvmatrix;
    28         class io_data;
    29         class AhrsData;
    30     }
    31     namespace gui {
    32         class LayoutPosition;
    33         class GroupBox;
    34         class DoubleSpinBox;
    35         class SpinBox;
    36         class Label;
    37         class PushButton;
    38     }
    39     namespace filter {
    40         class JoyReference;
    41     }
     26namespace core {
     27class cvmatrix;
     28class io_data;
     29class AhrsData;
     30}
     31namespace gui {
     32class LayoutPosition;
     33class GroupBox;
     34class DoubleSpinBox;
     35class SpinBox;
     36class Label;
     37class PushButton;
     38}
     39namespace filter {
     40class JoyReference;
     41}
    4242}
    4343
    4444class JoyReference_impl {
    4545
    46     public:
    47         JoyReference_impl(flair::filter::JoyReference *self,const flair::gui::LayoutPosition* position,std::string name);
    48         ~JoyReference_impl();
    49         void SetRollAxis(float value);
    50         void SetPitchAxis(float value);
    51         void SetYawAxis(float value);
    52         void SetAltitudeAxis(float value);
    53         float ZRef(void) const;
    54         float dZRef(void) const;
    55         float RollTrim(void) const;
    56         float PitchTrim(void) const;
    57         void SetYawRef(float value);
    58         void SetZRef(float value);
    59         void RollTrimUp(void);
    60         void RollTrimDown(void);
    61         void PitchTrimUp(void);
    62         void PitchTrimDown(void);
    63         void Update(flair::core::Time time);
    64         void UpdateFrom(const flair::core::io_data *data);
    65         flair::core::cvmatrix *output;
    66         flair::core::AhrsData *ahrsData;
     46public:
     47  JoyReference_impl(flair::filter::JoyReference *self,
     48                    const flair::gui::LayoutPosition *position,
     49                    std::string name);
     50  ~JoyReference_impl();
     51  void SetRollAxis(float value);
     52  void SetPitchAxis(float value);
     53  void SetYawAxis(float value);
     54  void SetAltitudeAxis(float value);
     55  float ZRef(void) const;
     56  float dZRef(void) const;
     57  float RollTrim(void) const;
     58  float PitchTrim(void) const;
     59  void SetYawRef(float value);
     60  void SetZRef(float value);
     61  void RollTrimUp(void);
     62  void RollTrimDown(void);
     63  void PitchTrimUp(void);
     64  void PitchTrimDown(void);
     65  void Update(flair::core::Time time);
     66  void UpdateFrom(const flair::core::io_data *data);
     67  flair::core::cvmatrix *output;
     68  flair::core::AhrsData *ahrsData;
    6769
    68     private:
    69         flair::core::cvmatrix *input;
     70private:
     71  flair::core::cvmatrix *input;
    7072
    71         flair::gui::GroupBox *reglages_groupbox;
    72         flair::gui::DoubleSpinBox *deb_roll,*deb_pitch,*deb_wz,*deb_dz;
    73         flair::gui::DoubleSpinBox *trim;
    74         flair::gui::Label *label_roll,*label_pitch;
    75         flair::gui::PushButton *button_roll,*button_pitch;
     73  flair::gui::GroupBox *reglages_groupbox;
     74  flair::gui::DoubleSpinBox *deb_roll, *deb_pitch, *deb_wz, *deb_dz;
     75  flair::gui::DoubleSpinBox *trim;
     76  flair::gui::Label *label_roll, *label_pitch;
     77  flair::gui::PushButton *button_roll, *button_pitch;
    7678
    77         float z_ref;
    78         flair::core::Quaternion q_z=flair::core::Quaternion(1,0,0,0);
    79         float trim_roll,trim_pitch;
    80         flair::core::Time previous_time;
     79  float z_ref;
     80  flair::core::Quaternion q_z = flair::core::Quaternion(1, 0, 0, 0);
     81  float trim_roll, trim_pitch;
     82  flair::core::Time previous_time;
    8183
    82         flair::filter::JoyReference *self;
     84  flair::filter::JoyReference *self;
    8385};
    8486

  • trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.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         class SpinBox;
    28         class DoubleSpinBox;
    29     }
    30     namespace filter
    31     {
    32         class LowPassFilter;
    33     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21}
     22namespace gui {
     23class LayoutPosition;
     24class SpinBox;
     25class DoubleSpinBox;
     26}
     27namespace filter {
     28class LowPassFilter;
     29}
    3430}
    3531
    36 class LowPassFilter_impl
    37 {
     32class LowPassFilter_impl {
    3833
    39     public:
    40         LowPassFilter_impl(const flair::filter::LowPassFilter* self,const flair::gui::LayoutPosition* position,std::string name,const flair::core::cvmatrix* init_value=NULL);
    41         ~LowPassFilter_impl();
    42         void UpdateFrom(const flair::core::io_data *data);
    43         flair::core::cvmatrix *output;
     34public:
     35  LowPassFilter_impl(const flair::filter::LowPassFilter *self,
     36                     const flair::gui::LayoutPosition *position,
     37                     std::string name,
     38                     const flair::core::cvmatrix *init_value = NULL);
     39  ~LowPassFilter_impl();
     40  void UpdateFrom(const flair::core::io_data *data);
     41  flair::core::cvmatrix *output;
    4442
    45     private:
    46         flair::core::Time previous_time;
    47         bool first_update;
    48         flair::core::cvmatrix* prev_value;
    49         flair::gui::DoubleSpinBox *freq,*T;
     43private:
     44  flair::core::Time previous_time;
     45  bool first_update;
     46  flair::core::cvmatrix *prev_value;
     47  flair::gui::DoubleSpinBox *freq, *T;
    5048};
    5149

  • trunk/lib/FlairFilter/src/unexported/NestedSat_impl.h

    r10 r15  
    1616#include <Object.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23         class io_data;
    24     }
    25     namespace gui
    26     {
    27         class Layout;
    28         class LayoutPosition;
    29         class DoubleSpinBox;
    30         class DataPlot1D;
    31     }
    32     namespace filter
    33     {
    34         class NestedSat;
    35     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21class io_data;
     22}
     23namespace gui {
     24class Layout;
     25class LayoutPosition;
     26class DoubleSpinBox;
     27class DataPlot1D;
     28}
     29namespace filter {
     30class NestedSat;
     31}
    3632}
    3733
    … …  
    3935* \brief Classe permettant le calcul d'un Pid avec saturations
    4036*/
    41 class NestedSat_impl
    42 {
    43     public:
    44         NestedSat_impl(flair::filter::NestedSat* self,const flair::gui::LayoutPosition* position,std::string name);
    45         ~NestedSat_impl();
    46         float Value(void);
    47         void UseDefaultPlot(const flair::gui::Layout* layout,int row,int col);
    48         void UpdateFrom(const flair::core::io_data *data);
    49         void ConvertSatFromDegToRad(void);
    50         float k;
     37class NestedSat_impl {
     38public:
     39  NestedSat_impl(flair::filter::NestedSat *self,
     40                 const flair::gui::LayoutPosition *position, std::string name);
     41  ~NestedSat_impl();
     42  float Value(void);
     43  void UseDefaultPlot(const flair::gui::Layout *layout, int row, int col);
     44  void UpdateFrom(const flair::core::io_data *data);
     45  void ConvertSatFromDegToRad(void);
     46  float k;
    5147
    52     private:
    53         flair::filter::NestedSat* self;
    54         flair::gui::DoubleSpinBox *kp,*kd,*sat,*dsat,*usat;
    55         flair::gui::DataPlot1D *plot;
    56         float Sat(float value,float borne);
     48private:
     49  flair::filter::NestedSat *self;
     50  flair::gui::DoubleSpinBox *kp, *kd, *sat, *dsat, *usat;
     51  flair::gui::DataPlot1D *plot;
     52  float Sat(float value, float borne);
    5753};
    5854
    59 
    6055#endif // NESTEDSAT_IMPL_H

  • trunk/lib/FlairFilter/src/unexported/PidThrust_impl.h

    r10 r15  
    1616#include <Object.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23         class io_data;
    24     }
    25     namespace gui
    26     {
    27         class LayoutPosition;
    28         class DoubleSpinBox;
    29     }
    30     namespace filter
    31     {
    32         class PidThrust;
    33     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21class io_data;
     22}
     23namespace gui {
     24class LayoutPosition;
     25class DoubleSpinBox;
     26}
     27namespace filter {
     28class PidThrust;
     29}
    3430}
    3531
    … …  
    3834*/
    3935
    40 class PidThrust_impl
    41 {
    42     public:
    43         PidThrust_impl(flair::filter::PidThrust* self,const flair::gui::LayoutPosition* position,std::string name);
    44         ~PidThrust_impl();
    45         void UseDefaultPlot(const flair::gui::LayoutPosition* position);
    46         void UpdateFrom(const flair::core::io_data *data);
    47         float i,offset_g;
    48         flair::gui::DoubleSpinBox *offset,*pas_offset;
     36class PidThrust_impl {
     37public:
     38  PidThrust_impl(flair::filter::PidThrust *self,
     39                 const flair::gui::LayoutPosition *position, std::string name);
     40  ~PidThrust_impl();
     41  void UseDefaultPlot(const flair::gui::LayoutPosition *position);
     42  void UpdateFrom(const flair::core::io_data *data);
     43  float i, offset_g;
     44  flair::gui::DoubleSpinBox *offset, *pas_offset;
    4945
    50     private:
    51         flair::filter::PidThrust* self;
    52         flair::core::Time previous_time;
    53         bool first_update;
     46private:
     47  flair::filter::PidThrust *self;
     48  flair::core::Time previous_time;
     49  bool first_update;
    5450
    55         //matrix
    56         flair::core::cvmatrix *state;
     51  // matrix
     52  flair::core::cvmatrix *state;
    5753
    58         flair::gui::DoubleSpinBox *T,*kp,*ki,*kd,*sat,*sati;
     54  flair::gui::DoubleSpinBox *T, *kp, *ki, *kd, *sat, *sati;
    5955};
    6056

  • trunk/lib/FlairFilter/src/unexported/Pid_impl.h

    r10 r15  
    1616#include <Object.h>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23         class io_data;
    24     }
    25     namespace gui
    26     {
    27         class LayoutPosition;
    28         class DoubleSpinBox;
    29     }
    30     namespace filter
    31     {
    32         class Pid;
    33     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21class io_data;
     22}
     23namespace gui {
     24class LayoutPosition;
     25class DoubleSpinBox;
     26}
     27namespace filter {
     28class Pid;
     29}
    3430}
    3531
    … …  
    3834*/
    3935
    40 class Pid_impl
    41 {
    42     public:
    43         Pid_impl(flair::filter::Pid* self,const flair::gui::LayoutPosition* position,std::string name);
    44         ~Pid_impl();
    45         void UseDefaultPlot(const flair::gui::LayoutPosition* position);
    46         void UpdateFrom(const flair::core::io_data *data);
    47         float i;
    48         bool first_update;
     36class Pid_impl {
     37public:
     38  Pid_impl(flair::filter::Pid *self, const flair::gui::LayoutPosition *position,
     39           std::string name);
     40  ~Pid_impl();
     41  void UseDefaultPlot(const flair::gui::LayoutPosition *position);
     42  void UpdateFrom(const flair::core::io_data *data);
     43  float i;
     44  bool first_update;
    4945
    50     private:
    51         flair::filter::Pid* self;
    52         flair::core::Time previous_time;
     46private:
     47  flair::filter::Pid *self;
     48  flair::core::Time previous_time;
    5349
    54         //matrix
    55         flair::core::cvmatrix *state;
     50  // matrix
     51  flair::core::cvmatrix *state;
    5652
    57         flair::gui::DoubleSpinBox *T,*kp,*ki,*kd,*sat,*sati;
     53  flair::gui::DoubleSpinBox *T, *kp, *ki, *kd, *sat, *sati;
    5854};
    5955

  • trunk/lib/FlairFilter/src/unexported/TrajectoryGenerator1D_impl.h

    r10 r15  
    1414#define TRAJECTORYGENERATOR1D_IMPL_H
    1515
    16 namespace flair
    17 {
    18     namespace core
    19     {
    20         class cvmatrix;
    21     }
    22     namespace gui
    23     {
    24         class LayoutPosition;
    25         class DoubleSpinBox;
    26     }
     16namespace flair {
     17namespace core {
     18class cvmatrix;
     19}
     20namespace gui {
     21class LayoutPosition;
     22class DoubleSpinBox;
     23}
    2724}
    2825
    … …  
    3027* \brief Class generating a trajectory in 1D
    3128*/
    32 class TrajectoryGenerator1D_impl
    33 {
     29class TrajectoryGenerator1D_impl {
    3430
    35     public:
    36         TrajectoryGenerator1D_impl(flair::filter::TrajectoryGenerator1D* self,const flair::gui::LayoutPosition* position,std::string name,std::string unit);
    37         ~TrajectoryGenerator1D_impl();
    38         void Update(flair::core::Time time);
    39         void StartTraj(float start_pos,float end_pos);
    40         void StopTraj(void);
    41         void Reset(void);
    42         flair::core::cvmatrix *output;
    43         float pos_off,vel_off;
    44         bool is_finished,is_started;
     31public:
     32  TrajectoryGenerator1D_impl(flair::filter::TrajectoryGenerator1D *self,
     33                             const flair::gui::LayoutPosition *position,
     34                             std::string name, std::string unit);
     35  ~TrajectoryGenerator1D_impl();
     36  void Update(flair::core::Time time);
     37  void StartTraj(float start_pos, float end_pos);
     38  void StopTraj(void);
     39  void Reset(void);
     40  flair::core::cvmatrix *output;
     41  float pos_off, vel_off;
     42  bool is_finished, is_started;
    4543
    46     private:
    47         float end_position;
    48         float pos,v,acc;
    49         flair::core::Time previous_time;
    50         bool first_update;
    51         flair::gui::DoubleSpinBox *T,*max_veloctity,*acceleration;
     44private:
     45  float end_position;
     46  float pos, v, acc;
     47  flair::core::Time previous_time;
     48  bool first_update;
     49  flair::gui::DoubleSpinBox *T, *max_veloctity, *acceleration;
    5250};
    5351

  • trunk/lib/FlairFilter/src/unexported/TrajectoryGenerator2DCircle_impl.h

    r10 r15  
    1717#include <Vector2D.h>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class cvmatrix;
    24         class io_data;
    25     }
    26     namespace gui
    27     {
    28         class LayoutPosition;
    29         class DoubleSpinBox;
    30     }
    31     namespace filter
    32     {
    33         class TrajectoryGenerator2DCircle;
    34     }
     19namespace flair {
     20namespace core {
     21class cvmatrix;
     22class io_data;
    3523}
    36 
     24namespace gui {
     25class LayoutPosition;
     26class DoubleSpinBox;
     27}
     28namespace filter {
     29class TrajectoryGenerator2DCircle;
     30}
     31}
    3732
    3833/*! \class TrajectoryGenerator2DCircle_impl
    … …  
    4035*/
    4136
    42 class TrajectoryGenerator2DCircle_impl
    43 {
     37class TrajectoryGenerator2DCircle_impl {
    4438
    45     public:
    46         TrajectoryGenerator2DCircle_impl(flair::filter::TrajectoryGenerator2DCircle* self,const flair::gui::LayoutPosition* position,std::string name);
    47         ~TrajectoryGenerator2DCircle_impl();
    48         void Update(flair::core::Time time);
    49         void StartTraj(const flair::core::Vector2D &start_pos,float nb_lap);
    50         void FinishTraj(void);
    51         bool is_running;
    52         flair::core::cvmatrix *output;
    53         flair::core::Vector2D pos_off,vel_off;
     39public:
     40  TrajectoryGenerator2DCircle_impl(
     41      flair::filter::TrajectoryGenerator2DCircle *self,
     42      const flair::gui::LayoutPosition *position, std::string name);
     43  ~TrajectoryGenerator2DCircle_impl();
     44  void Update(flair::core::Time time);
     45  void StartTraj(const flair::core::Vector2D &start_pos, float nb_lap);
     46  void FinishTraj(void);
     47  bool is_running;
     48  flair::core::cvmatrix *output;
     49  flair::core::Vector2D pos_off, vel_off;
    5450
    55     private:
    56         flair::core::Time previous_time;
    57         float CurrentTime,FinishTime;
    58         bool first_update,is_finishing;
    59         flair::core::Vector2D pos;
    60         float angle_off;
    61         flair::gui::DoubleSpinBox *T,*veloctity,*acceleration,*rayon;
    62         float nb_lap;
     51private:
     52  flair::core::Time previous_time;
     53  float CurrentTime, FinishTime;
     54  bool first_update, is_finishing;
     55  flair::core::Vector2D pos;
     56  float angle_off;
     57  flair::gui::DoubleSpinBox *T, *veloctity, *acceleration, *rayon;
     58  float nb_lap;
    6359};
    6460

  • trunk/lib/FlairFilter/src/unexported/UavMultiplex_impl.h

    r10 r15  
    1717#include <string>
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class FrameworkManager;
    24         class cvmatrix;
    25         class io_data;
    26     }
    27     namespace gui
    28     {
    29         class Tab;
    30         class ComboBox;
    31         class GroupBox;
    32         class TabWidget;
    33     }
    34     namespace filter
    35     {
    36         class UavMultiplex;
    37     }
     19namespace flair {
     20namespace core {
     21class FrameworkManager;
     22class cvmatrix;
     23class io_data;
    3824}
    39 
     25namespace gui {
     26class Tab;
     27class ComboBox;
     28class GroupBox;
     29class TabWidget;
     30}
     31namespace filter {
     32class UavMultiplex;
     33}
     34}
    4035
    4136/*! \class UavMultiplex_impl
    … …  
    4338* \brief Class defining uav multiplexing
    4439*/
    45 class UavMultiplex_impl
    46 {
    47     public:
     40class UavMultiplex_impl {
     41public:
     42  /*!
     43  * \brief Constructor
     44  *
     45  * Construct a uav multiplexing
     46  *
     47  * \param parent parent
     48  * \param name name
     49  */
     50  UavMultiplex_impl(const flair::core::FrameworkManager *parent,
     51                    flair::filter::UavMultiplex *self, std::string name);
    4852
    49         /*!
    50         * \brief Constructor
    51         *
    52         * Construct a uav multiplexing
    53         *
    54         * \param parent parent
    55         * \param name name
    56         */
    57         UavMultiplex_impl(const flair::core::FrameworkManager* parent,flair::filter::UavMultiplex* self,std::string name);
     53  /*!
     54  * \brief Destructor
     55  *
     56  */
     57  ~UavMultiplex_impl();
    5858
    59         /*!
    60         * \brief Destructor
    61         *
    62         */
    63         ~UavMultiplex_impl();
     59  flair::core::cvmatrix *input;
     60  void SetMultiplexComboBox(std::string name, int index);
     61  int MultiplexValue(int index) const;
    6462
    65         flair::core::cvmatrix *input;
    66         void SetMultiplexComboBox(std::string name,int index);
    67         int MultiplexValue(int index) const;
     63  flair::gui::TabWidget *tabwidget;
     64  flair::gui::Tab *setup_tab;
    6865
    69         flair::gui::TabWidget* tabwidget;
    70         flair::gui::Tab *setup_tab;
    71 
    72     private:
    73         flair::gui::Tab *main_tab;
    74         flair::gui::ComboBox **multiplexcombobox;
    75         flair::gui::GroupBox *groupbox;
    76         flair::filter::UavMultiplex* self;
     66private:
     67  flair::gui::Tab *main_tab;
     68  flair::gui::ComboBox **multiplexcombobox;
     69  flair::gui::GroupBox *groupbox;
     70  flair::filter::UavMultiplex *self;
    7771};
    7872

  • trunk/lib/FlairFilter/src/unexported/X4X8Multiplex_impl.h

    r10 r15  
    1616#include <string>
    1717
    18 namespace flair
    19 {
    20     namespace core
    21     {
    22         class cvmatrix;
    23         class io_data;
    24     }
    25     namespace gui
    26     {
    27         class DataPlot1D;
    28         class ComboBox;
    29     }
    30     namespace filter
    31     {
    32         class X4X8Multiplex;
    33     }
     18namespace flair {
     19namespace core {
     20class cvmatrix;
     21class io_data;
    3422}
    35 
     23namespace gui {
     24class DataPlot1D;
     25class ComboBox;
     26}
     27namespace filter {
     28class X4X8Multiplex;
     29}
     30}
    3631
    3732/*! \class X4X8Multiplex_impl
    … …  
    3934* \brief Class defining X4 and X8 multiplexing
    4035*/
    41 class X4X8Multiplex_impl
    42 {
    43     public:
    44         X4X8Multiplex_impl(flair::filter::X4X8Multiplex* self,int nb_mot);
    45         ~X4X8Multiplex_impl();
    46         void UseDefaultPlot(void);
    47         void UpdateFrom(const flair::core::io_data *data);
    48         int nb_mot;
    49         std::string MotorName(int index);
     36class X4X8Multiplex_impl {
     37public:
     38  X4X8Multiplex_impl(flair::filter::X4X8Multiplex *self, int nb_mot);
     39  ~X4X8Multiplex_impl();
     40  void UseDefaultPlot(void);
     41  void UpdateFrom(const flair::core::io_data *data);
     42  int nb_mot;
     43  std::string MotorName(int index);
    5044
    51     private:
    52         flair::core::cvmatrix *output;
    53         flair::gui::ComboBox *pas;
    54         flair::gui::DataPlot1D *plots[4];
    55         flair::filter::X4X8Multiplex* self;
    56         float Set(float trim,float u);
     45private:
     46  flair::core::cvmatrix *output;
     47  flair::gui::ComboBox *pas;
     48  flair::gui::DataPlot1D *plots[4];
     49  flair::filter::X4X8Multiplex *self;
     50  float Set(float trim, float u);
    5751};
    5852

  • trunk/lib/FlairMeta/src/HdsX8.cpp

    r10 r15  
    3232using namespace flair::actuator;
    3333
    34 namespace flair { namespace meta {
     34namespace flair {
     35namespace meta {
    3536
    36 HdsX8::HdsX8(FrameworkManager* parent,string uav_name,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
    37     RTDM_I2cPort* i2cport=new RTDM_I2cPort(parent,"rtdm_i2c","rti2c3");
    38     RTDM_SerialPort* imu_port=new RTDM_SerialPort(parent,"imu_port","rtser1");
     37HdsX8::HdsX8(FrameworkManager *parent, string uav_name,
     38             filter::UavMultiplex *multiplex)
     39    : Uav(parent, uav_name, multiplex) {
     40  RTDM_I2cPort *i2cport = new RTDM_I2cPort(parent, "rtdm_i2c", "rti2c3");
     41  RTDM_SerialPort *imu_port = new RTDM_SerialPort(parent, "imu_port", "rtser1");
    3942
    40     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
     43  if (multiplex == NULL)
     44    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
    4145
    42     SetBldc(new BlCtrlV2(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),i2cport));
    43     SetUsRangeFinder(new Srf08(parent,"SRF08",i2cport,0x70,60));
    44     SetAhrs(new Gx3_25_ahrs(parent,"imu",imu_port,Gx3_25_imu::EulerAnglesAndAngularRates,70));
    45     SetBatteryMonitor(((BlCtrlV2*)GetBldc())->GetBatteryMonitor());
     46  SetBldc(new BlCtrlV2(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     47                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
     48  SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
     49  SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
     50                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
     51  SetBatteryMonitor(((BlCtrlV2 *)GetBldc())->GetBatteryMonitor());
    4652
    47 /*
    48         if(VRPNType==Auto || VRPNType==AutoSerialPort)
    49         {
    50             RTDM_SerialPort* vrpn_port=new RTDM_SerialPort(parent,"vrpn_port","rtser3");
     53  /*
     54          if(VRPNType==Auto || VRPNType==AutoSerialPort)
     55          {
     56              RTDM_SerialPort* vrpn_port=new
     57     RTDM_SerialPort(parent,"vrpn_port","rtser3");
    5158
    52             vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
    53             uav_vrpn=new MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
    54         }
    55 */
    56     SetVerticalCamera(new Ps3Eye(parent,"camv",0,50));
     59              vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
     60              uav_vrpn=new
     61     MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
     62          }
     63  */
     64  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
    5765}
    5866
    59 HdsX8::~HdsX8() {
    60 
    61 }
     67HdsX8::~HdsX8() {}
    6268
    6369void HdsX8::StartSensors(void) {
    64     ((Gx3_25_ahrs*)GetAhrs())->Start();
    65     ((Srf08*)GetUsRangeFinder())->Start();
    66     ((Ps3Eye *)GetVerticalCamera())->Start();
    67     Uav::StartSensors();
     70  ((Gx3_25_ahrs *)GetAhrs())->Start();
     71  ((Srf08 *)GetUsRangeFinder())->Start();
     72  ((Ps3Eye *)GetVerticalCamera())->Start();
     73  Uav::StartSensors();
    6874}
    6975

  • trunk/lib/FlairMeta/src/HdsX8.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class HdsX8
    23     *
    24     * \brief Class defining an ardrone2 uav
    25     */
    26     class HdsX8 : public Uav {
    27         public:
    28             HdsX8(core::FrameworkManager* parent,std::string uav_name,filter::UavMultiplex *multiplex=NULL);
    29             ~HdsX8();
    30             void StartSensors(void);
     18namespace flair {
     19namespace meta {
     20/*! \class HdsX8
     21*
     22* \brief Class defining an ardrone2 uav
     23*/
     24class HdsX8 : public Uav {
     25public:
     26  HdsX8(core::FrameworkManager *parent, std::string uav_name,
     27        filter::UavMultiplex *multiplex = NULL);
     28  ~HdsX8();
     29  void StartSensors(void);
    3130
    32         private:
    33 
    34     };
     31private:
     32};
    3533} // end namespace meta
    3634} // end namespace flair

  • trunk/lib/FlairMeta/src/MetaDualShock3.cpp

    r10 r15  
    3131using namespace flair::gui;
    3232
    33 namespace flair { namespace meta {
     33namespace flair {
     34namespace meta {
    3435
    35 MetaDualShock3::MetaDualShock3(FrameworkManager* parent,string name,uint16_t port,uint8_t priority) : TargetEthController(parent,name,port,priority) {
    36     pimpl_=new MetaDualShock3_impl(this,name);
    37     parent->AddDeviceToLog(pimpl_->joy_ref);
    38     Start();
     36MetaDualShock3::MetaDualShock3(FrameworkManager *parent, string name,
     37                               uint16_t port, uint8_t priority)
     38    : TargetEthController(parent, name, port, priority) {
     39  pimpl_ = new MetaDualShock3_impl(this, name);
     40  parent->AddDeviceToLog(pimpl_->joy_ref);
     41  Start();
    3942}
    4043
    41 MetaDualShock3::~MetaDualShock3() {
    42     delete pimpl_;
    43 }
     44MetaDualShock3::~MetaDualShock3() { delete pimpl_; }
    4445
    45 AhrsData* MetaDualShock3::GetReferenceOrientation(void) const {
    46     return pimpl_->joy_ref->GetReferenceOrientation();
     46AhrsData *MetaDualShock3::GetReferenceOrientation(void) const {
     47  return pimpl_->joy_ref->GetReferenceOrientation();
    4748}
    4849
    4950void MetaDualShock3::ErrorNotify(void) {
    50     TargetEthController::FlashLed(4,10,0);
    51     TargetEthController::Rumble(0xff,20,0,0);
     51  TargetEthController::FlashLed(4, 10, 0);
     52  TargetEthController::Rumble(0xff, 20, 0, 0);
    5253}
    5354
    54 void MetaDualShock3::Rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout) {
    55     TargetEthController::Rumble(left_force,left_timeout,right_force,right_timeout);
     55void MetaDualShock3::Rumble(uint8_t left_force, uint8_t left_timeout,
     56                            uint8_t right_force, uint8_t right_timeout) {
     57  TargetEthController::Rumble(left_force, left_timeout, right_force,
     58                              right_timeout);
    5659}
    5760
    5861void MetaDualShock3::SetLedON(unsigned int ledId) {
    59     TargetEthController::SetLedOn(ledId);
     62  TargetEthController::SetLedOn(ledId);
    6063}
    6164
    6265void MetaDualShock3::SetLedOFF(unsigned int ledId) {
    63     TargetEthController::SetLedOff(ledId);
     66  TargetEthController::SetLedOff(ledId);
    6467}
    6568
    66 void MetaDualShock3::FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout) {
    67     TargetEthController::FlashLed(ledId,on_timeout,off_timeout);
     69void MetaDualShock3::FlashLed(unsigned int ledId, uint8_t on_timeout,
     70                              uint8_t off_timeout) {
     71  TargetEthController::FlashLed(ledId, on_timeout, off_timeout);
    6872}
    6973
    70 float MetaDualShock3::ZRef(void) const {
    71     return pimpl_->joy_ref->ZRef();
    72 }
     74float MetaDualShock3::ZRef(void) const { return pimpl_->joy_ref->ZRef(); }
    7375
    74 float MetaDualShock3::DzRef(void) const {
    75     return pimpl_->joy_ref->DzRef();
    76 }
     76float MetaDualShock3::DzRef(void) const { return pimpl_->joy_ref->DzRef(); }
    7777
    7878void MetaDualShock3::SetYawRef(float value) {
    79     pimpl_->joy_ref->SetYawRef(value);
     79  pimpl_->joy_ref->SetYawRef(value);
    8080}
    8181
    8282void MetaDualShock3::SetYawRef(Quaternion const &value) {
    83     pimpl_->joy_ref->SetYawRef(value);
     83  pimpl_->joy_ref->SetYawRef(value);
    8484}
    8585
    86 void MetaDualShock3::SetZRef(float value) {
    87     pimpl_->joy_ref->SetZRef(value);
    88 }
     86void MetaDualShock3::SetZRef(float value) { pimpl_->joy_ref->SetZRef(value); }
    8987
    9088float MetaDualShock3::RollTrim(void) const {
    91     return pimpl_->joy_ref->RollTrim();
     89  return pimpl_->joy_ref->RollTrim();
    9290}
    9391
    9492float MetaDualShock3::PitchTrim(void) const {
    95     return pimpl_->joy_ref->PitchTrim();
     93  return pimpl_->joy_ref->PitchTrim();
    9694}
    9795

  • trunk/lib/FlairMeta/src/MetaDualShock3.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace core {
    20         class AhrsData;
    21         class Quaternion;
    22     }
    23     namespace filter {
    24         class Ahrs;
    25     }
     19namespace core {
     20class AhrsData;
     21class Quaternion;
     22}
     23namespace filter {
     24class Ahrs;
     25}
    2626}
    2727
    … …  
    3131namespace meta {
    3232
    33     /*! \class MetaDualShock3
    34     *
    35     * \brief Classe intégrant la manette MetaDualShock3
    36     */
    37     class MetaDualShock3 : public sensor::TargetEthController {
    38         friend class ::MetaDualShock3_impl;
     33/*! \class MetaDualShock3
     34*
     35* \brief Classe intégrant la manette MetaDualShock3
     36*/
     37class MetaDualShock3 : public sensor::TargetEthController {
     38  friend class ::MetaDualShock3_impl;
    3939
    40         public:
    41             MetaDualShock3(core::FrameworkManager* parent,std::string name,uint16_t port,uint8_t priority);
    42             ~MetaDualShock3();
    43             core::AhrsData* GetReferenceOrientation(void) const;
    44             float ZRef(void) const;
    45             float DzRef(void) const;
    46             void SetYawRef(float value);
    47             /*!
    48             * \brief Set yaw reference
    49             *
    50             * Yaw part of the output quaternion is obtained by integrating the wz desired angular speed.\n
    51             * This method reset the yaw.
    52             *
    53             * \param value value, only the yaw part of the quaternion is used
    54             */
    55             void SetYawRef(core::Quaternion const &value);
    56             void SetZRef(float value);
    57             float RollTrim(void) const;
    58             float PitchTrim(void) const;
    59             void ErrorNotify(void);
    60             void Rumble(uint8_t left_force,uint8_t left_timeout=20,uint8_t right_force=0,uint8_t right_timeout=0);
    61             void SetLedON(unsigned int ledId);
    62             void SetLedOFF(unsigned int ledId);
    63             void FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout);
     40public:
     41  MetaDualShock3(core::FrameworkManager *parent, std::string name,
     42                 uint16_t port, uint8_t priority);
     43  ~MetaDualShock3();
     44  core::AhrsData *GetReferenceOrientation(void) const;
     45  float ZRef(void) const;
     46  float DzRef(void) const;
     47  void SetYawRef(float value);
     48  /*!
     49  * \brief Set yaw reference
     50  *
     51  * Yaw part of the output quaternion is obtained by integrating the wz desired
     52  *angular speed.\n
     53  * This method reset the yaw.
     54  *
     55  * \param value value, only the yaw part of the quaternion is used
     56  */
     57  void SetYawRef(core::Quaternion const &value);
     58  void SetZRef(float value);
     59  float RollTrim(void) const;
     60  float PitchTrim(void) const;
     61  void ErrorNotify(void);
     62  void Rumble(uint8_t left_force, uint8_t left_timeout = 20,
     63              uint8_t right_force = 0, uint8_t right_timeout = 0);
     64  void SetLedON(unsigned int ledId);
     65  void SetLedOFF(unsigned int ledId);
     66  void FlashLed(unsigned int ledId, uint8_t on_timeout, uint8_t off_timeout);
    6467
    65         private:
    66             class MetaDualShock3_impl* pimpl_;
    67 
    68     };
     68private:
     69  class MetaDualShock3_impl *pimpl_;
     70};
    6971} // end namespace meta
    7072} // end namespace flair

  • trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp

    r10 r15  
    2929using namespace flair::meta;
    3030
    31 MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3* self,string name): IODevice((IODevice*)self,name) {
    32     joy_ref=new JoyReference(self->GetTab()->NewRow(),"consignes joy");
    33     this->self=self;
    34     joy_init=false;
    35     wasRollTrimUpButtonPressed=false;
    36     wasRollTrimDownButtonPressed=false;
    37     wasPitchTrimUpButtonPressed=false;
    38     wasPitchTrimDownButtonPressed=false;
     31MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name)
     32    : IODevice((IODevice *)self, name) {
     33  joy_ref = new JoyReference(self->GetTab()->NewRow(), "consignes joy");
     34  this->self = self;
     35  joy_init = false;
     36  wasRollTrimUpButtonPressed = false;
     37  wasRollTrimDownButtonPressed = false;
     38  wasPitchTrimUpButtonPressed = false;
     39  wasPitchTrimDownButtonPressed = false;
    3940}
    4041
    41 MetaDualShock3_impl::~MetaDualShock3_impl() {
     42MetaDualShock3_impl::~MetaDualShock3_impl() {}
    4243
     44// receives updates from the controler
     45void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
     46  cvmatrix *input = (cvmatrix *)data;
     47
     48  // on prend une fois pour toute le mutex et on fait des accès directs
     49  input->GetMutex();
     50
     51  // up
     52  if (self->IsButtonPressed(12)) {
     53    if (!wasPitchTrimDownButtonPressed) {
     54      joy_ref->PitchTrimDown();
     55      wasPitchTrimDownButtonPressed = true;
     56    }
     57  } else {
     58    wasPitchTrimDownButtonPressed = false;
     59  }
     60
     61  // down
     62  if (self->IsButtonPressed(13)) {
     63    if (!wasPitchTrimUpButtonPressed) {
     64      joy_ref->PitchTrimUp();
     65      wasPitchTrimUpButtonPressed = true;
     66    }
     67  } else {
     68    wasPitchTrimUpButtonPressed = false;
     69  }
     70
     71  // right
     72  if (self->IsButtonPressed(15)) {
     73    if (!wasRollTrimUpButtonPressed) {
     74      joy_ref->RollTrimUp();
     75      wasRollTrimUpButtonPressed = true;
     76    }
     77  } else {
     78    wasRollTrimUpButtonPressed = false;
     79  }
     80
     81  // left
     82  if (self->IsButtonPressed(14)) {
     83    if (!wasRollTrimDownButtonPressed) {
     84      joy_ref->RollTrimDown();
     85      wasRollTrimDownButtonPressed = true;
     86    }
     87  } else {
     88    wasRollTrimDownButtonPressed = false;
     89  }
     90
     91  if (!getFrameworkManager()->ConnectionLost()) {
     92    input->GetMutex();
     93    joy_ref->SetRollAxis(input->ValueNoMutex(0, 0));
     94    joy_ref->SetPitchAxis(input->ValueNoMutex(1, 0));
     95    joy_ref->SetYawAxis(input->ValueNoMutex(2, 0));
     96    joy_ref->SetAltitudeAxis(input->ValueNoMutex(3, 0));
     97    input->ReleaseMutex();
     98  } else {
     99    joy_ref->SetRollAxis(0);
     100    joy_ref->SetPitchAxis(0);
     101    joy_ref->SetYawAxis(0);
     102    joy_ref->SetAltitudeAxis(0);
     103  }
     104  input->ReleaseMutex();
     105
     106  joy_ref->Update(data->DataTime());
     107
     108  if (!joy_init) {
     109    self->TargetEthController::FlashLed(1, 10, 10);
     110    joy_init = true;
     111  }
    43112}
    44 
    45 //receives updates from the controler
    46 void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
    47     cvmatrix *input=(cvmatrix*)data;
    48 
    49     //on prend une fois pour toute le mutex et on fait des accès directs
    50     input->GetMutex();
    51 
    52     //up
    53     if(self->IsButtonPressed(12)) {
    54         if(!wasPitchTrimDownButtonPressed) {
    55             joy_ref->PitchTrimDown();
    56             wasPitchTrimDownButtonPressed=true;
    57         }
    58     } else {
    59         wasPitchTrimDownButtonPressed=false;
    60     }
    61 
    62     //down
    63     if(self->IsButtonPressed(13)) {
    64         if(!wasPitchTrimUpButtonPressed) {
    65             joy_ref->PitchTrimUp();
    66             wasPitchTrimUpButtonPressed=true;
    67         }
    68     } else {
    69         wasPitchTrimUpButtonPressed=false;
    70     }
    71 
    72     //right
    73     if(self->IsButtonPressed(15)) {
    74         if(!wasRollTrimUpButtonPressed) {
    75             joy_ref->RollTrimUp();
    76             wasRollTrimUpButtonPressed=true;
    77         }
    78     } else {
    79         wasRollTrimUpButtonPressed=false;
    80     }
    81 
    82     //left
    83     if(self->IsButtonPressed(14)) {
    84         if(!wasRollTrimDownButtonPressed) {
    85             joy_ref->RollTrimDown();
    86             wasRollTrimDownButtonPressed=true;
    87         }
    88     } else {
    89         wasRollTrimDownButtonPressed=false;
    90     }
    91 
    92     if(!getFrameworkManager()->ConnectionLost()) {
    93         input->GetMutex();
    94         joy_ref->SetRollAxis(input->ValueNoMutex(0,0));
    95         joy_ref->SetPitchAxis(input->ValueNoMutex(1,0));
    96         joy_ref->SetYawAxis(input->ValueNoMutex(2,0));
    97         joy_ref->SetAltitudeAxis(input->ValueNoMutex(3,0));
    98         input->ReleaseMutex();
    99     } else {
    100         joy_ref->SetRollAxis(0);
    101         joy_ref->SetPitchAxis(0);
    102         joy_ref->SetYawAxis(0);
    103         joy_ref->SetAltitudeAxis(0);
    104     }
    105     input->ReleaseMutex();
    106 
    107     joy_ref->Update(data->DataTime());
    108 
    109     if(!joy_init) {
    110         self->TargetEthController::FlashLed(1,10,10);
    111         joy_init=true;
    112     }
    113 }

  • trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp

    r10 r15  
    3535using namespace flair::sensor;
    3636
    37 namespace flair { namespace meta
    38 {
     37namespace flair {
     38namespace meta {
    3939
    40 MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder* us): Object(us,us->ObjectName()) {
    41     this->us=us;
    42     pbas_z=new ButterworthLowPass(us,us->GetLayout()->NewRow(),"Passe bas",3);
    43     vz_euler=new EulerDerivative(pbas_z,us->GetLayout()->NewRow(),"Vz");
    44     pbas_vz=new ButterworthLowPass(vz_euler,us->GetLayout()->NewRow(),"Passe bas v",3);
    45 
     40MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
     41    : Object(us, us->ObjectName()) {
     42  this->us = us;
     43  pbas_z =
     44      new ButterworthLowPass(us, us->GetLayout()->NewRow(), "Passe bas", 3);
     45  vz_euler = new EulerDerivative(pbas_z, us->GetLayout()->NewRow(), "Vz");
     46  pbas_vz = new ButterworthLowPass(vz_euler, us->GetLayout()->NewRow(),
     47                                   "Passe bas v", 3);
    4648}
    4749
    48 MetaUsRangeFinder::~MetaUsRangeFinder() {
     50MetaUsRangeFinder::~MetaUsRangeFinder() {}
    4951
     52void MetaUsRangeFinder::UseDefaultPlot(void) {
     53  us->UseDefaultPlot();
     54
     55  us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0), DataPlot::Blue);
     56
     57  vz_plot = new DataPlot1D(us->GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
     58  vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
     59  vz_plot->AddCurve(pbas_vz->Matrix()->Element(0), DataPlot::Blue);
    5060}
    5161
    52 void MetaUsRangeFinder::UseDefaultPlot(void) {
    53     us->UseDefaultPlot();
     62gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() { return us->GetPlot(); }
    5463
    55     us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0),DataPlot::Blue);
     64gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() { return vz_plot; }
    5665
    57     vz_plot=new DataPlot1D(us->GetPlotTab()->LastRowLastCol(),"vz",-2,2);
    58     vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
    59     vz_plot->AddCurve(pbas_vz->Matrix()->Element(0),DataPlot::Blue);
    60 }
     66float MetaUsRangeFinder::z(void) const { return pbas_z->Output(); }
    6167
    62 gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() {
    63     return us->GetPlot();
    64 }
    65 
    66 gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() {
    67     return vz_plot;
    68 }
    69 
    70 float MetaUsRangeFinder::z(void) const {
    71     return pbas_z->Output();
    72 }
    73 
    74 float MetaUsRangeFinder::Vz(void) const {
    75     return pbas_vz->Output();
    76 }
     68float MetaUsRangeFinder::Vz(void) const { return pbas_vz->Output(); }
    7769
    7870} // end namespace sensor

  • trunk/lib/FlairMeta/src/MetaUsRangeFinder.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace filter {
    20         class ButterworthLowPass;
    21         class EulerDerivative;
    22     }
    23     namespace sensor {
    24         class UsRangeFinder;
    25     }
    26     namespace gui {
    27         class DataPlot1D;
    28     }
     19namespace filter {
     20class ButterworthLowPass;
     21class EulerDerivative;
     22}
     23namespace sensor {
     24class UsRangeFinder;
     25}
     26namespace gui {
     27class DataPlot1D;
     28}
    2929}
    3030
    31 namespace flair
    32 {
    33 namespace meta
    34 {
    35     /*! \class MetaUsRangeFinder
    36     *
    37     * \brief Classe haut niveau pour capteur à ultra son
    38     *
    39     * Contient une dérivée d'euler et un passe bas.
    40     * Cette classe est adaptée pour un capteur d'altitude.
    41     */
    42     class MetaUsRangeFinder: public core::Object {
    43         public:
    44             MetaUsRangeFinder(sensor::UsRangeFinder* us);
    45             ~MetaUsRangeFinder();
    46             void UseDefaultPlot(void);
    47             float z(void) const;
    48             float Vz(void) const;
    49             gui::DataPlot1D *GetZPlot();
    50             gui::DataPlot1D *GetVzPlot();
     31namespace flair {
     32namespace meta {
     33/*! \class MetaUsRangeFinder
     34*
     35* \brief Classe haut niveau pour capteur à ultra son
     36*
     37* Contient une dérivée d'euler et un passe bas.
     38* Cette classe est adaptée pour un capteur d'altitude.
     39*/
     40class MetaUsRangeFinder : public core::Object {
     41public:
     42  MetaUsRangeFinder(sensor::UsRangeFinder *us);
     43  ~MetaUsRangeFinder();
     44  void UseDefaultPlot(void);
     45  float z(void) const;
     46  float Vz(void) const;
     47  gui::DataPlot1D *GetZPlot();
     48  gui::DataPlot1D *GetVzPlot();
    5149
    52         private:
    53             sensor::UsRangeFinder* us;
    54             filter::ButterworthLowPass *pbas_z,*pbas_vz;
    55             filter::EulerDerivative *vz_euler;
    56             gui::DataPlot1D* vz_plot;
    57     };
     50private:
     51  sensor::UsRangeFinder *us;
     52  filter::ButterworthLowPass *pbas_z, *pbas_vz;
     53  filter::EulerDerivative *vz_euler;
     54  gui::DataPlot1D *vz_plot;
     55};
    5856} // end namespace meta
    5957} // end namespace flair

  • trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

    r10 r15  
    3636using namespace flair::filter;
    3737
    38 namespace flair { namespace meta {
     38namespace flair {
     39namespace meta {
    3940
    40 MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent,string name): VrpnObject(parent,name,parent->GetTabWidget())
    41 {
    42     ConstructorCommon(parent,name);
     41MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent, string name)
     42    : VrpnObject(parent, name, parent->GetTabWidget()) {
     43  ConstructorCommon(parent, name);
    4344}
    4445
    45 MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent,std::string name,uint8_t id): VrpnObject(parent,name,id,parent->GetTabWidget())
    46 {
    47     ConstructorCommon(parent,name);
     46MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent, std::string name,
     47                               uint8_t id)
     48    : VrpnObject(parent, name, id, parent->GetTabWidget()) {
     49  ConstructorCommon(parent, name);
    4850}
    4951
    50 void MetaVrpnObject::ConstructorCommon(const VrpnClient *parent,string name) {
    51     cvmatrix_descriptor* desc=new cvmatrix_descriptor(6,1);
    52     for(int i=0;i<6;i++) {
    53         desc->SetElementName(i,0,Output()->Name(i,0));
    54     }
    55     cvmatrix* prev_value=new cvmatrix(this,desc,elementDataType,name);
    56     for(int i=0;i<6;i++) {
    57         prev_value->SetValue(i,0,0);
    58     }
     52void MetaVrpnObject::ConstructorCommon(const VrpnClient *parent, string name) {
     53  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
     54  for (int i = 0; i < 6; i++) {
     55    desc->SetElementName(i, 0, Output()->Name(i, 0));
     56  }
     57  cvmatrix *prev_value = new cvmatrix(this, desc, elementDataType, name);
     58  for (int i = 0; i < 6; i++) {
     59    prev_value->SetValue(i, 0, 0);
     60  }
    5961
    60     pbas=new LowPassFilter(this,parent->GetLayout()->NewRow(),name + " Passe bas",prev_value);
     62  pbas = new LowPassFilter(this, parent->GetLayout()->NewRow(),
     63                           name + " Passe bas", prev_value);
    6164
    62     desc=new cvmatrix_descriptor(6,1);
    63     for(int i=0;i<6;i++) {
    64         desc->SetElementName(i,0,"d" + Output()->Name(i,0));
    65     }
    66     prev_value=new cvmatrix(this,desc,elementDataType,name);
    67     for(int i=0;i<6;i++) {
    68         prev_value->SetValue(i,0,0);
    69     }
     65  desc = new cvmatrix_descriptor(6, 1);
     66  for (int i = 0; i < 6; i++) {
     67    desc->SetElementName(i, 0, "d" + Output()->Name(i, 0));
     68  }
     69  prev_value = new cvmatrix(this, desc, elementDataType, name);
     70  for (int i = 0; i < 6; i++) {
     71    prev_value->SetValue(i, 0, 0);
     72  }
    7073
    71     euler=new EulerDerivative(pbas,parent->GetLayout()->NewRow(),name + "_euler",prev_value);
     74  euler = new EulerDerivative(pbas, parent->GetLayout()->NewRow(),
     75                              name + "_euler", prev_value);
    7276
    73     vx_opti_plot=new DataPlot1D(GetPlotTab()->NewRow(),"vx",-3,3);
    74     vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
    75     vy_opti_plot=new DataPlot1D(GetPlotTab()->LastRowLastCol(),"vy",-3,3);
    76     vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
    77     vz_opti_plot=new DataPlot1D(GetPlotTab()->LastRowLastCol(),"vz",-2,2);
    78     vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
     77  vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);
     78  vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
     79  vy_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vy", -3, 3);
     80  vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
     81  vz_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
     82  vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
    7983
    80     plot_tab=new Tab(parent->GetTabWidget(),"Mesures (xy) "+ name);
    81     xy_plot=new DataPlot2D(plot_tab->NewRow(),"xy","y",-5,5,"x",-5,5);
    82         xy_plot->AddCurve(Output()->Element(4,0),Output()->Element(3,0));
     84  plot_tab = new Tab(parent->GetTabWidget(), "Mesures (xy) " + name);
     85  xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
     86  xy_plot->AddCurve(Output()->Element(4, 0), Output()->Element(3, 0));
    8387}
    8488
    85 MetaVrpnObject::~MetaVrpnObject() {
    86     delete plot_tab;
    87 }
     89MetaVrpnObject::~MetaVrpnObject() { delete plot_tab; }
    8890
    89 DataPlot1D* MetaVrpnObject::VxPlot(void) const
    90 {
    91     return vx_opti_plot;
    92 }
     91DataPlot1D *MetaVrpnObject::VxPlot(void) const { return vx_opti_plot; }
    9392
    94 DataPlot1D* MetaVrpnObject::VyPlot(void) const
    95 {
    96     return vy_opti_plot;
    97 }
     93DataPlot1D *MetaVrpnObject::VyPlot(void) const { return vy_opti_plot; }
    9894
    99 DataPlot1D* MetaVrpnObject::VzPlot(void) const
    100 {
    101     return vz_opti_plot;
    102 }
     95DataPlot1D *MetaVrpnObject::VzPlot(void) const { return vz_opti_plot; }
    10396
    104 DataPlot2D* MetaVrpnObject::XyPlot(void) const {
    105     return xy_plot;
    106 }
     97DataPlot2D *MetaVrpnObject::XyPlot(void) const { return xy_plot; }
    10798
    108 void MetaVrpnObject::GetSpeed(Vector3D &speed) const
    109 {
    110     speed.x=euler->Output(3,0);
    111     speed.y=euler->Output(4,0);
    112     speed.z=euler->Output(5,0);
     99void MetaVrpnObject::GetSpeed(Vector3D &speed) const {
     100  speed.x = euler->Output(3, 0);
     101  speed.y = euler->Output(4, 0);
     102  speed.z = euler->Output(5, 0);
    113103}
    114104

  • trunk/lib/FlairMeta/src/MetaVrpnObject.h

    r10 r15  
    1717#include "io_data.h"
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class Vector3D;
    24         class FloatType;
    25     }
    26     namespace gui
    27     {
    28         class DataPlot1D;
    29         class DataPlot2D;
    30         class Tab;
    31     }
    32     namespace filter
    33     {
    34         class EulerDerivative;
    35         class LowPassFilter;
    36     }
    37     namespace sensor
    38     {
    39         class VrpnClient;
    40     }
     19namespace flair {
     20namespace core {
     21class Vector3D;
     22class FloatType;
     23}
     24namespace gui {
     25class DataPlot1D;
     26class DataPlot2D;
     27class Tab;
     28}
     29namespace filter {
     30class EulerDerivative;
     31class LowPassFilter;
     32}
     33namespace sensor {
     34class VrpnClient;
     35}
    4136}
    4237
    43 namespace flair
    44 {
    45 namespace meta
    46 {
     38namespace flair {
     39namespace meta {
    4740
    48     /*! \class MetaVrpnObject
    49     *
    50     * \brief Classe haut niveau intégrant un objet VRPN
    51     *
    52     * Contient un objet VRPN et une dérivée, d'euler.
    53     */
    54     class MetaVrpnObject: public sensor::VrpnObject {
    55         public:
    56             MetaVrpnObject(const sensor::VrpnClient *parent,std::string name);
    57             MetaVrpnObject(const sensor::VrpnClient *parent,std::string name,uint8_t id);
    58             ~MetaVrpnObject();
    59             gui::DataPlot1D* VxPlot(void) const;//1,0
    60             gui::DataPlot1D* VyPlot(void) const;//1,1
    61             gui::DataPlot1D* VzPlot(void) const;//1,2
    62             gui::DataPlot2D* XyPlot(void) const;
    63             void GetSpeed(core::Vector3D &speed) const;
     41/*! \class MetaVrpnObject
     42*
     43* \brief Classe haut niveau intégrant un objet VRPN
     44*
     45* Contient un objet VRPN et une dérivée, d'euler.
     46*/
     47class MetaVrpnObject : public sensor::VrpnObject {
     48public:
     49  MetaVrpnObject(const sensor::VrpnClient *parent, std::string name);
     50  MetaVrpnObject(const sensor::VrpnClient *parent, std::string name,
     51                 uint8_t id);
     52  ~MetaVrpnObject();
     53  gui::DataPlot1D *VxPlot(void) const; // 1,0
     54  gui::DataPlot1D *VyPlot(void) const; // 1,1
     55  gui::DataPlot1D *VzPlot(void) const; // 1,2
     56  gui::DataPlot2D *XyPlot(void) const;
     57  void GetSpeed(core::Vector3D &speed) const;
    6458
    65         private:
    66             void ConstructorCommon(const sensor::VrpnClient *parent,std::string name);
    67             filter::LowPassFilter *pbas;
    68             filter::EulerDerivative *euler;
    69             gui::DataPlot2D *xy_plot;
    70             gui::DataPlot1D *vx_opti_plot,*vy_opti_plot,*vz_opti_plot;
    71             gui::Tab* plot_tab;
    72             core::FloatType elementDataType;
    73 
    74     };
     59private:
     60  void ConstructorCommon(const sensor::VrpnClient *parent, std::string name);
     61  filter::LowPassFilter *pbas;
     62  filter::EulerDerivative *euler;
     63  gui::DataPlot2D *xy_plot;
     64  gui::DataPlot1D *vx_opti_plot, *vy_opti_plot, *vz_opti_plot;
     65  gui::Tab *plot_tab;
     66  core::FloatType elementDataType;
     67};
    7568} // end namespace meta
    7669} // end namespace flair

  • trunk/lib/FlairMeta/src/SimuX4.cpp

    r10 r15  
    3434using namespace flair::actuator;
    3535
    36 namespace flair { namespace meta {
     36namespace flair {
     37namespace meta {
    3738
    38 SimuX4::SimuX4(FrameworkManager* parent,string uav_name,int simu_id,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
     39SimuX4::SimuX4(FrameworkManager *parent, string uav_name, int simu_id,
     40               filter::UavMultiplex *multiplex)
     41    : Uav(parent, uav_name, multiplex) {
    3942
    40     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X4));
     43  if (multiplex == NULL)
     44    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X4));
    4145
    42     SetBldc(new SimuBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),simu_id));
    43     SetUsRangeFinder(new SimuUs(parent,"us",simu_id,60));
    44     SetAhrs(new SimuAhrs(parent,"imu",simu_id,70));
    45     Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
    46     SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
    47     GetBatteryMonitor()->SetBatteryValue(12);
    48     SetVerticalCamera(new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
     46  SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     47                       "motors", GetUavMultiplex()->MotorsCount(), simu_id));
     48  SetUsRangeFinder(new SimuUs(parent, "us", simu_id, 60));
     49  SetAhrs(new SimuAhrs(parent, "imu", simu_id, 70));
     50  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
     51  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
     52  GetBatteryMonitor()->SetBatteryValue(12);
     53  SetVerticalCamera(
     54      new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
    4955}
    5056
    51 SimuX4::~SimuX4() {
     57SimuX4::~SimuX4() {}
    5258
     59void SimuX4::StartSensors(void) {
     60  ((SimuAhrs *)GetAhrs())->Start();
     61  ((SimuUs *)GetUsRangeFinder())->Start();
     62  ((SimuCamera *)GetVerticalCamera())->Start();
     63  Uav::StartSensors();
    5364}
    5465
    55 void SimuX4::StartSensors(void) {
    56     ((SimuAhrs*)GetAhrs())->Start();
    57     ((SimuUs*)GetUsRangeFinder())->Start();
    58     ((SimuCamera *)GetVerticalCamera())->Start();
    59     Uav::StartSensors();
    60 }
    61 
    62 void SimuX4::SetupVRPNAutoIP(string name) {
    63     SetupVRPN("127.0.0.1:3883",name);
    64 }
     66void SimuX4::SetupVRPNAutoIP(string name) { SetupVRPN("127.0.0.1:3883", name); }
    6567
    6668} // end namespace meta

  • trunk/lib/FlairMeta/src/SimuX4.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class SimuX4
    23     *
    24     * \brief Class defining a simultation x4 uav
    25     */
    26     class SimuX4 : public Uav {
    27         public:
    28             //simu_id: 0 if simulating only one UAV
    29             //>0 otherwise
    30             SimuX4(core::FrameworkManager* parent,std::string uav_name,int simu_id=0,filter::UavMultiplex *multiplex=NULL);
    31             ~SimuX4();
    32             void StartSensors(void);
    33             void SetupVRPNAutoIP(std::string name);
    34     };
     18namespace flair {
     19namespace meta {
     20/*! \class SimuX4
     21*
     22* \brief Class defining a simultation x4 uav
     23*/
     24class SimuX4 : public Uav {
     25public:
     26  // simu_id: 0 if simulating only one UAV
     27  //>0 otherwise
     28  SimuX4(core::FrameworkManager *parent, std::string uav_name, int simu_id = 0,
     29         filter::UavMultiplex *multiplex = NULL);
     30  ~SimuX4();
     31  void StartSensors(void);
     32  void SetupVRPNAutoIP(std::string name);
     33};
    3534} // end namespace meta
    3635} // end namespace flair

  • trunk/lib/FlairMeta/src/SimuX8.cpp

    r10 r15  
    3434using namespace flair::actuator;
    3535
    36 namespace flair { namespace meta {
     36namespace flair {
     37namespace meta {
    3738
    38 SimuX8::SimuX8(FrameworkManager* parent,string uav_name,int simu_id,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
     39SimuX8::SimuX8(FrameworkManager *parent, string uav_name, int simu_id,
     40               filter::UavMultiplex *multiplex)
     41    : Uav(parent, uav_name, multiplex) {
    3942
    40     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
     43  if (multiplex == NULL)
     44    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
    4145
    42     SetBldc(new SimuBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),simu_id));
    43     SetUsRangeFinder(new SimuUs(parent,"us",simu_id,60));
    44     SetAhrs(new SimuAhrs(parent,"imu",simu_id,70));
    45     Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
    46     SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
    47     GetBatteryMonitor()->SetBatteryValue(12);
    48     SetVerticalCamera(new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
     46  SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     47                       "motors", GetUavMultiplex()->MotorsCount(), simu_id));
     48  SetUsRangeFinder(new SimuUs(parent, "us", simu_id, 60));
     49  SetAhrs(new SimuAhrs(parent, "imu", simu_id, 70));
     50  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
     51  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
     52  GetBatteryMonitor()->SetBatteryValue(12);
     53  SetVerticalCamera(
     54      new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
    4955}
    5056
    51 SimuX8::~SimuX8() {
     57SimuX8::~SimuX8() {}
    5258
     59void SimuX8::StartSensors(void) {
     60  ((SimuAhrs *)GetAhrs())->Start();
     61  ((SimuUs *)GetUsRangeFinder())->Start();
     62  ((SimuCamera *)GetVerticalCamera())->Start();
     63  Uav::StartSensors();
    5364}
    5465
    55 void SimuX8::StartSensors(void) {
    56     ((SimuAhrs*)GetAhrs())->Start();
    57     ((SimuUs*)GetUsRangeFinder())->Start();
    58     ((SimuCamera *)GetVerticalCamera())->Start();
    59     Uav::StartSensors();
    60 }
    61 
    62 void SimuX8::SetupVRPNAutoIP(string name) {
    63     SetupVRPN("127.0.0.1:3883",name);
    64 }
     66void SimuX8::SetupVRPNAutoIP(string name) { SetupVRPN("127.0.0.1:3883", name); }
    6567
    6668} // end namespace meta

  • trunk/lib/FlairMeta/src/SimuX8.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class SimuX8
    23     *
    24     * \brief Class defining a simultation x8 uav
    25     */
    26     class SimuX8 : public Uav {
    27         public:
    28             //simu_id: 0 if simulating only one UAV
    29             //>0 otherwise
    30             SimuX8(core::FrameworkManager* parent,std::string uav_name,int simu_id=0,filter::UavMultiplex *multiplex=NULL);
    31             ~SimuX8();
    32             void StartSensors(void);
    33             void SetupVRPNAutoIP(std::string name);
    34     };
     18namespace flair {
     19namespace meta {
     20/*! \class SimuX8
     21*
     22* \brief Class defining a simultation x8 uav
     23*/
     24class SimuX8 : public Uav {
     25public:
     26  // simu_id: 0 if simulating only one UAV
     27  //>0 otherwise
     28  SimuX8(core::FrameworkManager *parent, std::string uav_name, int simu_id = 0,
     29         filter::UavMultiplex *multiplex = NULL);
     30  ~SimuX8();
     31  void StartSensors(void);
     32  void SetupVRPNAutoIP(std::string name);
     33};
    3534} // end namespace meta
    3635} // end namespace flair

  • trunk/lib/FlairMeta/src/Uav.cpp

    r10 r15  
    3838using namespace flair::actuator;
    3939
    40 namespace flair { namespace meta {
     40namespace flair {
     41namespace meta {
    4142
    42 Uav::Uav(FrameworkManager* parent,string name,UavMultiplex *multiplex): Object(parent,name) {
    43     vrpnclient=NULL;
    44     uav_vrpn=NULL;
    45     verticalCamera=NULL;
    46     this->multiplex=multiplex;
     43Uav::Uav(FrameworkManager *parent, string name, UavMultiplex *multiplex)
     44    : Object(parent, name) {
     45  vrpnclient = NULL;
     46  uav_vrpn = NULL;
     47  verticalCamera = NULL;
     48  this->multiplex = multiplex;
    4749}
    4850
    49 Uav::~Uav() {
    50 }
     51Uav::~Uav() {}
    5152
    5253void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) {
    53     us=(UsRangeFinder*)inUs;
    54     meta_us=new MetaUsRangeFinder(us);
    55     getFrameworkManager()->AddDeviceToLog(us);
     54  us = (UsRangeFinder *)inUs;
     55  meta_us = new MetaUsRangeFinder(us);
     56  getFrameworkManager()->AddDeviceToLog(us);
    5657}
    5758
    5859void Uav::SetAhrs(const Ahrs *inAhrs) {
    59     ahrs=(Ahrs*)inAhrs;
    60     imu=(Imu*)ahrs->GetImu();
    61     getFrameworkManager()->AddDeviceToLog(imu);
     60  ahrs = (Ahrs *)inAhrs;
     61  imu = (Imu *)ahrs->GetImu();
     62  getFrameworkManager()->AddDeviceToLog(imu);
    6263}
    6364
    64 void Uav::SetBldc(const Bldc* inBldc) {
    65     bldc=(Bldc*)inBldc;
    66 }
     65void Uav::SetBldc(const Bldc *inBldc) { bldc = (Bldc *)inBldc; }
    6766
    68 void Uav::SetBatteryMonitor(const BatteryMonitor* inBattery) {
    69     battery=(BatteryMonitor*)inBattery;
     67void Uav::SetBatteryMonitor(const BatteryMonitor *inBattery) {
     68  battery = (BatteryMonitor *)inBattery;
    7069}
    7170
    7271void Uav::SetMultiplex(const UavMultiplex *inMultiplex) {
    73     multiplex=(UavMultiplex*)inMultiplex;
    74     getFrameworkManager()->AddDeviceToLog(multiplex);
     72  multiplex = (UavMultiplex *)inMultiplex;
     73  getFrameworkManager()->AddDeviceToLog(multiplex);
    7574}
    7675
    77 void Uav::SetVerticalCamera(const Camera* inVerticalCamera) {
    78     verticalCamera=(Camera*)inVerticalCamera;
     76void Uav::SetVerticalCamera(const Camera *inVerticalCamera) {
     77  verticalCamera = (Camera *)inVerticalCamera;
    7978}
    8079/*
    81 void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int VRPNSerialObjectId) {
     80void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int
     81VRPNSerialObjectId) {
    8282    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",vrpn_port,10000,80);
    8383    uav_vrpn=new MetaVrpnObject(vrpnclient,name,VRPNSerialObjectId);
    … …  
    8787*/
    8888void Uav::SetupVRPNAutoIP(string name) {
    89     SetupVRPN("192.168.147.197:3883",name);
     89  SetupVRPN("192.168.147.197:3883", name);
    9090}
    9191
    92 void Uav::SetupVRPN(string optitrack_address,string name) {
    93     vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",optitrack_address,10000,80);
    94     uav_vrpn=new MetaVrpnObject(vrpnclient,name);
     92void Uav::SetupVRPN(string optitrack_address, string name) {
     93  vrpnclient = new VrpnClient(getFrameworkManager(), "vrpn", optitrack_address,
     94                              10000, 80);
     95  uav_vrpn = new MetaVrpnObject(vrpnclient, name);
    9596
    96     getFrameworkManager()->AddDeviceToLog(uav_vrpn);
     97  getFrameworkManager()->AddDeviceToLog(uav_vrpn);
    9798
    98     GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),DataPlot::Green);
    99     //GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
    100     //GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
     99  GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),
     100                                 DataPlot::Green);
     101  // GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
     102  // GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
    101103}
    102104
    103 void Uav::StartSensors(void)  {
    104     if(vrpnclient!=NULL) {
    105         vrpnclient->Start();
    106     }
     105void Uav::StartSensors(void) {
     106  if (vrpnclient != NULL) {
     107    vrpnclient->Start();
     108  }
    107109}
    108110void Uav::UseDefaultPlot(void) {
    109     multiplex->UseDefaultPlot();
     111  multiplex->UseDefaultPlot();
    110112
    111     if(bldc->HasSpeedMeasurement()) {
    112         Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Speeds");
    113         DataPlot1D* plots[4];
    114         plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,7000);
    115         plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,7000);
    116         plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,7000);
    117         plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,7000);
     113  if (bldc->HasSpeedMeasurement()) {
     114    Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Speeds");
     115    DataPlot1D *plots[4];
     116    plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 7000);
     117    plots[1] =
     118        new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 7000);
     119    plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 7000);
     120    plots[3] =
     121        new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 7000);
    118122
    119         if(bldc->MotorsCount()==8) {
    120             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0),DataPlot::Red,"top");
    121             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),0),DataPlot::Blue,"bottom");
    122         } else {
    123             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0));
    124         }
     123    if (bldc->MotorsCount() == 8) {
     124      for (int i = 0; i < 4; i++)
     125        plots[i]->AddCurve(
     126            bldc->Output()->Element(multiplex->MultiplexValue(i), 0),
     127            DataPlot::Red, "top");
     128      for (int i = 0; i < 4; i++)
     129        plots[i]->AddCurve(
     130            bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 0),
     131            DataPlot::Blue, "bottom");
     132    } else {
     133      for (int i = 0; i < 4; i++)
     134        plots[i]->AddCurve(
     135            bldc->Output()->Element(multiplex->MultiplexValue(i), 0));
    125136    }
     137  }
    126138
    127     if(bldc->HasCurrentMeasurement()) {
    128         Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Currents");
    129         DataPlot1D* plots[4];
    130         plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,10);
    131         plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,10);
    132         plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,10);
    133         plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,10);
     139  if (bldc->HasCurrentMeasurement()) {
     140    Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Currents");
     141    DataPlot1D *plots[4];
     142    plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 10);
     143    plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 10);
     144    plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 10);
     145    plots[3] = new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 10);
    134146
    135         if(bldc->MotorsCount()==8) {
    136             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1),DataPlot::Red,"top");
    137             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),1),DataPlot::Blue,"bottom");
    138         } else {
    139             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1));
    140         }
     147    if (bldc->MotorsCount() == 8) {
     148      for (int i = 0; i < 4; i++)
     149        plots[i]->AddCurve(
     150            bldc->Output()->Element(multiplex->MultiplexValue(i), 1),
     151            DataPlot::Red, "top");
     152      for (int i = 0; i < 4; i++)
     153        plots[i]->AddCurve(
     154            bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 1),
     155            DataPlot::Blue, "bottom");
     156    } else {
     157      for (int i = 0; i < 4; i++)
     158        plots[i]->AddCurve(
     159            bldc->Output()->Element(multiplex->MultiplexValue(i), 1));
    141160    }
     161  }
    142162
    143     meta_us->UseDefaultPlot();
    144     ahrs->UseDefaultPlot();
     163  meta_us->UseDefaultPlot();
     164  ahrs->UseDefaultPlot();
    145165}
    146166
    147 UavMultiplex* Uav::GetUavMultiplex(void) const {
    148     return multiplex;
     167UavMultiplex *Uav::GetUavMultiplex(void) const { return multiplex; }
     168
     169Bldc *Uav::GetBldc(void) const { return bldc; }
     170
     171Ahrs *Uav::GetAhrs(void) const { return ahrs; }
     172
     173Imu *Uav::GetImu(void) const { return imu; }
     174
     175MetaUsRangeFinder *Uav::GetMetaUsRangeFinder(void) const { return meta_us; }
     176
     177UsRangeFinder *Uav::GetUsRangeFinder(void) const { return us; }
     178
     179BatteryMonitor *Uav::GetBatteryMonitor(void) const { return battery; }
     180
     181VrpnClient *Uav::GetVrpnClient(void) const {
     182  if (vrpnclient == NULL)
     183    Err("vrpn is not setup, call SetupVRPN before\n");
     184  return vrpnclient;
    149185}
    150186
    151 Bldc* Uav::GetBldc(void) const {
    152     return bldc;
    153 }
     187MetaVrpnObject *Uav::GetVrpnObject(void) const { return uav_vrpn; }
    154188
    155 Ahrs* Uav::GetAhrs(void) const {
    156     return ahrs;
    157 }
    158 
    159 Imu* Uav::GetImu(void) const {
    160     return imu;
    161 }
    162 
    163 MetaUsRangeFinder* Uav::GetMetaUsRangeFinder(void) const {
    164     return meta_us;
    165 }
    166 
    167 UsRangeFinder* Uav::GetUsRangeFinder(void) const {
    168     return us;
    169 }
    170 
    171 BatteryMonitor* Uav::GetBatteryMonitor(void) const {
    172     return battery;
    173 }
    174 
    175 VrpnClient* Uav::GetVrpnClient(void) const {
    176     if(vrpnclient==NULL) Err("vrpn is not setup, call SetupVRPN before\n");
    177     return vrpnclient;
    178 }
    179 
    180 MetaVrpnObject* Uav::GetVrpnObject(void) const {
    181     return uav_vrpn;
    182 }
    183 
    184 Camera* Uav::GetVerticalCamera(void) const {
    185     return verticalCamera;
    186 }
     189Camera *Uav::GetVerticalCamera(void) const { return verticalCamera; }
    187190
    188191} // end namespace meta

  • trunk/lib/FlairMeta/src/Uav.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace core {
    20         class FrameworkManager;
    21     }
    22     namespace filter {
    23         class Ahrs;
    24         class UavMultiplex;
    25     }
    26     namespace actuator {
    27         class Bldc;
    28     }
    29     namespace sensor {
    30         class UsRangeFinder;
    31         class BatteryMonitor;
    32         class VrpnClient;
    33         class Imu;
    34         class Camera;
    35     }
     19namespace core {
     20class FrameworkManager;
     21}
     22namespace filter {
     23class Ahrs;
     24class UavMultiplex;
     25}
     26namespace actuator {
     27class Bldc;
     28}
     29namespace sensor {
     30class UsRangeFinder;
     31class BatteryMonitor;
     32class VrpnClient;
     33class Imu;
     34class Camera;
     35}
    3636}
    3737
    3838namespace flair {
    3939namespace meta {
    40     class MetaUsRangeFinder;
    41     class MetaVrpnObject;
     40class MetaUsRangeFinder;
     41class MetaVrpnObject;
    4242
    43     /*! \class Uav
    44     *
    45     * \brief Base class to construct sensors/actuators depending on uav type
    46     */
    47     class Uav: public core::Object {
    48         public:
    49         /*
    50             typedef enum {
    51                 None,
    52                 Auto,//rt serial if hds x4 ou x8, auto ip sinon
    53                 AutoIP,
    54                 UserDefinedIP,
    55                 AutoSerialPort,
    56                 } VRPNType_t;
     43/*! \class Uav
     44*
     45* \brief Base class to construct sensors/actuators depending on uav type
    5746*/
    58             //uav_name: for optitrack
    59             Uav(core::FrameworkManager* parent,std::string name,filter::UavMultiplex *multiplex=NULL);
    60             ~Uav();
    61             void SetupVRPN(std::string optitrack_address,std::string name);
    62             //vrpn serial: broken, need to add serial port in uav specific code
    63             //void SetupVRPNSerial(core::SerialPort *vrpn_port,std::string name,int VRPNSerialObjectId);
    64             virtual void SetupVRPNAutoIP(std::string name);
     47class Uav : public core::Object {
     48public:
     49  /*
     50      typedef enum {
     51          None,
     52          Auto,//rt serial if hds x4 ou x8, auto ip sinon
     53          AutoIP,
     54          UserDefinedIP,
     55          AutoSerialPort,
     56          } VRPNType_t;
     57*/
     58  // uav_name: for optitrack
     59  Uav(core::FrameworkManager *parent, std::string name,
     60      filter::UavMultiplex *multiplex = NULL);
     61  ~Uav();
     62  void SetupVRPN(std::string optitrack_address, std::string name);
     63  // vrpn serial: broken, need to add serial port in uav specific code
     64  // void SetupVRPNSerial(core::SerialPort *vrpn_port,std::string name,int
     65  // VRPNSerialObjectId);
     66  virtual void SetupVRPNAutoIP(std::string name);
    6567
    66             virtual void StartSensors(void);
    67             void UseDefaultPlot(void);
    68             actuator::Bldc* GetBldc(void) const;
    69             filter::UavMultiplex* GetUavMultiplex(void) const;
    70             sensor::Imu* GetImu(void) const;
    71             filter::Ahrs* GetAhrs(void) const;
    72             MetaUsRangeFinder* GetMetaUsRangeFinder(void) const;
    73             sensor::UsRangeFinder* GetUsRangeFinder(void) const;
    74             sensor::BatteryMonitor* GetBatteryMonitor(void) const;
    75             sensor::VrpnClient* GetVrpnClient(void) const;
    76             meta::MetaVrpnObject* GetVrpnObject(void) const;
    77             sensor::Camera* GetVerticalCamera(void) const;
     68  virtual void StartSensors(void);
     69  void UseDefaultPlot(void);
     70  actuator::Bldc *GetBldc(void) const;
     71  filter::UavMultiplex *GetUavMultiplex(void) const;
     72  sensor::Imu *GetImu(void) const;
     73  filter::Ahrs *GetAhrs(void) const;
     74  MetaUsRangeFinder *GetMetaUsRangeFinder(void) const;
     75  sensor::UsRangeFinder *GetUsRangeFinder(void) const;
     76  sensor::BatteryMonitor *GetBatteryMonitor(void) const;
     77  sensor::VrpnClient *GetVrpnClient(void) const;
     78  meta::MetaVrpnObject *GetVrpnObject(void) const;
     79  sensor::Camera *GetVerticalCamera(void) const;
    7880
    79         protected:
    80             void SetBldc(const actuator::Bldc *bldc);
    81             void SetMultiplex(const filter::UavMultiplex *multiplex);
    82             void SetAhrs(const filter::Ahrs *ahrs);
    83             void SetUsRangeFinder(const sensor::UsRangeFinder *us);
    84             void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
    85             void SetVerticalCamera(const sensor::Camera *verticalCamera);
     81protected:
     82  void SetBldc(const actuator::Bldc *bldc);
     83  void SetMultiplex(const filter::UavMultiplex *multiplex);
     84  void SetAhrs(const filter::Ahrs *ahrs);
     85  void SetUsRangeFinder(const sensor::UsRangeFinder *us);
     86  void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
     87  void SetVerticalCamera(const sensor::Camera *verticalCamera);
    8688
    87         private:
    88             sensor::Imu* imu;
    89             filter::Ahrs* ahrs;
    90             actuator::Bldc* bldc;
    91             filter::UavMultiplex *multiplex;
    92             sensor::UsRangeFinder* us;
    93             MetaUsRangeFinder* meta_us;
    94             sensor::VrpnClient *vrpnclient;
    95             MetaVrpnObject* uav_vrpn;
    96             sensor::BatteryMonitor* battery;
    97             sensor::Camera* verticalCamera;
    98     };
     89private:
     90  sensor::Imu *imu;
     91  filter::Ahrs *ahrs;
     92  actuator::Bldc *bldc;
     93  filter::UavMultiplex *multiplex;
     94  sensor::UsRangeFinder *us;
     95  MetaUsRangeFinder *meta_us;
     96  sensor::VrpnClient *vrpnclient;
     97  MetaVrpnObject *uav_vrpn;
     98  sensor::BatteryMonitor *battery;
     99  sensor::Camera *verticalCamera;
     100};
    99101} // end namespace meta
    100102} // end namespace flair

  • trunk/lib/FlairMeta/src/UavFactory.cpp

    r10 r15  
    2929using namespace flair::meta;
    3030
    31 Uav* CreateUav(FrameworkManager* parent,string uav_name,string uav_type,UavMultiplex *multiplex) {
    32     /*if(uav_type=="ardrone2") {
     31Uav *CreateUav(FrameworkManager *parent, string uav_name, string uav_type,
     32               UavMultiplex *multiplex) {
     33  /*if(uav_type=="ardrone2") {
    3334        return new ArDrone2(parent,uav_name,multiplex);
    34     } else */if(uav_type=="hds_x4") {
    35         parent->Err("UAV type %s not yet implemented\n",uav_type.c_str());
    36         return NULL;
    37     } else if(uav_type=="hds_x8") {
    38         return new HdsX8(parent,uav_name,multiplex);
    39     } else if(uav_type=="xair") {
    40         return new XAir(parent,uav_name,multiplex);
    41     } else if(uav_type=="hds_xufo") {
    42         parent->Err("UAV type %s not yet implemented\n",uav_type.c_str());
    43         return NULL;
    44     } else if(uav_type.compare(0,7,"x4_simu")==0) {
    45         int simu_id=0;
    46         if(uav_type.size()>7) {
    47             simu_id=atoi(uav_type.substr (7,uav_type.size()-7).c_str());
    48         }
    49         return new SimuX4(parent,uav_name,simu_id,multiplex);
    50     } else if(uav_type.compare(0,7,"x8_simu") == 0) {
    51         int simu_id=0;
    52         if(uav_type.size()>7) {
    53             simu_id=atoi(uav_type.substr (7,uav_type.size()-7).c_str());
    54         }
    55         return new SimuX8(parent,uav_name,simu_id,multiplex);
    56     } else {
    57         parent->Err("UAV type %s unknown\n",uav_type.c_str());
    58         return NULL;
     35    } else */ if (uav_type == "hds_x4") {
     36    parent->Err("UAV type %s not yet implemented\n", uav_type.c_str());
     37    return NULL;
     38  } else if (uav_type == "hds_x8") {
     39    return new HdsX8(parent, uav_name, multiplex);
     40  } else if (uav_type == "xair") {
     41    return new XAir(parent, uav_name, multiplex);
     42  } else if (uav_type == "hds_xufo") {
     43    parent->Err("UAV type %s not yet implemented\n", uav_type.c_str());
     44    return NULL;
     45  } else if (uav_type.compare(0, 7, "x4_simu") == 0) {
     46    int simu_id = 0;
     47    if (uav_type.size() > 7) {
     48      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
    5949    }
     50    return new SimuX4(parent, uav_name, simu_id, multiplex);
     51  } else if (uav_type.compare(0, 7, "x8_simu") == 0) {
     52    int simu_id = 0;
     53    if (uav_type.size() > 7) {
     54      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
     55    }
     56    return new SimuX8(parent, uav_name, simu_id, multiplex);
     57  } else {
     58    parent->Err("UAV type %s unknown\n", uav_type.c_str());
     59    return NULL;
     60  }
    6061}

  • trunk/lib/FlairMeta/src/UavFactory.h

    r10 r15  
    2121#include <Uav.h>
    2222
    23 flair::meta::Uav* CreateUav(flair::core::FrameworkManager* parent,std::string uav_name,std::string uav_type,flair::filter::UavMultiplex *multiplex=NULL);
     23flair::meta::Uav *CreateUav(flair::core::FrameworkManager *parent,
     24                            std::string uav_name, std::string uav_type,
     25                            flair::filter::UavMultiplex *multiplex = NULL);
    2426
    2527#endif // UAVFACTORY

  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r10 r15  
    5151using namespace flair::meta;
    5252
    53 UavStateMachine::UavStateMachine(Uav* inUav,uint16_t ds3Port):
    54         Thread(getFrameworkManager(),"UavStateMachine",50),
    55         uav(inUav),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagZTrajectoryFinished(false) {
    56     altitudeState=AltitudeState_t::Stopped;
    57     uav->UseDefaultPlot();
    58 
    59     Tab *uavTab=new Tab(getFrameworkManager()->GetTabWidget(),"uav",0);
    60     buttonslayout=new GridLayout(uavTab->NewRow(),"buttons");
    61     button_kill=new PushButton(buttonslayout->NewRow(),"kill");
    62     button_start_log=new PushButton(buttonslayout->NewRow(),"start_log");
    63     button_stop_log=new PushButton(buttonslayout->LastRowLastCol(),"stop_log");
    64     button_take_off=new PushButton(buttonslayout->NewRow(),"take_off");
    65     button_land=new PushButton(buttonslayout->LastRowLastCol(),"land");
    66 
    67     Tab *lawTab=new Tab(getFrameworkManager()->GetTabWidget(),"control laws");
    68     TabWidget *tabWidget=new TabWidget(lawTab->NewRow(),"laws");
    69     setupLawTab=new Tab(tabWidget,"Setup");
    70     graphLawTab=new Tab(tabWidget,"Graphes");
    71 
    72     uRoll=new NestedSat(setupLawTab->At(0,0),"u_roll");
    73     uRoll->ConvertSatFromDegToRad();
    74     uRoll->UseDefaultPlot(graphLawTab->NewRow());
    75 
    76     uPitch=new NestedSat(setupLawTab->At(0,1),"u_pitch");
    77     uPitch->ConvertSatFromDegToRad();
    78     uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
    79 
    80     uYaw=new Pid(setupLawTab->At(0,2),"u_yaw");
    81     uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
    82 
    83     uZ=new PidThrust(setupLawTab->At(1,2),"u_z");
    84     uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
    85 
    86     getFrameworkManager()->AddDeviceToLog(uZ);
    87     uZ->AddDeviceToLog(uRoll);
    88     uZ->AddDeviceToLog(uPitch);
    89     uZ->AddDeviceToLog(uYaw);
    90 
    91     joy=new MetaDualShock3(getFrameworkManager(),"dualshock3",ds3Port,30);
    92     uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
    93 
    94     altitudeMode=AltitudeMode_t::Manual;
    95     orientationMode=OrientationMode_t::Manual;
    96     thrustMode=ThrustMode_t::Default;
    97     torqueMode=TorqueMode_t::Default;
    98 
    99     GroupBox* reglagesGroupbox=new GroupBox(uavTab->NewRow(),"takeoff/landing");
    100     desiredTakeoffAltitude=new DoubleSpinBox(reglagesGroupbox->NewRow(),"desired takeoff altitude"," m",0,5,0.1,2,1);
    101     desiredLandingAltitude=new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),"desired landing altitude"," m",0,1,0.1,1);
    102     altitudeTrajectory=new TrajectoryGenerator1D(uavTab->NewRow(),"alt cons","m");
    103     uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(0),DataPlot::Green);
    104     uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(1),DataPlot::Green);
    105 }
    106 
    107 UavStateMachine::~UavStateMachine() {
    108 }
    109 
    110 void UavStateMachine::AddDeviceToControlLawLog(const IODevice* device) {
    111     uZ->AddDeviceToLog(device);
    112 }
    113 
    114 void UavStateMachine::AddDataToControlLawLog(const core::io_data* data) {
    115     uZ->AddDataToLog(data);
    116 }
    117 
    118 const TargetController *UavStateMachine::GetJoystick(void) const {
    119     return joy;
    120 }
     53UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
     54    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
     55      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
     56      flagZTrajectoryFinished(false) {
     57  altitudeState = AltitudeState_t::Stopped;
     58  uav->UseDefaultPlot();
     59
     60  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
     61  buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
     62  button_kill = new PushButton(buttonslayout->NewRow(), "kill");
     63  button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
     64  button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
     65  button_take_off = new PushButton(buttonslayout->NewRow(), "take_off");
     66  button_land = new PushButton(buttonslayout->LastRowLastCol(), "land");
     67
     68  Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
     69  TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
     70  setupLawTab = new Tab(tabWidget, "Setup");
     71  graphLawTab = new Tab(tabWidget, "Graphes");
     72
     73  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
     74  uRoll->ConvertSatFromDegToRad();
     75  uRoll->UseDefaultPlot(graphLawTab->NewRow());
     76
     77  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
     78  uPitch->ConvertSatFromDegToRad();
     79  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
     80
     81  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
     82  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
     83
     84  uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
     85  uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
     86
     87  getFrameworkManager()->AddDeviceToLog(uZ);
     88  uZ->AddDeviceToLog(uRoll);
     89  uZ->AddDeviceToLog(uPitch);
     90  uZ->AddDeviceToLog(uYaw);
     91
     92  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
     93  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
     94
     95  altitudeMode = AltitudeMode_t::Manual;
     96  orientationMode = OrientationMode_t::Manual;
     97  thrustMode = ThrustMode_t::Default;
     98  torqueMode = TorqueMode_t::Default;
     99
     100  GroupBox *reglagesGroupbox =
     101      new GroupBox(uavTab->NewRow(), "takeoff/landing");
     102  desiredTakeoffAltitude =
     103      new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
     104                        " m", 0, 5, 0.1, 2, 1);
     105  desiredLandingAltitude =
     106      new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
     107                        "desired landing altitude", " m", 0, 1, 0.1, 1);
     108  altitudeTrajectory =
     109      new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
     110  uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
     111      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
     112  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
     113      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
     114}
     115
     116UavStateMachine::~UavStateMachine() {}
     117
     118void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
     119  uZ->AddDeviceToLog(device);
     120}
     121
     122void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
     123  uZ->AddDataToLog(data);
     124}
     125
     126const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
    121127
    122128const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
    123             return currentQuaternion;
     129  return currentQuaternion;
    124130}
    125131
    126132const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
    127     return currentAngularSpeed;
    128 }
    129 
    130 const Uav *UavStateMachine::GetUav(void) const {
    131     return uav;
    132 }
    133 
    134 void UavStateMachine::AltitudeValues(float &altitude,float &verticalSpeed) const{
    135     FailSafeAltitudeValues(altitude, verticalSpeed);
    136 }
    137 
    138 void UavStateMachine::FailSafeAltitudeValues(float &altitude,float &verticalSpeed) const {
    139     altitude=uav->GetMetaUsRangeFinder()->z();
    140     verticalSpeed=uav->GetMetaUsRangeFinder()->Vz();
     133  return currentAngularSpeed;
     134}
     135
     136const Uav *UavStateMachine::GetUav(void) const { return uav; }
     137
     138void UavStateMachine::AltitudeValues(float &altitude,
     139                                     float &verticalSpeed) const {
     140  FailSafeAltitudeValues(altitude, verticalSpeed);
     141}
     142
     143void UavStateMachine::FailSafeAltitudeValues(float &altitude,
     144                                             float &verticalSpeed) const {
     145  altitude = uav->GetMetaUsRangeFinder()->z();
     146  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
    141147}
    142148
    143149void UavStateMachine::Run() {
    144     WarnUponSwitches(true);
    145     uav->StartSensors();
    146 
    147     if(getFrameworkManager()->ErrorOccured()==true) {
    148         SafeStop();
    149     }
    150 
    151     while(!ToBeStopped()) {
    152         SecurityCheck();
    153 
    154         //get controller inputs
    155         CheckJoystick();
    156         CheckPushButton();
    157 
    158         if(IsPeriodSet()) {
    159             WaitPeriod();
    160         } else {
    161             WaitUpdate(uav->GetAhrs());
    162         }
    163         needToComputeDefaultTorques=true;
    164         needToComputeDefaultThrust=true;
    165 
    166         SignalEvent(Event_t::EnteringControlLoop);
    167 
    168         ComputeOrientation();
    169         ComputeAltitude();
    170 
    171         //compute thrust and torques to apply
    172         ComputeTorques();
    173         ComputeThrust();//logs are added to uz, so it must be updated at last
    174 
    175         // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set thrust (value between 0 and 1)
    176         uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
    177         uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
    178         uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
    179         uav->GetUavMultiplex()->SetThrust(-currentThrust);//on raisonne en negatif sur l'altitude, a revoir avec les equations
    180         uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
    181         uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
    182         uav->GetUavMultiplex()->SetYawTrim(0);
    183         uav->GetUavMultiplex()->Update(GetTime());
    184     }
    185 
    186     WarnUponSwitches(false);
     150  WarnUponSwitches(true);
     151  uav->StartSensors();
     152
     153  if (getFrameworkManager()->ErrorOccured() == true) {
     154    SafeStop();
     155  }
     156
     157  while (!ToBeStopped()) {
     158    SecurityCheck();
     159
     160    // get controller inputs
     161    CheckJoystick();
     162    CheckPushButton();
     163
     164    if (IsPeriodSet()) {
     165      WaitPeriod();
     166    } else {
     167      WaitUpdate(uav->GetAhrs());
     168    }
     169    needToComputeDefaultTorques = true;
     170    needToComputeDefaultThrust = true;
     171
     172    SignalEvent(Event_t::EnteringControlLoop);
     173
     174    ComputeOrientation();
     175    ComputeAltitude();
     176
     177    // compute thrust and torques to apply
     178    ComputeTorques();
     179    ComputeThrust(); // logs are added to uz, so it must be updated at last
     180
     181    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
     182    // thrust (value between 0 and 1)
     183    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
     184    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
     185    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
     186    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
     187                                                       // sur l'altitude, a
     188                                                       // revoir avec les
     189                                                       // equations
     190    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
     191    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
     192    uav->GetUavMultiplex()->SetYawTrim(0);
     193    uav->GetUavMultiplex()->Update(GetTime());
     194  }
     195
     196  WarnUponSwitches(false);
    187197}
    188198
    189199void UavStateMachine::ComputeOrientation(void) {
    190     if (failSafeMode) {
    191         GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
     200  if (failSafeMode) {
     201    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     202                                                          currentAngularSpeed);
     203  } else {
     204    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     205                                                   currentAngularSpeed);
     206  }
     207}
     208
     209const AhrsData *UavStateMachine::GetOrientation(void) const {
     210  return GetDefaultOrientation();
     211}
     212
     213const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
     214  return uav->GetAhrs()->GetDatas();
     215}
     216
     217void UavStateMachine::ComputeAltitude(void) {
     218  if (failSafeMode) {
     219    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
     220  } else {
     221    AltitudeValues(currentAltitude, currentVerticalSpeed);
     222  }
     223}
     224
     225void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
     226                                               float &refVerticalVelocity) {
     227  if (altitudeMode == AltitudeMode_t::Manual) {
     228    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
     229  } else {
     230    GetReferenceAltitude(refAltitude, refVerticalVelocity);
     231  }
     232}
     233
     234void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
     235                                                  float &refVerticalVelocity) {
     236  float zc, dzc;
     237
     238  switch (altitudeState) {
     239  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
     240  // until we detect a take off of 0.03m (hard coded value) above the ground.
     241  case AltitudeState_t::TakingOff: {
     242    if (currentAltitude > groundAltitude + 0.03) {
     243      altitudeTrajectory->StartTraj(currentAltitude,
     244                                    desiredTakeoffAltitude->Value());
     245      altitudeState = AltitudeState_t::Stabilized;
     246      SignalEvent(Event_t::Stabilized);
     247    }
     248    break;
     249  }
     250  // landing, only check if we reach desired landing altitude
     251  case AltitudeState_t::StartLanding: {
     252    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
     253      // The Uav target altitude has reached its landing value (typically 0)
     254      // but the real Uav altitude may not have reach this value yet because of
     255      // command delay. Moreover, it
     256      // may never exactly reach this value if the ground is not perfectly
     257      // leveled (critical case: there's a
     258      // deep and narrow hole right in the sensor line of sight). That's why we
     259      // have a 2 phases landing strategy.
     260      altitudeState = AltitudeState_t::FinishLanding;
     261      SignalEvent(Event_t::FinishLanding);
     262      joy->SetLedOFF(1); // DualShock3::led1
     263    }
     264  }
     265  // stabilized: check if z trajectory is finished
     266  case AltitudeState_t::Stabilized: {
     267    if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
     268      SignalEvent(Event_t::ZTrajectoryFinished);
     269      flagZTrajectoryFinished = true;
     270    }
     271    if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
     272      flagZTrajectoryFinished = false;
     273      altitudeTrajectory->StartTraj(currentAltitude,
     274                                    desiredTakeoffAltitude->Value());
     275      joy->SetZRef(0);
     276    }
     277  }
     278  }
     279
     280  // Récupère les consignes (du joystick dans l'implémentation par défaut). La
     281  // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
     282  // la manette
     283  zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
     284                    // generator
     285  dzc = joy->DzRef();
     286
     287  // z control law
     288  altitudeTrajectory->SetPositionOffset(zc);
     289  altitudeTrajectory->SetSpeedOffset(dzc);
     290
     291  altitudeTrajectory->Update(GetTime());
     292  refAltitude = altitudeTrajectory->Position();
     293  refVerticalVelocity = altitudeTrajectory->Speed();
     294}
     295
     296void UavStateMachine::GetReferenceAltitude(float &refAltitude,
     297                                           float &refVerticalVelocity) {
     298  Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
     299               "switching back to safe mode\n");
     300  EnterFailSafeMode();
     301};
     302
     303void UavStateMachine::ComputeThrust(void) {
     304  if (altitudeMode == AltitudeMode_t::Manual) {
     305    currentThrust = ComputeDefaultThrust();
     306  } else {
     307    currentThrust = ComputeCustomThrust();
     308  }
     309}
     310
     311float UavStateMachine::ComputeDefaultThrust(void) {
     312  if (needToComputeDefaultThrust) {
     313    // compute desired altitude
     314    float refAltitude, refVerticalVelocity;
     315    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
     316
     317    switch (altitudeState) {
     318    case AltitudeState_t::TakingOff: {
     319      // The progressive increase in motor speed is used to evaluate the motor
     320      // speed that compensate the uav weight. This value
     321      // will be used as an offset for altitude control afterwards
     322      uZ->OffsetStepUp();
     323      break;
     324    }
     325    case AltitudeState_t::StartLanding:
     326    case AltitudeState_t::Stabilized: {
     327      float p_error = currentAltitude - refAltitude;
     328      float d_error = currentVerticalSpeed - refVerticalVelocity;
     329      uZ->SetValues(p_error, d_error);
     330      break;
     331    }
     332    // decrease motor speed in open loop until value offset_g , uav should have
     333    // already landed or be very close to at this point
     334    case AltitudeState_t::FinishLanding: {
     335      if (uZ->OffsetStepDown() == false) {
     336        StopMotors();
     337      }
     338      break;
     339    }
     340    }
     341    uZ->Update(GetTime());
     342
     343    savedDefaultThrust = uZ->Output();
     344    needToComputeDefaultThrust = false;
     345  }
     346
     347  return savedDefaultThrust;
     348}
     349
     350float UavStateMachine::ComputeCustomThrust(void) {
     351  Thread::Warn("Default GetThrust method is not overloaded => switching back "
     352               "to safe mode\n");
     353  EnterFailSafeMode();
     354  return ComputeDefaultThrust();
     355}
     356
     357const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
     358  if (orientationMode == OrientationMode_t::Manual) {
     359    return GetDefaultReferenceOrientation();
     360  } else {
     361    return GetReferenceOrientation();
     362  }
     363}
     364
     365const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
     366  // We directly control yaw, pitch, roll angles
     367  return joy->GetReferenceOrientation();
     368}
     369
     370const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
     371  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
     372               "switching back to safe mode\n");
     373  EnterFailSafeMode();
     374  return GetDefaultReferenceOrientation();
     375}
     376
     377void UavStateMachine::ComputeTorques(void) {
     378  if (torqueMode == TorqueMode_t::Default) {
     379    ComputeDefaultTorques(currentTorques);
     380  } else {
     381    ComputeCustomTorques(currentTorques);
     382  }
     383}
     384
     385void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
     386  if (needToComputeDefaultTorques) {
     387    const AhrsData *refOrientation = ComputeReferenceOrientation();
     388    Quaternion refQuaternion;
     389    Vector3D refAngularRates;
     390    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
     391                                                 refAngularRates);
     392    Euler refAngles = refQuaternion.ToEuler();
     393    Euler currentAngles = currentQuaternion.ToEuler();
     394
     395    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
     396                    currentAngularSpeed.z - refAngularRates.z);
     397    uYaw->Update(GetTime());
     398    torques.yaw = uYaw->Output();
     399
     400    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
     401                      currentAngularSpeed.y);
     402    uPitch->Update(GetTime());
     403    torques.pitch = uPitch->Output();
     404
     405    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
     406    uRoll->Update(GetTime());
     407    torques.roll = uRoll->Output();
     408
     409    savedDefaultTorques = torques;
     410    needToComputeDefaultTorques = false;
     411  } else {
     412    torques = savedDefaultTorques;
     413  }
     414}
     415
     416void UavStateMachine::ComputeCustomTorques(Euler &torques) {
     417  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
     418               "switching back to safe mode\n");
     419  EnterFailSafeMode();
     420  ComputeDefaultTorques(torques);
     421}
     422
     423void UavStateMachine::TakeOff(void) {
     424  flagZTrajectoryFinished = false;
     425
     426  if (altitudeState == AltitudeState_t::Stopped) { // &&
     427    // GetBatteryMonitor()->IsBatteryLow()==false)
     428    // The uav always takes off in fail safe mode
     429    EnterFailSafeMode();
     430    joy->SetLedOFF(4); // DualShock3::led4
     431    joy->SetLedOFF(1); // DualShock3::led1
     432    joy->Rumble(0x70);
     433    joy->SetZRef(0);
     434
     435    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
     436                     // valeur initiale (station sol)
     437
     438    uav->GetUavMultiplex()->LockUserInterface();
     439    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
     440    // consignes moteurs
     441    // sans les faire tourner effectivement (en déplaçant à la main le drone)
     442    uav->GetBldc()->SetEnabled(true);
     443    groundAltitude = currentAltitude;
     444    altitudeState = AltitudeState_t::TakingOff;
     445
     446    SignalEvent(Event_t::TakingOff);
     447  } else {
     448    joy->ErrorNotify();
     449  }
     450}
     451
     452void UavStateMachine::Land(void) {
     453  if (altitudeMode != AltitudeMode_t::Manual) {
     454    SetAltitudeMode(AltitudeMode_t::Manual);
     455  }
     456  if (altitudeState == AltitudeState_t::Stabilized) {
     457    joy->SetLedOFF(4); // DualShock3::led4
     458    joy->Rumble(0x70);
     459
     460    altitudeTrajectory->StopTraj();
     461    joy->SetZRef(0);
     462    altitudeTrajectory->StartTraj(
     463        currentAltitude,
     464        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
     465    SignalEvent(Event_t::StartLanding);
     466    altitudeState = AltitudeState_t::StartLanding;
     467  } else {
     468    joy->ErrorNotify();
     469  }
     470}
     471
     472void UavStateMachine::SignalEvent(Event_t event) {
     473  switch (event) {
     474  case Event_t::StartLanding:
     475    Thread::Info("Altitude: entering 'StartLanding' state\n");
     476    break;
     477  case Event_t::Stopped:
     478    Thread::Info("Altitude: entering 'Stopped' state\n");
     479    break;
     480  case Event_t::TakingOff:
     481    Thread::Info("Altitude: taking off\n");
     482    break;
     483  case Event_t::Stabilized:
     484    Thread::Info("Altitude: entering 'Stabilized' state\n");
     485    break;
     486  case Event_t::FinishLanding:
     487    Thread::Info("Altitude: entering 'FinishLanding' state\n");
     488    break;
     489  case Event_t::EmergencyStop:
     490    Thread::Info("Emergency stop!\n");
     491    break;
     492  }
     493}
     494
     495void UavStateMachine::EmergencyStop(void) {
     496  if (altitudeState != AltitudeState_t::Stopped) {
     497    StopMotors();
     498    EnterFailSafeMode();
     499    joy->Rumble(0x70);
     500    SignalEvent(Event_t::EmergencyStop);
     501  }
     502}
     503
     504void UavStateMachine::StopMotors(void) {
     505  joy->FlashLed(1, 10, 10); // DualShock3::led1
     506  uav->GetBldc()->SetEnabled(false);
     507  uav->GetUavMultiplex()->UnlockUserInterface();
     508  SignalEvent(Event_t::Stopped);
     509  altitudeState = AltitudeState_t::Stopped;
     510  uav->GetAhrs()->UnlockUserInterface();
     511
     512  uZ->SetValues(0, 0);
     513  uZ->Reset();
     514}
     515
     516GridLayout *UavStateMachine::GetButtonsLayout(void) const {
     517  return buttonslayout;
     518}
     519
     520void UavStateMachine::SecurityCheck(void) {
     521  MandatorySecurityCheck();
     522  ExtraSecurityCheck();
     523}
     524
     525void UavStateMachine::MandatorySecurityCheck(void) {
     526  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
     527    flagConnectionLost = true;
     528    Thread::Err("Connection lost\n");
     529    EnterFailSafeMode();
     530    if (altitudeState == AltitudeState_t::Stopped) {
     531      SafeStop();
    192532    } else {
    193         GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
    194     }
    195 }
    196 
    197 const AhrsData *UavStateMachine::GetOrientation(void) const {
    198     return GetDefaultOrientation();
    199 }
    200 
    201 const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
    202     return uav->GetAhrs()->GetDatas();
    203 }
    204 
    205 void UavStateMachine::ComputeAltitude(void) {
    206     if (failSafeMode) {
    207         FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
    208     } else {
    209         AltitudeValues(currentAltitude,currentVerticalSpeed);
    210     }
    211 }
    212 
    213 void UavStateMachine::ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    214     if (altitudeMode==AltitudeMode_t::Manual) {
    215         GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
    216     } else {
    217         GetReferenceAltitude(refAltitude, refVerticalVelocity);
    218     }
    219 }
    220 
    221 void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    222     float zc,dzc;
    223 
    224     switch(altitudeState) {
    225         //initiate a takeoff: increase motor speed in open loop (see ComputeThrust) until we detect a take off of 0.03m (hard coded value) above the ground.
    226         case AltitudeState_t::TakingOff: {
    227             if(currentAltitude>groundAltitude+0.03) {
    228                 altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
    229                 altitudeState=AltitudeState_t::Stabilized;
    230                 SignalEvent(Event_t::Stabilized);
    231             }
    232             break;
    233         }
    234         //landing, only check if we reach desired landing altitude
    235         case AltitudeState_t::StartLanding: {
    236             if(altitudeTrajectory->Position()==desiredLandingAltitude->Value()) {
    237                 //The Uav target altitude has reached its landing value (typically 0)
    238                 // but the real Uav altitude may not have reach this value yet because of command delay. Moreover, it
    239                 // may never exactly reach this value if the ground is not perfectly leveled (critical case: there's a
    240                 // deep and narrow hole right in the sensor line of sight). That's why we have a 2 phases landing strategy.
    241                 altitudeState=AltitudeState_t::FinishLanding;
    242                 SignalEvent(Event_t::FinishLanding);
    243                 joy->SetLedOFF(1);//DualShock3::led1
    244             }
    245         }
    246         //stabilized: check if z trajectory is finished
    247         case AltitudeState_t::Stabilized: {
    248             if(!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
    249                 SignalEvent(Event_t::ZTrajectoryFinished);
    250                 flagZTrajectoryFinished=true;
    251             }
    252             if(flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
    253                 flagZTrajectoryFinished=false;
    254                 altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
    255                 joy->SetZRef(0);
    256             }
    257         }
    258     }
    259 
    260     //Récupère les consignes (du joystick dans l'implémentation par défaut). La consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par la manette
    261     zc=joy->ZRef();//a revoir, la position offset devrait se calculer dans le generator
    262     dzc=joy->DzRef();
    263 
    264     //z control law
    265     altitudeTrajectory->SetPositionOffset(zc);
    266     altitudeTrajectory->SetSpeedOffset(dzc);
    267 
    268     altitudeTrajectory->Update(GetTime());
    269     refAltitude=altitudeTrajectory->Position();
    270     refVerticalVelocity=altitudeTrajectory->Speed();
    271 }
    272 
    273 void UavStateMachine::GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    274     Thread::Warn("Default GetReferenceAltitude method is not overloaded => switching back to safe mode\n");
     533      Land();
     534    }
     535  }
     536  if ((altitudeState == AltitudeState_t::TakingOff ||
     537       altitudeState == AltitudeState_t::Stabilized) &&
     538      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
     539    flagBatteryLow = true;
     540    Printf("Battery Low\n");
    275541    EnterFailSafeMode();
    276 };
    277 
    278 void UavStateMachine::ComputeThrust(void) {
    279     if (altitudeMode==AltitudeMode_t::Manual) {
    280         currentThrust=ComputeDefaultThrust();
    281     } else {
    282         currentThrust=ComputeCustomThrust();
    283     }
    284 }
    285 
    286 float UavStateMachine::ComputeDefaultThrust(void) {
    287     if(needToComputeDefaultThrust) {
    288         //compute desired altitude
    289         float refAltitude, refVerticalVelocity;
    290         ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
    291 
    292         switch(altitudeState) {
    293             case AltitudeState_t::TakingOff: {
    294                 //The progressive increase in motor speed is used to evaluate the motor speed that compensate the uav weight. This value
    295                 //will be used as an offset for altitude control afterwards
    296                 uZ->OffsetStepUp();
    297                 break;
    298             }
    299             case AltitudeState_t::StartLanding:
    300             case AltitudeState_t::Stabilized: {
    301                 float p_error=currentAltitude-refAltitude;
    302                 float d_error=currentVerticalSpeed-refVerticalVelocity;
    303                 uZ->SetValues(p_error,d_error);
    304                 break;
    305             }
    306             //decrease motor speed in open loop until value offset_g , uav should have already landed or be very close to at this point
    307             case AltitudeState_t::FinishLanding: {
    308                 if(uZ->OffsetStepDown()==false) {
    309                     StopMotors();
    310                 }
    311                 break;
    312             }
    313         }
    314         uZ->Update(GetTime());
    315 
    316         savedDefaultThrust=uZ->Output();
    317         needToComputeDefaultThrust=false;
    318     }
    319 
    320     return savedDefaultThrust;
    321 }
    322 
    323 float UavStateMachine::ComputeCustomThrust(void) {
    324     Thread::Warn("Default GetThrust method is not overloaded => switching back to safe mode\n");
    325     EnterFailSafeMode();
    326     return ComputeDefaultThrust();
    327 }
    328 
    329 const AhrsData* UavStateMachine::ComputeReferenceOrientation(void) {
    330     if(orientationMode==OrientationMode_t::Manual) {
    331         return GetDefaultReferenceOrientation();
    332     } else {
    333         return GetReferenceOrientation();
    334     }
    335 }
    336 
    337 const AhrsData* UavStateMachine::GetDefaultReferenceOrientation(void) const {
    338     // We directly control yaw, pitch, roll angles
    339     return joy->GetReferenceOrientation();
    340 }
    341 
    342 const AhrsData* UavStateMachine::GetReferenceOrientation(void) {
    343     Thread::Warn("Default GetReferenceOrientation method is not overloaded => switching back to safe mode\n");
    344     EnterFailSafeMode();
    345     return GetDefaultReferenceOrientation();
    346 }
    347 
    348 void UavStateMachine::ComputeTorques(void) {
    349     if(torqueMode==TorqueMode_t::Default) {
    350         ComputeDefaultTorques(currentTorques);
    351     } else {
    352         ComputeCustomTorques(currentTorques);
    353     }
    354 }
    355 
    356 void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
    357     if(needToComputeDefaultTorques) {
    358         const AhrsData *refOrientation=ComputeReferenceOrientation();
    359         Quaternion refQuaternion;
    360         Vector3D refAngularRates;
    361         refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
    362         Euler refAngles=refQuaternion.ToEuler();
    363         Euler currentAngles=currentQuaternion.ToEuler();
    364 
    365         uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),currentAngularSpeed.z-refAngularRates.z);
    366         uYaw->Update(GetTime());
    367         torques.yaw=uYaw->Output();
    368 
    369         uPitch->SetValues(refAngles.pitch,currentAngles.pitch,currentAngularSpeed.y);
    370         uPitch->Update(GetTime());
    371         torques.pitch=uPitch->Output();
    372 
    373         uRoll->SetValues(refAngles.roll,currentAngles.roll,currentAngularSpeed.x);
    374         uRoll->Update(GetTime());
    375         torques.roll=uRoll->Output();
    376 
    377         savedDefaultTorques=torques;
    378         needToComputeDefaultTorques=false;
    379     } else {
    380         torques=savedDefaultTorques;
    381     }
    382 }
    383 
    384 void UavStateMachine::ComputeCustomTorques(Euler &torques) {
    385     Thread::Warn("Default ComputeCustomTorques method is not overloaded => switching back to safe mode\n");
    386     EnterFailSafeMode();
    387     ComputeDefaultTorques(torques);
    388 }
    389 
    390 void UavStateMachine::TakeOff(void) {
    391     flagZTrajectoryFinished=false;
    392 
    393     if(altitudeState==AltitudeState_t::Stopped) {// && GetBatteryMonitor()->IsBatteryLow()==false)
    394         //The uav always takes off in fail safe mode
    395         EnterFailSafeMode();
    396         joy->SetLedOFF(4);//DualShock3::led4
    397         joy->SetLedOFF(1);//DualShock3::led1
    398         joy->Rumble(0x70);
    399         joy->SetZRef(0);
    400 
    401         uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa valeur initiale (station sol)
    402 
    403         uav->GetUavMultiplex()->LockUserInterface();
    404         //Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les consignes moteurs
    405         // sans les faire tourner effectivement (en déplaçant à la main le drone)
    406         uav->GetBldc()->SetEnabled(true);
    407         groundAltitude=currentAltitude;
    408         altitudeState=AltitudeState_t::TakingOff;
    409 
    410         SignalEvent(Event_t::TakingOff);
    411     } else {
     542    Land();
     543  }
     544}
     545
     546void UavStateMachine::CheckJoystick(void) {
     547  GenericCheckJoystick();
     548  ExtraCheckJoystick();
     549}
     550
     551void UavStateMachine::GenericCheckJoystick(void) {
     552  static bool isEmergencyStopButtonPressed = false;
     553  static bool isTakeOffButtonPressed = false;
     554  static bool isSafeModeButtonPressed = false;
     555
     556  if (joy->IsButtonPressed(1)) { // select
     557    if (!isEmergencyStopButtonPressed) {
     558      isEmergencyStopButtonPressed = true;
     559      Thread::Info("Emergency stop from joystick\n");
     560      EmergencyStop();
     561    }
     562  } else
     563    isEmergencyStopButtonPressed = false;
     564
     565  if (joy->IsButtonPressed(0)) { // start
     566    if (!isTakeOffButtonPressed) {
     567      isTakeOffButtonPressed = true;
     568      switch (altitudeState) {
     569      case AltitudeState_t::Stopped:
     570        TakeOff();
     571        break;
     572      case AltitudeState_t::Stabilized:
     573        Land();
     574        break;
     575      default:
    412576        joy->ErrorNotify();
    413     }
    414 }
    415 
    416 void UavStateMachine::Land(void) {
    417     if (altitudeMode!=AltitudeMode_t::Manual) {
    418         SetAltitudeMode(AltitudeMode_t::Manual);
    419     }
    420     if(altitudeState==AltitudeState_t::Stabilized) {
    421         joy->SetLedOFF(4);//DualShock3::led4
    422         joy->Rumble(0x70);
    423 
    424         altitudeTrajectory->StopTraj();
    425         joy->SetZRef(0);
    426         altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
    427         SignalEvent(Event_t::StartLanding);
    428         altitudeState=AltitudeState_t::StartLanding;
    429     } else {
    430         joy->ErrorNotify();
    431     }
    432 }
    433 
    434 void UavStateMachine::SignalEvent(Event_t event) {
    435     switch(event) {
    436     case Event_t::StartLanding:
    437         Thread::Info("Altitude: entering 'StartLanding' state\n");
    438577        break;
    439     case Event_t::Stopped:
    440         Thread::Info("Altitude: entering 'Stopped' state\n");
    441         break;
    442     case Event_t::TakingOff:
    443         Thread::Info("Altitude: taking off\n");
    444         break;
    445     case Event_t::Stabilized:
    446         Thread::Info("Altitude: entering 'Stabilized' state\n");
    447         break;
    448     case Event_t::FinishLanding:
    449         Thread::Info("Altitude: entering 'FinishLanding' state\n");
    450         break;
    451     case Event_t::EmergencyStop:
    452         Thread::Info("Emergency stop!\n");
    453         break;
    454     }
    455 }
    456 
    457 void UavStateMachine::EmergencyStop(void) {
    458     if(altitudeState!=AltitudeState_t::Stopped) {
    459         StopMotors();
    460         EnterFailSafeMode();
    461         joy->Rumble(0x70);
    462         SignalEvent(Event_t::EmergencyStop);
    463     }
    464 }
    465 
    466 void UavStateMachine::StopMotors(void)
    467 {
    468     joy->FlashLed(1,10,10);//DualShock3::led1
    469     uav->GetBldc()->SetEnabled(false);
    470     uav->GetUavMultiplex()->UnlockUserInterface();
    471     SignalEvent(Event_t::Stopped);
    472     altitudeState=AltitudeState_t::Stopped;
    473     uav->GetAhrs()->UnlockUserInterface();
    474 
    475     uZ->SetValues(0,0);
    476     uZ->Reset();
    477 }
    478 
    479 GridLayout* UavStateMachine::GetButtonsLayout(void) const {
    480     return buttonslayout;
    481 }
    482 
    483 void UavStateMachine::SecurityCheck(void) {
    484     MandatorySecurityCheck();
    485     ExtraSecurityCheck();
    486 }
    487 
    488 void UavStateMachine::MandatorySecurityCheck(void) {
    489     if(getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
    490         flagConnectionLost=true;
    491         Thread::Err("Connection lost\n");
    492         EnterFailSafeMode();
    493         if(altitudeState==AltitudeState_t::Stopped) {
    494             SafeStop();
    495         } else {
    496             Land();
    497         }
    498     }
    499     if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
    500         flagBatteryLow=true;
    501         Printf("Battery Low\n");
    502         EnterFailSafeMode();
    503         Land();
    504     }
    505 }
    506 
    507 void UavStateMachine::CheckJoystick(void) {
    508     GenericCheckJoystick();
    509     ExtraCheckJoystick();
    510 }
    511 
    512 void UavStateMachine::GenericCheckJoystick(void) {
    513     static bool isEmergencyStopButtonPressed=false;
    514     static bool isTakeOffButtonPressed=false;
    515     static bool isSafeModeButtonPressed=false;
    516 
    517    if(joy->IsButtonPressed(1)) { //select
    518         if (!isEmergencyStopButtonPressed) {
    519             isEmergencyStopButtonPressed=true;
    520             Thread::Info("Emergency stop from joystick\n");
    521             EmergencyStop();
    522         }
    523     } else isEmergencyStopButtonPressed=false;
    524 
    525     if(joy->IsButtonPressed(0)) { //start
    526         if (!isTakeOffButtonPressed) {
    527             isTakeOffButtonPressed=true;
    528             switch(altitudeState) {
    529                 case AltitudeState_t::Stopped:
    530                     TakeOff();
    531                     break;
    532                 case AltitudeState_t::Stabilized:
    533                     Land();
    534                     break;
    535                 default:
    536                     joy->ErrorNotify();
    537                     break;
    538             }
    539         }
    540     } else isTakeOffButtonPressed=false;
    541 
    542     //cross
    543     //gsanahuj:conflict with Majd programs.
    544     //check if l1,l2,r1 and r2 are not pressed
    545     //to allow a combination in user program
    546     if(joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) && !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) && !joy->IsButtonPressed(10)) {
    547         if (!isSafeModeButtonPressed) {
    548             isSafeModeButtonPressed=true;
    549             Thread::Info("Entering fail safe mode\n");
    550             EnterFailSafeMode();
    551         }
    552     } else isSafeModeButtonPressed=false;
     578      }
     579    }
     580  } else
     581    isTakeOffButtonPressed = false;
     582
     583  // cross
     584  // gsanahuj:conflict with Majd programs.
     585  // check if l1,l2,r1 and r2 are not pressed
     586  // to allow a combination in user program
     587  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
     588      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
     589      !joy->IsButtonPressed(10)) {
     590    if (!isSafeModeButtonPressed) {
     591      isSafeModeButtonPressed = true;
     592      Thread::Info("Entering fail safe mode\n");
     593      EnterFailSafeMode();
     594    }
     595  } else
     596    isSafeModeButtonPressed = false;
    553597}
    554598
    555599void UavStateMachine::CheckPushButton(void) {
    556     GenericCheckPushButton();
    557     ExtraCheckPushButton();
     600  GenericCheckPushButton();
     601  ExtraCheckPushButton();
    558602}
    559603
    560604void UavStateMachine::GenericCheckPushButton(void) {
    561     if(button_kill->Clicked()==true) SafeStop();
    562     if(button_take_off->Clicked()==true) TakeOff();
    563     if(button_land->Clicked()==true) Land();
    564     if(button_start_log->Clicked()==true) getFrameworkManager()->StartLog();
    565     if(button_stop_log->Clicked()==true) getFrameworkManager()->StopLog();
     605  if (button_kill->Clicked() == true)
     606    SafeStop();
     607  if (button_take_off->Clicked() == true)
     608    TakeOff();
     609  if (button_land->Clicked() == true)
     610    Land();
     611  if (button_start_log->Clicked() == true)
     612    getFrameworkManager()->StartLog();
     613  if (button_stop_log->Clicked() == true)
     614    getFrameworkManager()->StopLog();
    566615}
    567616
    568617void UavStateMachine::EnterFailSafeMode(void) {
    569     SetAltitudeMode(AltitudeMode_t::Manual);
    570     SetOrientationMode(OrientationMode_t::Manual);
    571     SetThrustMode(ThrustMode_t::Default);
    572     SetTorqueMode(TorqueMode_t::Default);
    573 
    574     GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
    575     joy->SetYawRef(currentQuaternion);
     618  SetAltitudeMode(AltitudeMode_t::Manual);
     619  SetOrientationMode(OrientationMode_t::Manual);
     620  SetThrustMode(ThrustMode_t::Default);
     621  SetTorqueMode(TorqueMode_t::Default);
     622
     623  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     624                                                        currentAngularSpeed);
     625  joy->SetYawRef(currentQuaternion);
     626  uYaw->Reset();
     627  uPitch->Reset();
     628  uRoll->Reset();
     629
     630  failSafeMode = true;
     631  SignalEvent(Event_t::EnteringFailSafeMode);
     632}
     633
     634bool UavStateMachine::ExitFailSafeMode(void) {
     635  // only exit fail safe mode if in Stabilized altitude state
     636  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
     637  // le ruban perturbe l'us
     638  /*
     639    if (altitudeState!=AltitudeState_t::Stabilized) {
     640        return false;
     641    } else*/ {
     642    failSafeMode = false;
     643    return true;
     644  }
     645}
     646
     647bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
     648  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
     649    if (!ExitFailSafeMode())
     650      return false;
     651  }
     652  // When transitionning from Custom to Default torque mode, we should reset the
     653  // default control laws
     654  if ((torqueMode == TorqueMode_t::Custom) &&
     655      (newTorqueMode == TorqueMode_t::Default)) {
    576656    uYaw->Reset();
    577657    uPitch->Reset();
    578658    uRoll->Reset();
    579 
    580     failSafeMode=true;
    581     SignalEvent(Event_t::EnteringFailSafeMode);
    582 }
    583 
    584 bool UavStateMachine::ExitFailSafeMode(void) {
    585     // only exit fail safe mode if in Stabilized altitude state
    586     //gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
    587     //le ruban perturbe l'us
    588     /*
    589     if (altitudeState!=AltitudeState_t::Stabilized) {
    590         return false;
    591     } else*/ {
    592         failSafeMode=false;
    593         return true;
    594     }
    595 }
    596 
    597 bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
    598     if ((newTorqueMode==TorqueMode_t::Custom) && (failSafeMode)) {
    599         if (!ExitFailSafeMode()) return false;
    600     }
    601     //When transitionning from Custom to Default torque mode, we should reset the default control laws
    602     if ((torqueMode==TorqueMode_t::Custom) && (newTorqueMode==TorqueMode_t::Default)) {
    603         uYaw->Reset();
    604         uPitch->Reset();
    605         uRoll->Reset();
    606     }
    607     torqueMode=newTorqueMode;
    608     return true;
     659  }
     660  torqueMode = newTorqueMode;
     661  return true;
    609662}
    610663
    611664bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
    612     if ((newAltitudeMode==AltitudeMode_t::Custom) && (failSafeMode)) {
    613         if (!ExitFailSafeMode()) return false;
    614     }
    615     altitudeMode=newAltitudeMode;
    616     GotoAltitude(desiredTakeoffAltitude->Value());
    617 
    618     return true;
     665  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
     666    if (!ExitFailSafeMode())
     667      return false;
     668  }
     669  altitudeMode = newAltitudeMode;
     670  GotoAltitude(desiredTakeoffAltitude->Value());
     671
     672  return true;
    619673}
    620674
    621675bool UavStateMachine::GotoAltitude(float desiredAltitude) {
    622     if (altitudeMode!=AltitudeMode_t::Manual) {
    623         return false;
    624     }
    625     altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),desiredAltitude);
    626     return true;
    627 }
    628 
    629 bool UavStateMachine::SetOrientationMode(OrientationMode_t const &newOrientationMode) {
    630     if ((newOrientationMode==OrientationMode_t::Custom) && (failSafeMode)) {
    631         if (!ExitFailSafeMode()) return false;
    632     }
    633     //When transitionning from Custom to Manual mode we must reset to yaw reference to the current absolute yaw angle,
    634     // overwise the Uav will abruptly change orientation
    635     if ((orientationMode==OrientationMode_t::Custom) && (newOrientationMode==OrientationMode_t::Manual)) {
    636         joy->SetYawRef(currentQuaternion);
    637     }
    638     orientationMode=newOrientationMode;
    639     return true;
     676  if (altitudeMode != AltitudeMode_t::Manual) {
     677    return false;
     678  }
     679  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
     680                                desiredAltitude);
     681  return true;
     682}
     683
     684bool UavStateMachine::SetOrientationMode(
     685    OrientationMode_t const &newOrientationMode) {
     686  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
     687    if (!ExitFailSafeMode())
     688      return false;
     689  }
     690  // When transitionning from Custom to Manual mode we must reset to yaw
     691  // reference to the current absolute yaw angle,
     692  // overwise the Uav will abruptly change orientation
     693  if ((orientationMode == OrientationMode_t::Custom) &&
     694      (newOrientationMode == OrientationMode_t::Manual)) {
     695    joy->SetYawRef(currentQuaternion);
     696  }
     697  orientationMode = newOrientationMode;
     698  return true;
    640699}
    641700
    642701bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
    643     if ((newThrustMode==ThrustMode_t::Custom) && (failSafeMode)) {
    644         if (!ExitFailSafeMode()) return false;
    645     }
    646     thrustMode=newThrustMode;
    647     return true;
    648 }
     702  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
     703    if (!ExitFailSafeMode())
     704      return false;
     705  }
     706  thrustMode = newThrustMode;
     707  return true;
     708}

  • trunk/lib/FlairMeta/src/UavStateMachine.h

    r10 r15  
    2626
    2727namespace flair {
    28     namespace core {
    29         class FrameworkManager;
    30         class AhrsData;
    31         class io_data;
    32     }
    33     namespace gui {
    34         class PushButton;
    35         class GridLayout;
    36         class Tab;
    37         class DoubleSpinBox;
    38     }
    39     namespace filter {
    40         class ControlLaw;
    41         class NestedSat;
    42         class Pid;
    43         class PidThrust;
    44         class TrajectoryGenerator1D;
    45     }
    46     namespace sensor {
    47         class TargetController;
    48     }
    49     namespace meta {
    50         class MetaDualShock3;
    51         class Uav;
    52     }
    53 }
    54 
    55 namespace flair { namespace meta {
    56 
    57  /*! \class UavStateMachine
     28namespace core {
     29class FrameworkManager;
     30class AhrsData;
     31class io_data;
     32}
     33namespace gui {
     34class PushButton;
     35class GridLayout;
     36class Tab;
     37class DoubleSpinBox;
     38}
     39namespace filter {
     40class ControlLaw;
     41class NestedSat;
     42class Pid;
     43class PidThrust;
     44class TrajectoryGenerator1D;
     45}
     46namespace sensor {
     47class TargetController;
     48}
     49namespace meta {
     50class MetaDualShock3;
     51class Uav;
     52}
     53}
     54
     55namespace flair {
     56namespace meta {
     57
     58/*! \class UavStateMachine
    5859*
    5960* \brief State machine for UAV
    6061*
    61 * thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
     62* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
     63*SetPeriodMS
    6264*/
    6365
    64 class UavStateMachine : public core::Thread
    65 {
    66     protected:
    67         enum class AltitudeMode_t {
    68             Manual,
    69             Custom
    70         };
    71         const AltitudeMode_t &GetAltitudeMode(void) const {
    72             return altitudeMode;
    73         }
    74         bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
    75 
    76         //uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
    77         //available in mode AltitudeMode_t::Manual
    78         //return true if goto is possible
    79         bool GotoAltitude(float desiredAltitude);
    80 
    81         enum class OrientationMode_t {
    82             Manual,
    83             Custom
    84         };
    85         const OrientationMode_t &GetOrientationMode(void) const {
    86             return orientationMode;
    87         }
    88         bool SetOrientationMode(const OrientationMode_t &orientationMode);
    89 
    90         enum class ThrustMode_t {
    91             Default,
    92             Custom
    93         };
    94         const ThrustMode_t &GetThrustMode() const {
    95             return thrustMode;
    96         }
    97         bool SetThrustMode(const ThrustMode_t &thrustMode);
    98 
    99         enum class TorqueMode_t {
    100             Default,
    101             Custom
    102         };
    103         const TorqueMode_t &GetTorqueMode(void) const {
    104             return torqueMode;
    105         }
    106         bool SetTorqueMode(const TorqueMode_t &torqueMode);
    107 
    108         enum class Event_t {
    109             EnteringFailSafeMode,
    110             EnteringControlLoop,
    111             StartLanding,
    112             FinishLanding,
    113             Stopped,
    114             TakingOff,
    115             EmergencyStop,
    116             Stabilized, //as soon as uav is 3cm far from the ground
    117             ZTrajectoryFinished,
    118         };
    119 
    120         UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
    121         ~UavStateMachine();
    122 
    123         const core::Quaternion &GetCurrentQuaternion(void) const;
    124 
    125         const core::Vector3D &GetCurrentAngularSpeed(void) const;
    126 
    127         const meta::Uav *GetUav(void) const;
    128 
    129         void Land(void);
    130         void TakeOff(void);
    131         void EmergencyStop(void);
    132         //! Used to signal an event
    133         /*!
    134             \param event the event which occured
    135         */
    136         virtual void SignalEvent(Event_t event);
    137 
    138         virtual const core::AhrsData *GetOrientation(void) const;
    139         const core::AhrsData *GetDefaultOrientation(void) const;
    140 
    141         virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
    142         void EnterFailSafeMode(void);
    143         bool ExitFailSafeMode(void);
    144         void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
    145 
    146         gui::GridLayout *GetButtonsLayout(void) const;
    147         virtual void ExtraSecurityCheck(void){};
    148         virtual void ExtraCheckJoystick(void){};
    149         virtual void ExtraCheckPushButton(void){};
    150 
    151         void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    152         virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    153         //float GetDefaultThrustOffset(void);
    154         const core::AhrsData *GetDefaultReferenceOrientation(void) const;
    155         virtual const core::AhrsData *GetReferenceOrientation(void);
    156 
    157         /*!
    158         * \brief Compute Custom Torques
    159         *
    160         * Reimplement this method to use TorqueMode_t::Custom. \n
    161         * This method is called internally by UavStateMachine, do not call it by yourself. \n
    162         * See GetTorques if you need torques values.
    163         *
    164         * \param torques custom torques
    165         */
    166         virtual void ComputeCustomTorques(core::Euler &torques);
    167 
    168         /*!
    169         * \brief Compute Default Torques
    170         *
    171         * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
    172         * Torques are only computed once by loop, other calls to this method will use previously computed torques.
    173         *
    174         * \param torques default torques
    175         */
    176         void ComputeDefaultTorques(core::Euler &torques);
    177 
    178         /*!
    179         * \brief Get Torques
    180         *
    181         * \return torques current torques
    182         */
    183         //const core::Euler &GetTorques() const;
    184 
    185         /*!
    186         * \brief Compute Custom Thrust
    187         *
    188         * Reimplement this method to use ThrustMode_t::Custom. \n
    189         * This method is called internally by UavStateMachine, do not call it by yourself. \n
    190         * See GetThrust if you need thrust value.
    191         *
    192         * \return custom Thrust
    193         */
    194         virtual float ComputeCustomThrust(void);
    195 
    196         /*!
    197         * \brief Compute Default Thrust
    198         *
    199         * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
    200         * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
    201         *
    202         * \return default thrust
    203         */
    204         float ComputeDefaultThrust(void);
    205 
    206         /*!
    207         * \brief Get Thrust
    208         *
    209         * \return current thrust
    210         */
    211         //float GetThrust() const;
    212 
    213         /*!
    214         * \brief Add an IODevice to the control law logs
    215         *
    216         * The IODevice will be automatically logged among the Uz logs,
    217         * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
    218         * and FrameworkManager::AddDeviceToLog). \n
    219         *
    220         * \param device IODevice to log
    221         */
    222         void AddDeviceToControlLawLog(const core::IODevice* device);
    223 
    224         /*!
    225         * \brief Add an io_data to the control law logs
    226         *
    227         * The io_data will be automatically logged among the Uz logs,
    228         * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
    229         * and FrameworkManager::AddDeviceToLog). \n
    230         *
    231         * \param data io_data to log
    232         */
    233         void AddDataToControlLawLog(const core::io_data* data);
    234 
    235         const sensor::TargetController *GetJoystick(void) const;
    236 
    237         gui::Tab *setupLawTab,*graphLawTab;
    238 
    239     private:
    240         /*!
    241         \enum AltitudeState_t
    242         \brief States of the altitude state machine
    243         */
    244         enum class AltitudeState_t {
    245             Stopped,        /*!< the uav motors are stopped */
    246             TakingOff,      /*!< take off initiated. Motors accelerate progressively until the UAV lift up */
    247             Stabilized,     /*!< the uav is actively maintaining its altitude */
    248             StartLanding,   /*!< landing initiated. Altitude is required to reach the landing altitude (0 by default) */
    249             FinishLanding   /*!< motors are gradually stopped */
    250         };
    251         AltitudeState_t altitudeState;
    252         void ProcessAltitudeFiniteStateMachine();
    253         void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    254 
    255 
    256         float groundAltitude; // effective altitude when the uav leaves the ground
    257         float currentAltitude,currentVerticalSpeed;
    258 
    259         bool failSafeMode;
    260         void SecurityCheck(void);
    261         void MandatorySecurityCheck(void);
    262         void CheckJoystick();
    263         void GenericCheckJoystick();
    264         void CheckPushButton(void);
    265         void GenericCheckPushButton(void);
    266         void Run(void);
    267         void StopMotors(void);
    268 
    269         meta::Uav *uav;
    270 
    271         core::Quaternion currentQuaternion;
    272         core::Vector3D currentAngularSpeed;
    273 
    274         const core::AhrsData *ComputeReferenceOrientation(void);
    275 
    276         void ComputeOrientation(void);
    277         void ComputeAltitude(void);
    278 
    279         void ComputeTorques(void);
    280         core::Euler currentTorques,savedDefaultTorques;
    281         bool needToComputeDefaultTorques;
    282 
    283         void ComputeThrust(void);
    284         float currentThrust,savedDefaultThrust;
    285         bool needToComputeDefaultThrust;
    286 
    287         gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
    288         gui::GridLayout *buttonslayout;
    289         gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
    290         AltitudeMode_t altitudeMode;
    291         OrientationMode_t orientationMode;
    292         ThrustMode_t thrustMode;
    293         TorqueMode_t torqueMode;
    294         bool flagBatteryLow;
    295         bool flagConnectionLost;
    296         bool flagZTrajectoryFinished;
    297         filter::NestedSat *uRoll,*uPitch;
    298         filter::Pid *uYaw;
    299         filter::PidThrust *uZ;
    300 
    301         MetaDualShock3 *joy;
    302         filter::TrajectoryGenerator1D *altitudeTrajectory;
     66class UavStateMachine : public core::Thread {
     67protected:
     68  enum class AltitudeMode_t { Manual, Custom };
     69  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
     70  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
     71
     72  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
     73  // available in mode AltitudeMode_t::Manual
     74  // return true if goto is possible
     75  bool GotoAltitude(float desiredAltitude);
     76
     77  enum class OrientationMode_t { Manual, Custom };
     78  const OrientationMode_t &GetOrientationMode(void) const {
     79    return orientationMode;
     80  }
     81  bool SetOrientationMode(const OrientationMode_t &orientationMode);
     82
     83  enum class ThrustMode_t { Default, Custom };
     84  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
     85  bool SetThrustMode(const ThrustMode_t &thrustMode);
     86
     87  enum class TorqueMode_t { Default, Custom };
     88  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
     89  bool SetTorqueMode(const TorqueMode_t &torqueMode);
     90
     91  enum class Event_t {
     92    EnteringFailSafeMode,
     93    EnteringControlLoop,
     94    StartLanding,
     95    FinishLanding,
     96    Stopped,
     97    TakingOff,
     98    EmergencyStop,
     99    Stabilized, // as soon as uav is 3cm far from the ground
     100    ZTrajectoryFinished,
     101  };
     102
     103  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
     104  ~UavStateMachine();
     105
     106  const core::Quaternion &GetCurrentQuaternion(void) const;
     107
     108  const core::Vector3D &GetCurrentAngularSpeed(void) const;
     109
     110  const meta::Uav *GetUav(void) const;
     111
     112  void Land(void);
     113  void TakeOff(void);
     114  void EmergencyStop(void);
     115  //! Used to signal an event
     116  /*!
     117      \param event the event which occured
     118  */
     119  virtual void SignalEvent(Event_t event);
     120
     121  virtual const core::AhrsData *GetOrientation(void) const;
     122  const core::AhrsData *GetDefaultOrientation(void) const;
     123
     124  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
     125  void EnterFailSafeMode(void);
     126  bool ExitFailSafeMode(void);
     127  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
     128
     129  gui::GridLayout *GetButtonsLayout(void) const;
     130  virtual void ExtraSecurityCheck(void){};
     131  virtual void ExtraCheckJoystick(void){};
     132  virtual void ExtraCheckPushButton(void){};
     133
     134  void GetDefaultReferenceAltitude(float &refAltitude,
     135                                   float &refVerticalVelocity);
     136  virtual void GetReferenceAltitude(float &refAltitude,
     137                                    float &refVerticalVelocity);
     138  // float GetDefaultThrustOffset(void);
     139  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
     140  virtual const core::AhrsData *GetReferenceOrientation(void);
     141
     142  /*!
     143  * \brief Compute Custom Torques
     144  *
     145  * Reimplement this method to use TorqueMode_t::Custom. \n
     146  * This method is called internally by UavStateMachine, do not call it by
     147  *yourself. \n
     148  * See GetTorques if you need torques values.
     149  *
     150  * \param torques custom torques
     151  */
     152  virtual void ComputeCustomTorques(core::Euler &torques);
     153
     154  /*!
     155  * \brief Compute Default Torques
     156  *
     157  * This method is called internally by UavStateMachine when using
     158  *TorqueMode_t::Default. \n
     159  * Torques are only computed once by loop, other calls to this method will use
     160  *previously computed torques.
     161  *
     162  * \param torques default torques
     163  */
     164  void ComputeDefaultTorques(core::Euler &torques);
     165
     166  /*!
     167  * \brief Get Torques
     168  *
     169  * \return torques current torques
     170  */
     171  // const core::Euler &GetTorques() const;
     172
     173  /*!
     174  * \brief Compute Custom Thrust
     175  *
     176  * Reimplement this method to use ThrustMode_t::Custom. \n
     177  * This method is called internally by UavStateMachine, do not call it by
     178  *yourself. \n
     179  * See GetThrust if you need thrust value.
     180  *
     181  * \return custom Thrust
     182  */
     183  virtual float ComputeCustomThrust(void);
     184
     185  /*!
     186  * \brief Compute Default Thrust
     187  *
     188  * This method is called internally by UavStateMachine when using
     189  *ThrustMode_t::Default. \n
     190  * Thrust is only computed once by loop, other calls to this method will use
     191  *previously computed thrust.
     192  *
     193  * \return default thrust
     194  */
     195  float ComputeDefaultThrust(void);
     196
     197  /*!
     198  * \brief Get Thrust
     199  *
     200  * \return current thrust
     201  */
     202  // float GetThrust() const;
     203
     204  /*!
     205  * \brief Add an IODevice to the control law logs
     206  *
     207  * The IODevice will be automatically logged among the Uz logs,
     208  * if logging is enabled (see IODevice::SetDataToLog,
     209  *FrameworkManager::StartLog
     210  * and FrameworkManager::AddDeviceToLog). \n
     211  *
     212  * \param device IODevice to log
     213  */
     214  void AddDeviceToControlLawLog(const core::IODevice *device);
     215
     216  /*!
     217  * \brief Add an io_data to the control law logs
     218  *
     219  * The io_data will be automatically logged among the Uz logs,
     220  * if logging is enabled (see IODevice::SetDataToLog,
     221  *FrameworkManager::StartLog
     222  * and FrameworkManager::AddDeviceToLog). \n
     223  *
     224  * \param data io_data to log
     225  */
     226  void AddDataToControlLawLog(const core::io_data *data);
     227
     228  const sensor::TargetController *GetJoystick(void) const;
     229
     230  gui::Tab *setupLawTab, *graphLawTab;
     231
     232private:
     233  /*!
     234  \enum AltitudeState_t
     235  \brief States of the altitude state machine
     236  */
     237  enum class AltitudeState_t {
     238    Stopped,      /*!< the uav motors are stopped */
     239    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
     240                     the UAV lift up */
     241    Stabilized,   /*!< the uav is actively maintaining its altitude */
     242    StartLanding, /*!< landing initiated. Altitude is required to reach the
     243                     landing altitude (0 by default) */
     244    FinishLanding /*!< motors are gradually stopped */
     245  };
     246  AltitudeState_t altitudeState;
     247  void ProcessAltitudeFiniteStateMachine();
     248  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
     249
     250  float groundAltitude; // effective altitude when the uav leaves the ground
     251  float currentAltitude, currentVerticalSpeed;
     252
     253  bool failSafeMode;
     254  void SecurityCheck(void);
     255  void MandatorySecurityCheck(void);
     256  void CheckJoystick();
     257  void GenericCheckJoystick();
     258  void CheckPushButton(void);
     259  void GenericCheckPushButton(void);
     260  void Run(void);
     261  void StopMotors(void);
     262
     263  meta::Uav *uav;
     264
     265  core::Quaternion currentQuaternion;
     266  core::Vector3D currentAngularSpeed;
     267
     268  const core::AhrsData *ComputeReferenceOrientation(void);
     269
     270  void ComputeOrientation(void);
     271  void ComputeAltitude(void);
     272
     273  void ComputeTorques(void);
     274  core::Euler currentTorques, savedDefaultTorques;
     275  bool needToComputeDefaultTorques;
     276
     277  void ComputeThrust(void);
     278  float currentThrust, savedDefaultThrust;
     279  bool needToComputeDefaultThrust;
     280
     281  gui::PushButton *button_kill, *button_take_off, *button_land,
     282      *button_start_log, *button_stop_log;
     283  gui::GridLayout *buttonslayout;
     284  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
     285  AltitudeMode_t altitudeMode;
     286  OrientationMode_t orientationMode;
     287  ThrustMode_t thrustMode;
     288  TorqueMode_t torqueMode;
     289  bool flagBatteryLow;
     290  bool flagConnectionLost;
     291  bool flagZTrajectoryFinished;
     292  filter::NestedSat *uRoll, *uPitch;
     293  filter::Pid *uYaw;
     294  filter::PidThrust *uZ;
     295
     296  MetaDualShock3 *joy;
     297  filter::TrajectoryGenerator1D *altitudeTrajectory;
    303298};
    304 
    305299};
    306300};

  • trunk/lib/FlairMeta/src/XAir.cpp

    r10 r15  
    3535using namespace flair::actuator;
    3636
    37 namespace flair { namespace meta {
     37namespace flair {
     38namespace meta {
    3839
    39 XAir::XAir(FrameworkManager* parent,string uav_name,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
    40     RTDM_I2cPort* i2cport=new RTDM_I2cPort(parent,"rtdm_i2c","rti2c3");
    41     RTDM_SerialPort* imu_port=new RTDM_SerialPort(parent,"imu_port","rtser1");
     40XAir::XAir(FrameworkManager *parent, string uav_name,
     41           filter::UavMultiplex *multiplex)
     42    : Uav(parent, uav_name, multiplex) {
     43  RTDM_I2cPort *i2cport = new RTDM_I2cPort(parent, "rtdm_i2c", "rti2c3");
     44  RTDM_SerialPort *imu_port = new RTDM_SerialPort(parent, "imu_port", "rtser1");
    4245
    43     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
     46  if (multiplex == NULL)
     47    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
    4448
    45     SetBldc(new AfroBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),i2cport));
    46     SetUsRangeFinder(new Srf08(parent,"SRF08",i2cport,0x70,60));
    47     SetAhrs(new Gx3_25_ahrs(parent,"imu",imu_port,Gx3_25_imu::EulerAnglesAndAngularRates,70));
    48     Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
    49     SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
    50     GetBatteryMonitor()->SetBatteryValue(12);
     49  SetBldc(new AfroBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     50                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
     51  SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
     52  SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
     53                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
     54  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
     55  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
     56  GetBatteryMonitor()->SetBatteryValue(12);
    5157
    52 /*
    53         if(VRPNType==Auto || VRPNType==AutoSerialPort)
    54         {
    55             RTDM_SerialPort* vrpn_port=new RTDM_SerialPort(parent,"vrpn_port","rtser3");
     58  /*
     59          if(VRPNType==Auto || VRPNType==AutoSerialPort)
     60          {
     61              RTDM_SerialPort* vrpn_port=new
     62     RTDM_SerialPort(parent,"vrpn_port","rtser3");
    5663
    57             vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
    58             uav_vrpn=new MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
    59         }
    60 */
    61     SetVerticalCamera(new Ps3Eye(parent,"camv",0,50));
     64              vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
     65              uav_vrpn=new
     66     MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
     67          }
     68  */
     69  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
    6270}
    6371
    64 XAir::~XAir() {
    65 
    66 }
     72XAir::~XAir() {}
    6773
    6874void XAir::StartSensors(void) {
    69     ((Gx3_25_ahrs*)GetAhrs())->Start();
    70     ((Srf08*)GetUsRangeFinder())->Start();
    71     ((Ps3Eye *)GetVerticalCamera())->Start();
    72     Uav::StartSensors();
     75  ((Gx3_25_ahrs *)GetAhrs())->Start();
     76  ((Srf08 *)GetUsRangeFinder())->Start();
     77  ((Ps3Eye *)GetVerticalCamera())->Start();
     78  Uav::StartSensors();
    7379}
    7480

  • trunk/lib/FlairMeta/src/XAir.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class XAir
    23     *
    24     * \brief Class defining an ardrone2 uav
    25     */
    26     class XAir : public Uav {
    27         public:
    28             XAir(core::FrameworkManager* parent,std::string uav_name,filter::UavMultiplex *multiplex=NULL);
    29             ~XAir();
    30             void StartSensors(void);
     18namespace flair {
     19namespace meta {
     20/*! \class XAir
     21*
     22* \brief Class defining an ardrone2 uav
     23*/
     24class XAir : public Uav {
     25public:
     26  XAir(core::FrameworkManager *parent, std::string uav_name,
     27       filter::UavMultiplex *multiplex = NULL);
     28  ~XAir();
     29  void StartSensors(void);
    3130
    32         private:
    33 
    34     };
     31private:
     32};
    3533} // end namespace meta
    3634} // end namespace flair

  • trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace meta
    21     {
    22         class MetaDualShock3;
    23     }
    24     namespace filter
    25     {
    26         class JoyReference;
    27     }
     18namespace flair {
     19namespace meta {
     20class MetaDualShock3;
     21}
     22namespace filter {
     23class JoyReference;
     24}
    2825}
    2926
    … …  
    3229* \brief Classe intégrant la manette DualShock3 et les consignes joystick
    3330*/
    34 class MetaDualShock3_impl: public flair::core::IODevice
    35 {
    36     public:
    37         MetaDualShock3_impl(flair::meta::MetaDualShock3* self,std::string name);
    38         ~MetaDualShock3_impl();
    39         flair::filter::JoyReference *joy_ref;
     31class MetaDualShock3_impl : public flair::core::IODevice {
     32public:
     33  MetaDualShock3_impl(flair::meta::MetaDualShock3 *self, std::string name);
     34  ~MetaDualShock3_impl();
     35  flair::filter::JoyReference *joy_ref;
    4036
    41     private:
    42         void UpdateFrom(const flair::core::io_data *data);
    43         flair::meta::MetaDualShock3* self;
    44         bool joy_init;
    45         bool wasRollTrimUpButtonPressed,wasRollTrimDownButtonPressed,wasPitchTrimUpButtonPressed,wasPitchTrimDownButtonPressed;
     37private:
     38  void UpdateFrom(const flair::core::io_data *data);
     39  flair::meta::MetaDualShock3 *self;
     40  bool joy_init;
     41  bool wasRollTrimUpButtonPressed, wasRollTrimDownButtonPressed,
     42      wasPitchTrimUpButtonPressed, wasPitchTrimDownButtonPressed;
    4643};
    4744

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  • trunk/lib/FlairSimulator/src/AnimPoursuite.cpp

    r10 r15  
    2828using namespace scene;
    2929
    30 namespace flair
    31 {
    32 namespace simulator
    33 {
     30namespace flair {
     31namespace simulator {
    3432
    35 AnimPoursuite::AnimPoursuite(const ISceneNode* parent,float rotateSpeed , float zoomSpeed )
    36 {
    37     this->parent=parent;
    38     this->zoomSpeed=zoomSpeed;
    39     this->rotateSpeed=rotateSpeed;
    40     currentZoom=100;
    41     RotY=20;
    42     RotZ=0;
    43     Rotating=false;
    44     LMouseKey= false;
     33AnimPoursuite::AnimPoursuite(const ISceneNode *parent, float rotateSpeed,
     34                             float zoomSpeed) {
     35  this->parent = parent;
     36  this->zoomSpeed = zoomSpeed;
     37  this->rotateSpeed = rotateSpeed;
     38  currentZoom = 100;
     39  RotY = 20;
     40  RotZ = 0;
     41  Rotating = false;
     42  LMouseKey = false;
    4543}
    4644
    47 AnimPoursuite::~AnimPoursuite()
    48 {
     45AnimPoursuite::~AnimPoursuite() {}
    4946
     47void AnimPoursuite::setPositionOffset(vector3df newpos) { pos_offset = newpos; }
     48
     49void AnimPoursuite::setTargetOffset(vector3df newpos) {
     50  target_offset = newpos;
    5051}
    5152
    52 void AnimPoursuite::setPositionOffset(vector3df newpos)
    53 {
    54     pos_offset=newpos;
     53float AnimPoursuite::sat(float value) {
     54  if (value > 89)
     55    value = 89;
     56  if (value < -89)
     57    value = -89;
     58  return value;
    5559}
    5660
    57 void AnimPoursuite::setTargetOffset(vector3df newpos)
    58 {
    59     target_offset=newpos;
     61void AnimPoursuite::animateNode(ISceneNode *node, u32 timeMs) {
     62  ICameraSceneNode *camera = static_cast<ICameraSceneNode *>(node);
     63  vector3df pos;
     64
     65  float nRotY = RotY;
     66  float nRotZ = RotZ;
     67
     68  if (LMouseKey == true) {
     69    if (!Rotating) {
     70      RotateStart = MousePos;
     71      Rotating = true;
     72      nRotY = RotY;
     73      nRotZ = RotZ;
     74    } else {
     75      nRotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
     76      nRotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
     77      nRotY = sat(nRotY);
     78    }
     79  } else if (Rotating) {
     80    RotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
     81    RotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
     82    RotY = sat(RotY);
     83    nRotY = RotY;
     84    nRotZ = RotZ;
     85    Rotating = false;
     86  }
     87
     88  pos.X = -currentZoom;
     89  pos.Y = 0;
     90  pos.Z = 0;
     91
     92  pos.rotateXZBy(-nRotY);
     93  pos.rotateXYBy(getSimulator()->Yaw() + nRotZ + parent->getRotation().Z);
     94
     95  camera->setPosition(parent->getPosition() + pos + pos_offset);
     96  camera->setTarget(parent->getPosition() + target_offset);
    6097}
    6198
    62 float AnimPoursuite::sat(float value)
    63 {
    64     if(value>89) value=89;
    65     if(value<-89) value=-89;
    66     return value;
     99ISceneNodeAnimator *AnimPoursuite::createClone(ISceneNode *node,
     100                                               ISceneManager *newManager) {
     101  return NULL;
    67102}
    68103
    69 void AnimPoursuite::animateNode(ISceneNode* node, u32 timeMs)
    70 {
    71     ICameraSceneNode* camera = static_cast<ICameraSceneNode*>(node);
    72     vector3df pos;
     104bool AnimPoursuite::MouseMoved(const SEvent &event,
     105                               irr::core::position2df MousePos) {
     106  if (event.EventType != EET_MOUSE_INPUT_EVENT)
     107    return false;
    73108
    74     float nRotY = RotY;
    75     float nRotZ = RotZ;
     109  switch (event.MouseInput.Event) {
    76110
    77     if (LMouseKey==true)
    78     {
    79         if (!Rotating)
    80         {
    81             RotateStart = MousePos;
    82             Rotating = true;
    83             nRotY = RotY;
    84             nRotZ = RotZ;
    85         }
    86         else
    87         {
    88             nRotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
    89             nRotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
    90             nRotY=sat(nRotY);
    91         }
    92     }
    93     else if (Rotating)
    94     {
    95         RotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
    96         RotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
    97         RotY=sat(RotY);
    98         nRotY = RotY;
    99         nRotZ = RotZ;
    100         Rotating = false;
    101     }
     111  case EMIE_MOUSE_WHEEL:
     112    currentZoom -= event.MouseInput.Wheel * zoomSpeed;
     113    if (currentZoom <= 0)
     114      currentZoom = zoomSpeed;
     115    break;
     116  case EMIE_LMOUSE_PRESSED_DOWN:
     117    LMouseKey = true;
     118    break;
     119  case EMIE_LMOUSE_LEFT_UP:
     120    LMouseKey = false;
     121    break;
     122  case EMIE_MOUSE_MOVED:
     123    this->MousePos = MousePos;
     124    break;
     125  default:
     126    return false;
     127    break;
     128  }
    102129
    103     pos.X = -currentZoom;
    104     pos.Y=0;
    105     pos.Z=0;
    106 
    107     pos.rotateXZBy(-nRotY);
    108     pos.rotateXYBy(getSimulator()->Yaw()+nRotZ+parent->getRotation().Z);
    109 
    110     camera->setPosition(parent->getPosition()+pos+pos_offset);
    111     camera->setTarget(parent->getPosition()+target_offset);
    112 }
    113 
    114 ISceneNodeAnimator* AnimPoursuite::createClone(ISceneNode* node,
    115             ISceneManager* newManager)
    116 {
    117     return NULL;
    118 }
    119 
    120 bool AnimPoursuite::MouseMoved(const SEvent& event,irr::core::position2df MousePos)
    121 {
    122     if (event.EventType != EET_MOUSE_INPUT_EVENT)
    123         return false;
    124 
    125     switch(event.MouseInput.Event)
    126     {
    127 
    128     case EMIE_MOUSE_WHEEL:
    129         currentZoom -= event.MouseInput.Wheel * zoomSpeed;
    130         if(currentZoom<=0) currentZoom=zoomSpeed;
    131         break;
    132     case EMIE_LMOUSE_PRESSED_DOWN:
    133         LMouseKey = true;
    134         break;
    135     case EMIE_LMOUSE_LEFT_UP:
    136         LMouseKey= false;
    137         break;
    138     case EMIE_MOUSE_MOVED:
    139         this->MousePos = MousePos;
    140         break;
    141     default:
    142         return false;
    143         break;
    144     }
    145 
    146     return true;
     130  return true;
    147131}
    148132
    … …  
    150134} // end namespace flair
    151135
    152 #endif //GL
     136#endif // GL

  • trunk/lib/FlairSimulator/src/Blade.cpp

    r10 r15  
    3333using namespace flair::core;
    3434
    35 namespace flair
    36 {
    37 namespace simulator
    38 {
     35namespace flair {
     36namespace simulator {
    3937
    40 Blade::Blade(Model* parent,const vector3df& position,bool inverted,s32 id)
    41     : ISceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,position)
    42 {
    43     ISceneManager* mgr=getGui()->getSceneManager();
     38Blade::Blade(Model *parent, const vector3df &position, bool inverted, s32 id)
     39    : ISceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,
     40                 position) {
     41  ISceneManager *mgr = getGui()->getSceneManager();
    4442
    45     const IGeometryCreator *geo;
    46     geo=mgr->getGeometryCreator();
    47     pale=geo->createCubeMesh(vector3df(63.5,0.5,5));
     43  const IGeometryCreator *geo;
     44  geo = mgr->getGeometryCreator();
     45  pale = geo->createCubeMesh(vector3df(63.5, 0.5, 5));
    4846
    49     float angle;
    50     if(inverted==false)
    51     {
    52         angle=20;
    53     }
    54     else
    55     {
    56         angle=-20;
    57     }
     47  float angle;
     48  if (inverted == false) {
     49    angle = 20;
     50  } else {
     51    angle = -20;
     52  }
    5853
    59     ITexture* texture=getGui()->getTexture("carbone.jpg");
    60     pale_1=new MeshSceneNode(parent, pale, vector3df(-30,0,0),vector3df(-angle+90,0,0),texture);
    61     pale_1->setParent(this);
    62     pale_2=new MeshSceneNode(parent, pale, vector3df(30,0,0),vector3df(angle+90,0,0),texture);
    63     pale_2->setParent(this);
     54  ITexture *texture = getGui()->getTexture("carbone.jpg");
     55  pale_1 = new MeshSceneNode(parent, pale, vector3df(-30, 0, 0),
     56                             vector3df(-angle + 90, 0, 0), texture);
     57  pale_1->setParent(this);
     58  pale_2 = new MeshSceneNode(parent, pale, vector3df(30, 0, 0),
     59                             vector3df(angle + 90, 0, 0), texture);
     60  pale_2->setParent(this);
    6461
    65     anim = mgr->createRotationAnimator(vector3df(0.f, 0.f, 0.f));
    66     addAnimator(anim);
     62  anim = mgr->createRotationAnimator(vector3df(0.f, 0.f, 0.f));
     63  addAnimator(anim);
    6764}
    6865
    69 void Blade::OnRegisterSceneNode()
    70 {
    71     if (IsVisible)
    72         SceneManager->registerNodeForRendering(this);
     66void Blade::OnRegisterSceneNode() {
     67  if (IsVisible)
     68    SceneManager->registerNodeForRendering(this);
    7369
    74     ISceneNode::OnRegisterSceneNode();
     70  ISceneNode::OnRegisterSceneNode();
    7571}
    7672
    77 void Blade::SetRotationSpeed(float value)
    78 {
    79     IAttributes* attribs =getSceneManager()->getFileSystem()->createEmptyAttributes();
     73void Blade::SetRotationSpeed(float value) {
     74  IAttributes *attribs =
     75      getSceneManager()->getFileSystem()->createEmptyAttributes();
    8076
    81     attribs->setAttribute("Type","rotation");
    82     attribs->setAttribute("Rotation",vector3df(0.f, 0.f,value));
    83     anim->deserializeAttributes(attribs);
     77  attribs->setAttribute("Type", "rotation");
     78  attribs->setAttribute("Rotation", vector3df(0.f, 0.f, value));
     79  anim->deserializeAttributes(attribs);
    8480
    85     attribs->drop();
     81  attribs->drop();
    8682}
    8783
    88 void Blade::render()
    89 {
    90     IVideoDriver* driver = SceneManager->getVideoDriver();
    91     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     84void Blade::render() {
     85  IVideoDriver *driver = SceneManager->getVideoDriver();
     86  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    9287}
    9388
    9489} // end namespace simulator
    9590} // end namespace flair
    96 #endif //GL
     91#endif // GL

  • trunk/lib/FlairSimulator/src/Blade.h

    r10 r15  
    2020#include <ISceneNode.h>
    2121
    22 namespace irr
    23 {
    24     namespace scene
    25     {
    26         class IMesh;
    27     }
     22namespace irr {
     23namespace scene {
     24class IMesh;
     25}
    2826}
    2927
    30 namespace flair
    31 {
    32 namespace simulator
    33 {
    34     class MeshSceneNode;
    35     class Model;
     28namespace flair {
     29namespace simulator {
     30class MeshSceneNode;
     31class Model;
    3632
    37     class Blade : public irr::scene::ISceneNode
    38     {
    39         public:
     33class Blade : public irr::scene::ISceneNode {
     34public:
     35  Blade(Model *parent,
     36        const irr::core::vector3df &position = irr::core::vector3df(0, 0, 0),
     37        bool inverted = false, irr::s32 id = -1);
     38  virtual void OnRegisterSceneNode(void);
     39  virtual void render(void);
     40  virtual const irr::core::aabbox3d<irr::f32> &getBoundingBox(void) const {
     41    return Box;
     42  }
     43  void SetRotationSpeed(float value);
    4044
    41             Blade(Model* parent,const irr::core::vector3df& position = irr::core::vector3df(0,0,0),bool inverted=false,irr::s32 id=-1);
    42             virtual void OnRegisterSceneNode(void);
    43             virtual void render(void);
    44             virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox(void) const
    45             {
    46                 return Box;
    47             }
    48             void SetRotationSpeed(float value);
    49 
    50 
    51         private:
    52             irr::scene::IMesh *pale;
    53             irr::core::aabbox3d<irr::f32> Box;
    54             MeshSceneNode *pale_1,*pale_2;
    55             irr::scene::ISceneNodeAnimator *anim;
    56     };
     45private:
     46  irr::scene::IMesh *pale;
     47  irr::core::aabbox3d<irr::f32> Box;
     48  MeshSceneNode *pale_1, *pale_2;
     49  irr::scene::ISceneNodeAnimator *anim;
     50};
    5751} // end namespace simulator
    5852} // end namespace flair

  • trunk/lib/FlairSimulator/src/Castle.cpp

    r10 r15  
    2424using namespace flair::core;
    2525
    26 namespace flair
    27 {
    28 namespace simulator
    29 {
     26namespace flair {
     27namespace simulator {
    3028
    31 Castle::Castle(const Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path): Gui(parent,"Castle",app_width,app_height,scene_width,scene_height, media_path)
    32 {
    33     //carte
    34     std::string file=media_path +"/map-20kdm2.pk3";
    35         getDevice()->getFileSystem()->addFileArchive(file.c_str());
    36         setMesh("20kdm2.bsp",vector3df(-1400,-65,-1349));
     29Castle::Castle(const Simulator *parent, int app_width, int app_height,
     30               int scene_width, int scene_height, std::string media_path)
     31    : Gui(parent, "Castle", app_width, app_height, scene_width, scene_height,
     32          media_path) {
     33  // carte
     34  std::string file = media_path + "/map-20kdm2.pk3";
     35  getDevice()->getFileSystem()->addFileArchive(file.c_str());
     36  setMesh("20kdm2.bsp", vector3df(-1400, -65, -1349));
    3737}
    3838
    39 Castle::~Castle()
    40 {
    41 
    42 }
     39Castle::~Castle() {}
    4340
    4441} // end namespace simulator
    4542} // end namespace flair
    46 #endif //GL
     43#endif // GL

  • trunk/lib/FlairSimulator/src/Castle.h

    r10 r15  
    1919
    2020#include <Gui.h>
    21 namespace flair
    22 {
    23 namespace simulator
    24 {
    25     class Castle:public Gui
    26     {
    27         public:
    28             Castle(const flair::simulator::Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path);
    29             ~Castle();
     21namespace flair {
     22namespace simulator {
     23class Castle : public Gui {
     24public:
     25  Castle(const flair::simulator::Simulator *parent, int app_width,
     26         int app_height, int scene_width, int scene_height,
     27         std::string media_path);
     28  ~Castle();
    3029
    31         private:
    32 
    33     };
     30private:
     31};
    3432} // end namespace simulator
    3533} // end namespace flair

  • trunk/lib/FlairSimulator/src/DiscreteTimeVariable.h

    r10 r15  
    2020#include <stdlib.h>
    2121
    22 namespace flair
    23 {
    24 namespace simulator
    25 {
    26     template <typename T,size_t size>
    27     class DiscreteTimeVariable
    28     {
    29         public:
    30             DiscreteTimeVariable(){};
    31             ~DiscreteTimeVariable(){};
    32             T& operator[](ssize_t idx)
    33             {
    34                 if(idx>0) idx=0;
    35                 if(idx<(ssize_t)(-size+1)) idx=-size+1;
    36                 return array[-idx];
    37             }
    38             const T&operator[](ssize_t idx) const
    39             {
    40                 return const_cast<T&>(*this)[idx];
    41             };
    42             void Update(void)
    43             {
    44                 for(int i=size-1;i>0;i--)
    45                 {
    46                     array[i]=array[i-1];
    47                 }
    48             }
     22namespace flair {
     23namespace simulator {
     24template <typename T, size_t size> class DiscreteTimeVariable {
     25public:
     26  DiscreteTimeVariable(){};
     27  ~DiscreteTimeVariable(){};
     28  T &operator[](ssize_t idx) {
     29    if (idx > 0)
     30      idx = 0;
     31    if (idx < (ssize_t)(-size + 1))
     32      idx = -size + 1;
     33    return array[-idx];
     34  }
     35  const T &operator[](ssize_t idx) const {
     36    return const_cast<T &>(*this)[idx];
     37  };
     38  void Update(void) {
     39    for (int i = size - 1; i > 0; i--) {
     40      array[i] = array[i - 1];
     41    }
     42  }
    4943
    50         private:
    51             T array[size];
    52 
    53     };
     44private:
     45  T array[size];
     46};
    5447} // end namespace simulator
    5548} // end namespace flair

  • trunk/lib/FlairSimulator/src/GenericObject.cpp

    r10 r15  
    3737using namespace flair::simulator;
    3838
    39 namespace flair
     39namespace flair {
     40namespace simulator {
     41
     42GenericObject::GenericObject(Simulator *parent, std::string name,
     43                             ISceneManager *sceneManager)
     44    : IMeshSceneNode(sceneManager->getRootSceneNode(), sceneManager,
     45                     -1) //, IODevice(parent,name)
    4046{
    41 namespace simulator
    42 {
     47  setMaterialFlag(EMF_LIGHTING, false);
     48  Material = getMaterial(0);
     49  // setMaterialTexture(0,sceneManager->getVideoDriver()->getTexture("/home/cesar/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
     50  Material.NormalizeNormals = true;
     51  Material.Wireframe = false;
     52  Material.Lighting = false;
    4353
    44 GenericObject::GenericObject(Simulator* parent,std::string name, ISceneManager* sceneManager) : IMeshSceneNode(sceneManager->getRootSceneNode(), sceneManager, -1)//, IODevice(parent,name)
    45 {
    46     setMaterialFlag(EMF_LIGHTING,false);
    47     Material=getMaterial(0);
    48     //setMaterialTexture(0,sceneManager->getVideoDriver()->getTexture("/home/cesar/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
    49     Material.NormalizeNormals = true;
    50     Material.Wireframe = false;
    51     Material.Lighting = false;
    52 
    53     parent->pimpl_->objects.push_back(this);
     54  parent->pimpl_->objects.push_back(this);
    5455}
    5556
    56 GenericObject::~GenericObject()
    57 {
     57GenericObject::~GenericObject() {}
    5858
     59void GenericObject::setScale(float value) {
     60  ISceneNode::setScale(vector3df(value, value, value));
    5961}
    6062
    61 void GenericObject::setScale(float value)
    62 {
    63     ISceneNode::setScale(vector3df(value,value,value));
     63void GenericObject::setScale(vector3df scale) { ISceneNode::setScale(scale); }
     64
     65ITriangleSelector *GenericObject::TriangleSelector(void) { return selector; }
     66
     67void GenericObject::OnRegisterSceneNode(void) {
     68  if (IsVisible)
     69    SceneManager->registerNodeForRendering(this);
     70
     71  ISceneNode::OnRegisterSceneNode();
    6472}
    6573
    66 void GenericObject::setScale(vector3df scale)
    67 {
    68     ISceneNode::setScale(scale);
     74void GenericObject::render(void) {
     75  IVideoDriver *driver = SceneManager->getVideoDriver();
     76
     77  driver->setMaterial(Material);
     78
     79  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     80  driver->drawMeshBuffer(mesh->getMeshBuffer(0));
    6981}
    7082
    71 ITriangleSelector* GenericObject::TriangleSelector(void)
    72 {
    73     return selector;
     83void GenericObject::setMesh(IMesh *mesh) {
     84  this->mesh = mesh;
     85  box = mesh->getBoundingBox();
     86
     87  selector = getSceneManager()->createTriangleSelector(mesh, this);
     88  setTriangleSelector(selector);
    7489}
    7590
    76 void GenericObject::OnRegisterSceneNode(void)
    77 {
    78     if (IsVisible)
    79         SceneManager->registerNodeForRendering(this);
     91IMesh *GenericObject::getMesh(void) { return mesh; }
    8092
    81     ISceneNode::OnRegisterSceneNode();
     93void GenericObject::setPosition(irr::core::vector3df pos) {
     94  ISceneNode::setPosition(ToIrrlichtCoordinates(pos));
    8295}
    8396
    84 void GenericObject::render(void)
    85 {
    86     IVideoDriver* driver = SceneManager->getVideoDriver();
     97void GenericObject::setRotation(irr::core::vector3df rotation) {
     98  Euler eulerA(rotation.X, rotation.Y, rotation.Z);
     99  Euler eulerB;
     100  Quaternion quatA, quatB;
     101  eulerA.ToQuaternion(quatA);
     102  quatB = ToIrrlichtOrientation(quatA);
     103  quatB.ToEuler(eulerB);
    87104
    88     driver->setMaterial(Material);
    89 
    90     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    91     driver->drawMeshBuffer(mesh->getMeshBuffer(0));
    92 }
    93 
    94 void GenericObject::setMesh(IMesh* mesh)
    95 {
    96     this->mesh=mesh;
    97     box=mesh->getBoundingBox();
    98 
    99     selector = getSceneManager()->createTriangleSelector(mesh,this);
    100     setTriangleSelector(selector);
    101 }
    102 
    103 IMesh* GenericObject::getMesh(void)
    104 {
    105     return mesh;
    106 }
    107 
    108 void GenericObject::setPosition(irr::core::vector3df pos)
    109 {
    110     ISceneNode::setPosition(ToIrrlichtCoordinates(pos));
    111 }
    112 
    113 void GenericObject::setRotation(irr::core::vector3df rotation)
    114 {
    115     Euler eulerA(rotation.X,rotation.Y,rotation.Z);
    116     Euler eulerB;
    117     Quaternion quatA,quatB;
    118     eulerA.ToQuaternion(quatA);
    119     quatB=ToIrrlichtOrientation(quatA);
    120     quatB.ToEuler(eulerB);
    121 
    122     ISceneNode::setRotation(Euler::ToDegree(1)*vector3df(eulerB.roll,eulerB.pitch,eulerB.yaw));
     105  ISceneNode::setRotation(Euler::ToDegree(1) *
     106                          vector3df(eulerB.roll, eulerB.pitch, eulerB.yaw));
    123107}
    124108

  • trunk/lib/FlairSimulator/src/Gui.cpp

    r10 r15  
    4040
    4141namespace {
    42     flair::simulator::Gui* gui_=NULL;
    43     std::vector <std::string> extensions;
    44     bool getGlInfo();
     42flair::simulator::Gui *gui_ = NULL;
     43std::vector<std::string> extensions;
     44bool getGlInfo();
    4545}
    4646
    47 namespace flair { namespace simulator {
     47namespace flair {
     48namespace simulator {
    4849
    49 Gui* getGui(void) {
    50     return gui_;
     50Gui *getGui(void) { return gui_; }
     51
     52bool noGui(void) {
     53  if (gui_ == NULL) {
     54    return true;
     55  } else {
     56    return false;
     57  }
    5158}
    5259
    53 bool noGui(void) {
    54     if(gui_==NULL) {
    55         return true;
    56     } else {
    57         return false;
     60// getGlinfo, code from Song Ho Ahn (song.ahn@gmail.com)
     61bool getGlInfo() {
     62  char *str = 0;
     63  char *tok = 0;
     64
     65  // get all extensions as a string
     66  str = (char *)glGetString(GL_EXTENSIONS);
     67
     68  // split extensions
     69  if (str) {
     70    tok = strtok((char *)str, " ");
     71    while (tok) {
     72      extensions.push_back(tok); // put a extension into struct
     73      tok = strtok(0, " ");      // next token
    5874    }
     75  } else {
     76    printf("cannot get gl extensions\n");
     77  }
     78
     79  // sort extension by alphabetical order
     80  std::sort(extensions.begin(), extensions.end());
    5981}
    6082
    61 //getGlinfo, code from Song Ho Ahn (song.ahn@gmail.com)
    62 bool getGlInfo() {
    63     char* str = 0;
    64     char* tok = 0;
     83// isGlExtensionSupported, code from Song Ho Ahn (song.ahn@gmail.com)
     84bool isGlExtensionSupported(const std::string &ext) {
     85  if (extensions.size() == 0)
     86    getGlInfo();
    6587
    66     // get all extensions as a string
    67     str = (char*)glGetString(GL_EXTENSIONS);
     88  // search corresponding extension
     89  std::vector<std::string>::const_iterator iter = extensions.begin();
     90  std::vector<std::string>::const_iterator endIter = extensions.end();
    6891
    69     // split extensions
    70     if(str) {
    71         tok = strtok((char*)str, " ");
    72         while(tok) {
    73             extensions.push_back(tok);    // put a extension into struct
    74             tok = strtok(0, " ");               // next token
    75         }
    76     } else {
    77         printf("cannot get gl extensions\n");
    78     }
    79 
    80     // sort extension by alphabetical order
    81     std::sort(extensions.begin(), extensions.end());
     92  while (iter != endIter) {
     93    if (ext == *iter)
     94      return true;
     95    else
     96      ++iter;
     97  }
     98  return false;
    8299}
    83100
    84 //isGlExtensionSupported, code from Song Ho Ahn (song.ahn@gmail.com)
    85 bool isGlExtensionSupported(const std::string& ext) {
    86     if(extensions.size()==0) getGlInfo();
     101float ToIrrlichtScale(float value) { return value * 100.; }
    87102
    88     // search corresponding extension
    89     std::vector<std::string>::const_iterator iter = extensions.begin();
    90     std::vector<std::string>::const_iterator endIter = extensions.end();
    91 
    92     while(iter != endIter) {
    93         if(ext == *iter)
    94             return true;
    95         else
    96             ++iter;
    97     }
    98     return false;
    99 }
    100 
    101 float ToIrrlichtScale(float value) {
    102     return value*100.;
    103 }
    104 
    105 float ToSimulatorScale(float value) {
    106     return value/100.;
    107 }
     103float ToSimulatorScale(float value) { return value / 100.; }
    108104
    109105vector3df ToIrrlichtCoordinates(vector3df vect) {
    110     return ToIrrlichtScale(1)*vector3df(vect.X,vect.Y,-vect.Z);
     106  return ToIrrlichtScale(1) * vector3df(vect.X, vect.Y, -vect.Z);
    111107}
    112108
    113109vector3df ToIrrlichtCoordinates(Vector3D vect) {
    114     return ToIrrlichtScale(1)*vector3df(vect.x,vect.y,-vect.z);
     110  return ToIrrlichtScale(1) * vector3df(vect.x, vect.y, -vect.z);
    115111}
    116112
    117113Vector3D ToSimulatorCoordinates(vector3df vect) {
    118     return ToSimulatorScale(1)*Vector3D(vect.X,vect.Y,-vect.Z);
     114  return ToSimulatorScale(1) * Vector3D(vect.X, vect.Y, -vect.Z);
    119115}
    120116
    121117Quaternion ToIrrlichtOrientation(Quaternion quat) {
    122     /* original code
    123     Euler euler;
    124     quat.ToEuler(euler);
    125     matrix4 m;
    126     m.setRotationRadians(vector3df(0, 0, euler.yaw));
    127     matrix4 n;
    128     n.setRotationRadians(vector3df(0, -euler.pitch,0));
    129     m *= n;
    130     n.setRotationRadians(vector3df(-euler.roll, 0, 0));
    131     m *= n;
    132     */
    133     //seems to be equivalent to:
    134     return Quaternion(quat.q0,-quat.q1,-quat.q2,quat.q3);
     118  /* original code
     119  Euler euler;
     120  quat.ToEuler(euler);
     121  matrix4 m;
     122  m.setRotationRadians(vector3df(0, 0, euler.yaw));
     123  matrix4 n;
     124  n.setRotationRadians(vector3df(0, -euler.pitch,0));
     125  m *= n;
     126  n.setRotationRadians(vector3df(-euler.roll, 0, 0));
     127  m *= n;
     128  */
     129  // seems to be equivalent to:
     130  return Quaternion(quat.q0, -quat.q1, -quat.q2, quat.q3);
    135131}
    136132
    137 Gui::Gui(const Simulator* parent,std::string name,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,E_DRIVER_TYPE driver_type): Object(parent,name,"Gui") {
    138     if(gui_!=NULL) Err("Gui should be instanced only one time\n");
    139     pimpl_=new Gui_impl(this,app_width,app_height,scene_width,scene_height,media_path,driver_type);
    140     gui_=this;
     133Gui::Gui(const Simulator *parent, std::string name, int app_width,
     134         int app_height, int scene_width, int scene_height,
     135         std::string media_path, E_DRIVER_TYPE driver_type)
     136    : Object(parent, name, "Gui") {
     137  if (gui_ != NULL)
     138    Err("Gui should be instanced only one time\n");
     139  pimpl_ = new Gui_impl(this, app_width, app_height, scene_width, scene_height,
     140                        media_path, driver_type);
     141  gui_ = this;
    141142}
    142143
    143 Gui::~Gui() {
    144     delete pimpl_;
     144Gui::~Gui() { delete pimpl_; }
     145
     146float Gui::getAspectRatio(void) const {
     147  return (float)pimpl_->scene_width / (float)pimpl_->scene_height;
    145148}
    146149
    147 float Gui::getAspectRatio(void) const {
    148     return (float)pimpl_->scene_width/(float)pimpl_->scene_height;
     150ISceneManager *Gui::getSceneManager(void) const { return pimpl_->smgr; }
     151
     152void Gui::setMesh(std::string file, vector3df position, vector3df rotation,
     153                  vector3df scale) {
     154  pimpl_->setMesh(file, position, rotation, scale);
    149155}
    150156
    151 ISceneManager* Gui::getSceneManager(void) const {
    152     return pimpl_->smgr;
     157vector3df Gui::getRotation(void) const { return pimpl_->node->getRotation(); }
     158
     159IrrlichtDevice *Gui::getDevice(void) const { return pimpl_->device; }
     160
     161ITexture *Gui::getTexture(std::string filename) const {
     162  filename = pimpl_->media_path + "/" + filename;
     163  return pimpl_->driver->getTexture(filename.c_str());
    153164}
    154165
    155 void Gui::setMesh(std::string file,vector3df position,vector3df rotation,vector3df scale) {
    156     pimpl_->setMesh(file,position,rotation,scale);
    157 }
    158 
    159 vector3df Gui::getRotation(void) const {
    160     return pimpl_->node->getRotation();
    161 }
    162 
    163 IrrlichtDevice *Gui::getDevice(void) const {
    164     return pimpl_->device;
    165 }
    166 
    167 ITexture* Gui::getTexture(std::string filename) const {
    168     filename=pimpl_->media_path+ "/" +filename;
    169     return pimpl_->driver->getTexture(filename.c_str());
    170 }
    171 
    172 IAnimatedMesh* Gui::getMesh(std::string filename) const {
    173     filename=pimpl_->media_path+ "/" +filename;
    174     return pimpl_->smgr->getMesh(filename.c_str());
     166IAnimatedMesh *Gui::getMesh(std::string filename) const {
     167  filename = pimpl_->media_path + "/" + filename;
     168  return pimpl_->smgr->getMesh(filename.c_str());
    175169}
    176170
    177171} // end namespace simulator
    178172} // end namespace flair
    179 #endif //GL
     173#endif // GL

  • trunk/lib/FlairSimulator/src/Gui.h

    r10 r15  
    2222#include <vector3d.h>
    2323
    24 namespace irr
    25 {
    26     class IrrlichtDevice;
    27     namespace video
    28     {
    29         class ITexture;
    30     }
    31     namespace scene
    32     {
    33         class IAnimatedMesh;
    34         class ISceneManager;
    35     }
     24namespace irr {
     25class IrrlichtDevice;
     26namespace video {
     27class ITexture;
     28}
     29namespace scene {
     30class IAnimatedMesh;
     31class ISceneManager;
     32}
    3633}
    3734
    38 namespace flair
    39 {
    40     namespace core
    41     {
    42        class Object;
    43        class Vector3D;
    44        class Euler;
    45        class Quaternion;
    46     }
     35namespace flair {
     36namespace core {
     37class Object;
     38class Vector3D;
     39class Euler;
     40class Quaternion;
     41}
    4742}
    4843
    … …  
    5146class Model_impl;
    5247
    53 namespace flair
    54 {
    55 namespace simulator
    56 {
    57     class Simulator;
     48namespace flair {
     49namespace simulator {
     50class Simulator;
    5851
    59     class Gui: public core::Object
    60     {
    61         friend class ::Simulator_impl;
     52class Gui : public core::Object {
     53  friend class ::Simulator_impl;
    6254
    63         public:
    64             Gui(const Simulator* parent,std::string name,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,irr::video::E_DRIVER_TYPE driver_type=irr::video::EDT_OPENGL);
    65             ~Gui();
    66             irr::core::vector3df getRotation(void) const;
    67             irr::video::ITexture* getTexture(std::string filename) const;
    68             irr::scene::IAnimatedMesh* getMesh(std::string filename) const;
    69             irr::scene::ISceneManager* getSceneManager(void) const;
    70             float getAspectRatio(void) const;
     55public:
     56  Gui(const Simulator *parent, std::string name, int app_width, int app_height,
     57      int scene_width, int scene_height, std::string media_path,
     58      irr::video::E_DRIVER_TYPE driver_type = irr::video::EDT_OPENGL);
     59  ~Gui();
     60  irr::core::vector3df getRotation(void) const;
     61  irr::video::ITexture *getTexture(std::string filename) const;
     62  irr::scene::IAnimatedMesh *getMesh(std::string filename) const;
     63  irr::scene::ISceneManager *getSceneManager(void) const;
     64  float getAspectRatio(void) const;
    7165
    72         protected:
    73             irr::IrrlichtDevice *getDevice(void) const;
    74             void setMesh(std::string file,irr::core::vector3df position = irr::core::vector3df(0,0,0),irr::core::vector3df rotation= irr::core::vector3df(0,0,0),irr::core::vector3df scale= irr::core::vector3df(1,1,1));
     66protected:
     67  irr::IrrlichtDevice *getDevice(void) const;
     68  void setMesh(std::string file,
     69               irr::core::vector3df position = irr::core::vector3df(0, 0, 0),
     70               irr::core::vector3df rotation = irr::core::vector3df(0, 0, 0),
     71               irr::core::vector3df scale = irr::core::vector3df(1, 1, 1));
    7572
    76         private:
    77             Gui_impl *pimpl_;
    78     };
     73private:
     74  Gui_impl *pimpl_;
     75};
    7976
    80     /*!
    81     * \brief get Gui
    82     *
    83     * \return the Gui
    84     */
    85     Gui* getGui(void);
     77/*!
     78* \brief get Gui
     79*
     80* \return the Gui
     81*/
     82Gui *getGui(void);
    8683
    87     bool noGui(void);
     84bool noGui(void);
    8885
    89     bool isGlExtensionSupported(const std::string& ext); // check if a extension is supported
     86bool isGlExtensionSupported(
     87    const std::string &ext); // check if a extension is supported
    9088
    91     /*!
    92     * \brief Convert to irrlicht scale
    93     *
    94     * \param value value in simulator scale
    95     *
    96     * \return value in irrlicht scale
    97     */
    98     float ToIrrlichtScale(float value);
     89/*!
     90* \brief Convert to irrlicht scale
     91*
     92* \param value value in simulator scale
     93*
     94* \return value in irrlicht scale
     95*/
     96float ToIrrlichtScale(float value);
    9997
    100     /*!
    101     * \brief Convert to simulator scale
    102     *
    103     * \param value value in irrlicht scale
    104     *
    105     * \return value in simulator scale
    106     */
    107     float ToSimulatorScale(float value);
     98/*!
     99* \brief Convert to simulator scale
     100*
     101* \param value value in irrlicht scale
     102*
     103* \return value in simulator scale
     104*/
     105float ToSimulatorScale(float value);
    108106
    109     /*!
    110     * \brief Convert to irrlicht coordinates
    111     *
    112     * irrlicht is left handed and as a different scale
    113     *
    114     * \param vect vector in simulator coordinates
    115     *
    116     * \return vector in irrlicht coordinates
    117     */
    118     irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df vect);
     107/*!
     108* \brief Convert to irrlicht coordinates
     109*
     110* irrlicht is left handed and as a different scale
     111*
     112* \param vect vector in simulator coordinates
     113*
     114* \return vector in irrlicht coordinates
     115*/
     116irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df vect);
    119117
    120     /*!
    121     * \brief Convert to irrlicht coordinates
    122     *
    123     * irrlicht is left handed and as a different scale
    124     *
    125     * \param vect vector in simulator coordinates
    126     *
    127     * \return vector in irrlicht coordinates
    128     */
    129     irr::core::vector3df ToIrrlichtCoordinates(core::Vector3D vect);
     118/*!
     119* \brief Convert to irrlicht coordinates
     120*
     121* irrlicht is left handed and as a different scale
     122*
     123* \param vect vector in simulator coordinates
     124*
     125* \return vector in irrlicht coordinates
     126*/
     127irr::core::vector3df ToIrrlichtCoordinates(core::Vector3D vect);
    130128
    131     /*!
    132     * \brief Convert to simulator coordinates
    133     *
    134     * irrlicht is left handed and as a different scale
    135     *
    136     * \param vect vector in irrlicht coordinates
    137     *
    138     * \return vector in simulator coordinates
    139     */
    140     core::Vector3D ToSimulatorCoordinates(irr::core::vector3df vect);
     129/*!
     130* \brief Convert to simulator coordinates
     131*
     132* irrlicht is left handed and as a different scale
     133*
     134* \param vect vector in irrlicht coordinates
     135*
     136* \return vector in simulator coordinates
     137*/
     138core::Vector3D ToSimulatorCoordinates(irr::core::vector3df vect);
    141139
    142     /*!
    143     * \brief Convert to irrlicht orientation
    144     *
    145     * irrlicht is left handed
    146     *
    147     * \param quat quaternion in simulator frame
    148     *
    149     * \return quaternion in irrlicht frame
    150     */
    151     core::Quaternion ToIrrlichtOrientation(core::Quaternion quat);
     140/*!
     141* \brief Convert to irrlicht orientation
     142*
     143* irrlicht is left handed
     144*
     145* \param quat quaternion in simulator frame
     146*
     147* \return quaternion in irrlicht frame
     148*/
     149core::Quaternion ToIrrlichtOrientation(core::Quaternion quat);
    152150
    153151} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Gui_impl.cpp

    r10 r15  
    3737using namespace flair::simulator;
    3838
    39 class MyEventReceiver : public IEventReceiver
    40 {
     39class MyEventReceiver : public IEventReceiver {
    4140public:
    42         // This is the one method that we have to implement
    43         virtual bool OnEvent(const SEvent& event)
    44         {
    45                 // Remember whether each key is down or up
    46                 if (event.EventType == EET_KEY_INPUT_EVENT )
    47                         KeyIsDown[event.KeyInput.Key] = event.KeyInput.PressedDown;
    48 
    49                 //Send all other events to camera
    50                 if (event.EventType == EET_MOUSE_INPUT_EVENT && camera)
    51                         //return camera->OnEvent(event);
    52                         return camera->MouseMoved(event,cursorControl->getRelativePosition());
    53 
    54                 if(model)
    55                         return model->OnEvent(event);
    56 
    57                 return false;
    58         }
    59 
    60         // This is used to check whether a key is being held down
    61         virtual bool IsKeyDown(EKEY_CODE keyCode)
    62         {
    63                 if(KeyIsDown[keyCode]==true)
    64                 {
    65                         KeyIsDown[keyCode]=false;
    66                         return true;
    67                 }
    68                 else
    69                 {
    70                         return false;
    71                 }
    72         }
    73 
    74         MyEventReceiver(ICursorControl* cursorControl)
    75         {
    76                 this->cursorControl=cursorControl;
    77                 camera=NULL;
    78                 model=NULL;
    79                 for (u32 i=0; i<KEY_KEY_CODES_COUNT; ++i)
    80                         KeyIsDown[i] = false;
    81         }
    82         void SetCamera(AnimPoursuite* camera)
    83         {
    84                 this->camera=camera;
    85         }
    86         void SetModel(Model* model)
    87         {
    88                 this->model=model;
    89         }
     41  // This is the one method that we have to implement
     42  virtual bool OnEvent(const SEvent &event) {
     43    // Remember whether each key is down or up
     44    if (event.EventType == EET_KEY_INPUT_EVENT)
     45      KeyIsDown[event.KeyInput.Key] = event.KeyInput.PressedDown;
     46
     47    // Send all other events to camera
     48    if (event.EventType == EET_MOUSE_INPUT_EVENT && camera)
     49      // return camera->OnEvent(event);
     50      return camera->MouseMoved(event, cursorControl->getRelativePosition());
     51
     52    if (model)
     53      return model->OnEvent(event);
     54
     55    return false;
     56  }
     57
     58  // This is used to check whether a key is being held down
     59  virtual bool IsKeyDown(EKEY_CODE keyCode) {
     60    if (KeyIsDown[keyCode] == true) {
     61      KeyIsDown[keyCode] = false;
     62      return true;
     63    } else {
     64      return false;
     65    }
     66  }
     67
     68  MyEventReceiver(ICursorControl *cursorControl) {
     69    this->cursorControl = cursorControl;
     70    camera = NULL;
     71    model = NULL;
     72    for (u32 i = 0; i < KEY_KEY_CODES_COUNT; ++i)
     73      KeyIsDown[i] = false;
     74  }
     75  void SetCamera(AnimPoursuite *camera) { this->camera = camera; }
     76  void SetModel(Model *model) { this->model = model; }
    9077
    9178private:
    92         // We use this array to store the current state of each key
    93         bool KeyIsDown[KEY_KEY_CODES_COUNT];
    94         AnimPoursuite* camera;
    95         Model* model;
    96         ICursorControl* cursorControl;
     79  // We use this array to store the current state of each key
     80  bool KeyIsDown[KEY_KEY_CODES_COUNT];
     81  AnimPoursuite *camera;
     82  Model *model;
     83  ICursorControl *cursorControl;
    9784};
    9885
    99 Gui_impl::Gui_impl(Gui* self,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,E_DRIVER_TYPE driver_type)
    100 {
    101         this->self=self;
    102         dbtFile_w=NULL;
    103         dbtFile_r=NULL;
    104         this->media_path=media_path;
    105         this->scene_width=scene_width;
    106         this->scene_height=scene_height;
    107 
    108         device =createDevice(driver_type, dimension2d<u32>(app_width, app_height),16, false, false, false);
    109         receiver=new MyEventReceiver(device->getCursorControl());
    110         device->setEventReceiver(receiver);
    111         device->getLogger()->setLogLevel(ELL_NONE);
    112 
    113         device->getCursorControl()->setVisible(false);
    114         device->setResizable(false);
    115 
    116         //font = device->getGUIEnvironment()->getBuiltInFont();
    117         driver = device->getVideoDriver();
    118         smgr = device->getSceneManager();
    119 
    120         smgr->setAmbientLight(video::SColorf(1,1,1));
    121 
    122 
    123         /*
    124     env = device->getGUIEnvironment();
     86Gui_impl::Gui_impl(Gui *self, int app_width, int app_height, int scene_width,
     87                   int scene_height, std::string media_path,
     88                   E_DRIVER_TYPE driver_type) {
     89  this->self = self;
     90  dbtFile_w = NULL;
     91  dbtFile_r = NULL;
     92  this->media_path = media_path;
     93  this->scene_width = scene_width;
     94  this->scene_height = scene_height;
     95
     96  device = createDevice(driver_type, dimension2d<u32>(app_width, app_height),
     97                        16, false, false, false);
     98  receiver = new MyEventReceiver(device->getCursorControl());
     99  device->setEventReceiver(receiver);
     100  device->getLogger()->setLogLevel(ELL_NONE);
     101
     102  device->getCursorControl()->setVisible(false);
     103  device->setResizable(false);
     104
     105  // font = device->getGUIEnvironment()->getBuiltInFont();
     106  driver = device->getVideoDriver();
     107  smgr = device->getSceneManager();
     108
     109  smgr->setAmbientLight(video::SColorf(1, 1, 1));
     110
     111  /*
     112env = device->getGUIEnvironment();
    125113IGUISkin* skin = env->getSkin();
    126         font = env->getFont("./fonthaettenschweiler.bmp");
    127 
    128         if (font)
    129                 skin->setFont(font);
    130 
    131         // create menu
    132 
    133         IGUIContextMenu* menu = env->addMenu();
    134         menu->setMinSize(core::dimension2du(640,20));
    135         menu->addItem(L"File", -1, true, true);
    136         menu->addItem(L"View", -1, true, true);
    137         menu->addItem(L"Camera", -1, true, true);
    138         menu->addItem(L"Help", -1, true, true);
    139 
    140         // disable alpha
    141 
    142         for (s32 i=0; i<gui::EGDC_COUNT ; ++i)
    143         {
    144                 video::SColor col = env->getSkin()->getColor((gui::EGUI_DEFAULT_COLOR)i);
    145                 col.setAlpha(255);
    146                 env->getSkin()->setColor((gui::EGUI_DEFAULT_COLOR)i, col);
    147         }
    148          */
    149 }
    150 
    151 Gui_impl::~Gui_impl()
    152 {
    153         //printf("del Gui_impl\n");
    154         device->drop();
    155 
    156         delete receiver;
    157         // printf("del Gui_impl ok\n");
    158 }
    159 
    160 void Gui_impl::setMesh(std::string file,vector3df position,vector3df rotation,vector3df scale)
    161 {
    162         IAnimatedMesh* mesh = smgr->getMesh(file.c_str());
    163 
    164         if (!mesh)      {
    165                 // model could not be loaded
    166         self->Err("Model %s could not be loaded\n",file.c_str());
    167         return;
    168         }
    169 
    170 
    171         node = smgr->addOctreeSceneNode(mesh->getMesh(0), 0, -1, 1024);
    172         node->setPosition(position);
    173         rotation+=irr::core::vector3df(90,0,Euler::ToDegree(getSimulator()->Yaw()));
    174         node->setRotation(rotation);
    175         for(int i=0; i<node->getMaterialCount();i++){
    176                 node->getMaterial(i).TextureLayer->TextureWrapU = video::ETC_REPEAT;
    177                 node->getMaterial(i).TextureLayer->TextureWrapV = video::ETC_REPEAT;
    178         }
    179         //Ceillig
    180         //node->getMaterial(0).getTextureMatrix(0).setTextureScale(scale.X/2.0,scale.Z/2.0);
    181         //Walls
    182         node->getMaterial(1).getTextureMatrix(0).setTextureScale(1/(scale.Y/2.5),1/(scale.Z/2.5));
    183         //Floor
    184         node->getMaterial(2).getTextureMatrix(0).setTextureScale(1/(scale.X/20.0),1/(scale.Z/20.0));
    185 
    186         node->setScale(scale);
    187         //selector
    188         selector = smgr->createOctreeTriangleSelector(node->getMesh(), node, 128);
    189         node->setTriangleSelector(selector);
    190 }
    191 
    192 void Gui_impl::RunGui(std::vector<Model*> models,std::vector<GenericObject*> objects)
    193 {
    194         int lastFPS = -1;
    195         int cam_id=0;
    196 
    197         receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
    198         receiver->SetModel(models.at(cam_id));
    199 
    200         for(size_t i=0;i<models.size();i++)
    201         {
    202                 models.at(i)->Draw();
    203         }
    204 
    205         for(size_t i=0;i<models.size();i++)
    206         {
    207                 models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(selector);
    208                 for(size_t j=0;j<objects.size();j++)
    209                 {
    210                         models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(objects.at(j)->TriangleSelector());
    211                 }
    212                 for(size_t j=0;j<models.size();j++)
    213                 {
    214                         if(i==j) continue;
    215                         models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(models.at(j)->pimpl_->TriangleSelector());
    216                 }
    217         }
    218 
    219         selector->drop(); // As soon as we're done with the selector, drop it.*/
    220 
    221         //wait all models to be started
    222         for(size_t i=0;i<models.size();i++)
    223         {
    224                 models.at(i)->pimpl_->SynchronizationPoint();
    225         }
    226 
    227         setWindowCaption(models.at(0),0);
    228 
    229         while(device->run())
    230         {
    231                 if(dbtFile_r!=NULL)//rejeu
    232                 {
    233                         takeScreenshot();//on enregistre l'image precedente
    234                         road_time_t time;
    235                         road_timerange_t tr = 0;
    236                         if(read_hdfile(dbtFile_r,(void*)dbtbuf,&time,&tr)!=0)
    237                         {
    238                                 vector3df vect;
    239                                 char *buf=dbtbuf;
    240                                 for(size_t i=0;i<models.size();i++)
    241                                 {
    242                                         models.at(i)->ReaddbtBuf(buf);
    243                                         buf+=models.at(i)->dbtSize();
    244                                 }
    245                         }
    246                         else
    247                         {
    248                                 //Printf("fin play\n");
    249                                 close_hdfile(dbtFile_r);
    250                                 dbtFile_r=NULL;
    251                                 free(dbtbuf);
    252                                 for(size_t i=0;i<models.size();i++)
    253                                 {
    254                                         models.at(i)->pimpl_->Resume();
    255                                 }
    256                         }
    257                 }
    258                 else//mode normal
    259                 {
    260                         for(size_t i=0;i<models.size();i++)
    261                         {
    262                                 models.at(i)->pimpl_->UpdatePos();
    263                         }
    264                 }
    265 
    266                 driver->beginScene(true, true, video::SColor(255,200,200,200));
    267 
    268                 //vue poursuite
    269                 smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
    270                 driver->setViewPort(core::rect<s32>(0,0,scene_width,scene_height));
    271                 smgr->drawAll();//commente voir plus bas
    272                 /*
    273         env->drawAll();
    274 if (font)
    275 {
    276                                 font->draw(L"This demo shows that Irrlicht is also capable of drawing 2D graphics.",
    277                                         core::rect<s32>(130,10,300,50),
    278                                         video::SColor(255,255,255,255));
    279 }
    280 else
    281 {
    282     printf("err\n");
    283 }
    284 device->setWindowCaption(L"toto");*/
    285 
    286                 if(dbtFile_r==NULL)//mode normal
    287                 {
    288                         for(size_t i=0;i<models.size();i++)
    289                         {
    290                                 models.at(i)->pimpl_->CheckCollision();
    291                         }
    292                 }
    293 
    294                 for(size_t i=0;i<models.size();i++)
    295                 {
    296                         models.at(i)->ProcessUpdate(NULL);
    297                 }
    298 
    299                 //on fait ca ici, devrait etre un peu plus haut
    300                 //mais a priori souci avec models.at(i)->pimpl_->CheckCollision(); (setelipsoid?)
    301                 smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
    302                 driver->setViewPort(core::rect<s32>(0,0,scene_width,scene_height));
    303                 smgr->drawAll();
    304 
    305                 driver->endScene();
    306 
    307                 int fps =driver->getFPS();
    308                 //printf("fps %i\n",fps);
    309                 if (lastFPS != fps)
    310                 {
    311                         setWindowCaption(models.at(cam_id),fps);
    312                         lastFPS = fps;
    313                 }
    314 
    315                 if(receiver->IsKeyDown(KEY_PRIOR))
    316                 {
    317                         cam_id++;
    318                         if(cam_id>=(int)models.size()) cam_id=0;
    319                         receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
    320                         receiver->SetModel(models.at(cam_id));
    321                         setWindowCaption(models.at(cam_id),fps);
    322                 }
    323                 if(receiver->IsKeyDown(KEY_NEXT))
    324                 {
    325                         cam_id--;
    326                         if(cam_id<0) cam_id=models.size()-1;
    327                         receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
    328                         receiver->SetModel(models.at(cam_id));
    329                         setWindowCaption(models.at(cam_id),fps);
    330                 }
    331 
    332                 //enregistrement DBT
    333                 if(receiver->IsKeyDown(KEY_KEY_R) && dbtFile_w==NULL)
    334                 {
    335                         dbtFile_w = inithdFile((char*)"./record.dbt",UAV,dbtSize(models));
    336                         dbtbuf=(char*)malloc(dbtSize(models));
    337                 }
    338                 if(receiver->IsKeyDown(KEY_KEY_S) && dbtFile_w!=NULL)
    339                 {
    340                         close_hdfile(dbtFile_w);
    341                         dbtFile_w=NULL;
    342                         free(dbtbuf);
    343                         //rt_printf("stop rec\n");
    344                 }
    345                 if(dbtFile_w!=NULL)
    346                 {
    347                         Time time=GetTime();
    348                         vector3df vect;
    349                         char *buf=dbtbuf;
    350 
    351                         for(size_t i=0;i<models.size();i++)
    352                         {
    353                                 models.at(i)->WritedbtBuf(buf);
    354                                 buf+=models.at(i)->dbtSize();
    355                         }
    356 
    357                         write_hdfile(dbtFile_w,dbtbuf,(road_time_t)(time/1000),(road_timerange_t)(time%1000),dbtSize(models));
    358                 }
    359 
    360                 //lecture dbt
    361                 if(receiver->IsKeyDown(KEY_KEY_P) && dbtFile_r==NULL)
    362                 {
    363                         dbtFile_r = open_hdfile((char*)"./record.dbt",READ_MODE);
    364                         dbtbuf=(char*)malloc(dbtSize(models));
    365                         //on suspend les models pour ne pas interferer
    366                         for(size_t i=0;i<models.size();i++)
    367                         {
    368                                 models.at(i)->pimpl_->Suspend();
    369                         }
    370                 }
    371                 if(receiver->IsKeyDown(KEY_KEY_S) && dbtFile_r!=NULL)
    372                 {
    373                         //rt_printf("stop play\n");
    374                         close_hdfile(dbtFile_r);
    375                         dbtFile_r=NULL;
    376                         free(dbtbuf);
    377                         //on resume les models
    378                         for(size_t i=0;i<models.size();i++)
    379                         {
    380                                 models.at(i)->pimpl_->Resume();
    381                         }
    382                 }
    383 
    384         }
    385 
    386         receiver->SetCamera(NULL);
    387         receiver->SetModel(NULL);
    388 
    389 }
    390 
    391 void Gui_impl::setWindowCaption(Object* object, int fps)
    392 {
    393         std::ostringstream text;
    394         text << "Cam: " << object->ObjectName().c_str() << ", Kbd: " << object->ObjectName().c_str() << ", FPS: " << fps;
    395 
    396         device->setWindowCaption(stringw(text.str().c_str()).c_str());
    397 }
    398 
    399 void Gui_impl::takeScreenshot(void)
    400 {
    401         static int cpt=0;
    402         //get image from the last rendered frame
    403         IImage* const image = driver->createScreenShot();
    404         if (image) //should always be true, but you never know. ;)
    405         {
    406                 //construct a filename, consisting of local time and file extension
    407                 c8 filename[64];
    408                 //snprintf(filename, 64, "screenshot_%u.png", device->getTimer()->getRealTime());
    409                 snprintf(filename, 64, "screenshot_%u.png", cpt);
    410                 cpt++;
    411                 //write screenshot to file
    412                 if (!driver->writeImageToFile(image, filename))
    413                         device->getLogger()->log(L"Failed to take screenshot.", ELL_WARNING);
    414 
    415                 //Don't forget to drop image since we don't need it anymore.
    416                 image->drop();
    417         }
    418 }
    419 
    420 size_t Gui_impl::dbtSize(std::vector<Model*> models)
    421 {
    422         size_t size=0;
    423         for(size_t i=0;i<models.size();i++)
    424         {
    425                 size+=models.at(i)->dbtSize();
    426         }
    427 
    428         return size;
    429 }
    430 #endif //GL
     114  font = env->getFont("./fonthaettenschweiler.bmp");
     115
     116  if (font)
     117          skin->setFont(font);
     118
     119  // create menu
     120
     121  IGUIContextMenu* menu = env->addMenu();
     122  menu->setMinSize(core::dimension2du(640,20));
     123  menu->addItem(L"File", -1, true, true);
     124  menu->addItem(L"View", -1, true, true);
     125  menu->addItem(L"Camera", -1, true, true);
     126  menu->addItem(L"Help", -1, true, true);
     127
     128  // disable alpha
     129
     130  for (s32 i=0; i<gui::EGDC_COUNT ; ++i)
     131  {
     132          video::SColor col =
     133env->getSkin()->getColor((gui::EGUI_DEFAULT_COLOR)i);
     134          col.setAlpha(255);
     135          env->getSkin()->setColor((gui::EGUI_DEFAULT_COLOR)i, col);
     136  }
     137   */
     138}
     139
     140Gui_impl::~Gui_impl() {
     141  // printf("del Gui_impl\n");
     142  device->drop();
     143
     144  delete receiver;
     145  // printf("del Gui_impl ok\n");
     146}
     147
     148void Gui_impl::setMesh(std::string file, vector3df position, vector3df rotation,
     149                       vector3df scale) {
     150  IAnimatedMesh *mesh = smgr->getMesh(file.c_str());
     151
     152  if (!mesh) {
     153    // model could not be loaded
     154    self->Err("Model %s could not be loaded\n", file.c_str());
     155    return;
     156  }
     157
     158  node = smgr->addOctreeSceneNode(mesh->getMesh(0), 0, -1, 1024);
     159  node->setPosition(position);
     160  rotation +=
     161      irr::core::vector3df(90, 0, Euler::ToDegree(getSimulator()->Yaw()));
     162  node->setRotation(rotation);
     163  for (int i = 0; i < node->getMaterialCount(); i++) {
     164    node->getMaterial(i).TextureLayer->TextureWrapU = video::ETC_REPEAT;
     165    node->getMaterial(i).TextureLayer->TextureWrapV = video::ETC_REPEAT;
     166  }
     167  // Ceillig
     168  // node->getMaterial(0).getTextureMatrix(0).setTextureScale(scale.X/2.0,scale.Z/2.0);
     169  // Walls
     170  node->getMaterial(1).getTextureMatrix(0).setTextureScale(1 / (scale.Y / 2.5),
     171                                                           1 / (scale.Z / 2.5));
     172  // Floor
     173  node->getMaterial(2).getTextureMatrix(0).setTextureScale(
     174      1 / (scale.X / 20.0), 1 / (scale.Z / 20.0));
     175
     176  node->setScale(scale);
     177  // selector
     178  selector = smgr->createOctreeTriangleSelector(node->getMesh(), node, 128);
     179  node->setTriangleSelector(selector);
     180}
     181
     182void Gui_impl::RunGui(std::vector<Model *> models,
     183                      std::vector<GenericObject *> objects) {
     184  int lastFPS = -1;
     185  int cam_id = 0;
     186
     187  receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
     188  receiver->SetModel(models.at(cam_id));
     189
     190  for (size_t i = 0; i < models.size(); i++) {
     191    models.at(i)->Draw();
     192  }
     193
     194  for (size_t i = 0; i < models.size(); i++) {
     195    models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(selector);
     196    for (size_t j = 0; j < objects.size(); j++) {
     197      models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(
     198          objects.at(j)->TriangleSelector());
     199    }
     200    for (size_t j = 0; j < models.size(); j++) {
     201      if (i == j)
     202        continue;
     203      models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(
     204          models.at(j)->pimpl_->TriangleSelector());
     205    }
     206  }
     207
     208  selector->drop(); // As soon as we're done with the selector, drop it.*/
     209
     210  // wait all models to be started
     211  for (size_t i = 0; i < models.size(); i++) {
     212    models.at(i)->pimpl_->SynchronizationPoint();
     213  }
     214
     215  setWindowCaption(models.at(0), 0);
     216
     217  while (device->run()) {
     218    if (dbtFile_r != NULL) // rejeu
     219    {
     220      takeScreenshot(); // on enregistre l'image precedente
     221      road_time_t time;
     222      road_timerange_t tr = 0;
     223      if (read_hdfile(dbtFile_r, (void *)dbtbuf, &time, &tr) != 0) {
     224        vector3df vect;
     225        char *buf = dbtbuf;
     226        for (size_t i = 0; i < models.size(); i++) {
     227          models.at(i)->ReaddbtBuf(buf);
     228          buf += models.at(i)->dbtSize();
     229        }
     230      } else {
     231        // Printf("fin play\n");
     232        close_hdfile(dbtFile_r);
     233        dbtFile_r = NULL;
     234        free(dbtbuf);
     235        for (size_t i = 0; i < models.size(); i++) {
     236          models.at(i)->pimpl_->Resume();
     237        }
     238      }
     239    } else // mode normal
     240    {
     241      for (size_t i = 0; i < models.size(); i++) {
     242        models.at(i)->pimpl_->UpdatePos();
     243      }
     244    }
     245
     246    driver->beginScene(true, true, video::SColor(255, 200, 200, 200));
     247
     248    // vue poursuite
     249    smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
     250    driver->setViewPort(core::rect<s32>(0, 0, scene_width, scene_height));
     251    smgr->drawAll(); // commente voir plus bas
     252                     /*
     253                 env->drawAll();
     254                 if (font)
     255                 {
     256                                     font->draw(L"This demo shows that Irrlicht is also capable
     257                 of drawing 2D graphics.",
     258                                             core::rect<s32>(130,10,300,50),
     259                                             video::SColor(255,255,255,255));
     260                 }
     261                 else
     262                 {
     263                 printf("err\n");
     264                 }
     265                 device->setWindowCaption(L"toto");*/
     266
     267    if (dbtFile_r == NULL) // mode normal
     268    {
     269      for (size_t i = 0; i < models.size(); i++) {
     270        models.at(i)->pimpl_->CheckCollision();
     271      }
     272    }
     273
     274    for (size_t i = 0; i < models.size(); i++) {
     275      models.at(i)->ProcessUpdate(NULL);
     276    }
     277
     278    // on fait ca ici, devrait etre un peu plus haut
     279    // mais a priori souci avec models.at(i)->pimpl_->CheckCollision();
     280    // (setelipsoid?)
     281    smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
     282    driver->setViewPort(core::rect<s32>(0, 0, scene_width, scene_height));
     283    smgr->drawAll();
     284
     285    driver->endScene();
     286
     287    int fps = driver->getFPS();
     288    // printf("fps %i\n",fps);
     289    if (lastFPS != fps) {
     290      setWindowCaption(models.at(cam_id), fps);
     291      lastFPS = fps;
     292    }
     293
     294    if (receiver->IsKeyDown(KEY_PRIOR)) {
     295      cam_id++;
     296      if (cam_id >= (int)models.size())
     297        cam_id = 0;
     298      receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
     299      receiver->SetModel(models.at(cam_id));
     300      setWindowCaption(models.at(cam_id), fps);
     301    }
     302    if (receiver->IsKeyDown(KEY_NEXT)) {
     303      cam_id--;
     304      if (cam_id < 0)
     305        cam_id = models.size() - 1;
     306      receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
     307      receiver->SetModel(models.at(cam_id));
     308      setWindowCaption(models.at(cam_id), fps);
     309    }
     310
     311    // enregistrement DBT
     312    if (receiver->IsKeyDown(KEY_KEY_R) && dbtFile_w == NULL) {
     313      dbtFile_w = inithdFile((char *)"./record.dbt", UAV, dbtSize(models));
     314      dbtbuf = (char *)malloc(dbtSize(models));
     315    }
     316    if (receiver->IsKeyDown(KEY_KEY_S) && dbtFile_w != NULL) {
     317      close_hdfile(dbtFile_w);
     318      dbtFile_w = NULL;
     319      free(dbtbuf);
     320      // rt_printf("stop rec\n");
     321    }
     322    if (dbtFile_w != NULL) {
     323      Time time = GetTime();
     324      vector3df vect;
     325      char *buf = dbtbuf;
     326
     327      for (size_t i = 0; i < models.size(); i++) {
     328        models.at(i)->WritedbtBuf(buf);
     329        buf += models.at(i)->dbtSize();
     330      }
     331
     332      write_hdfile(dbtFile_w, dbtbuf, (road_time_t)(time / 1000),
     333                   (road_timerange_t)(time % 1000), dbtSize(models));
     334    }
     335
     336    // lecture dbt
     337    if (receiver->IsKeyDown(KEY_KEY_P) && dbtFile_r == NULL) {
     338      dbtFile_r = open_hdfile((char *)"./record.dbt", READ_MODE);
     339      dbtbuf = (char *)malloc(dbtSize(models));
     340      // on suspend les models pour ne pas interferer
     341      for (size_t i = 0; i < models.size(); i++) {
     342        models.at(i)->pimpl_->Suspend();
     343      }
     344    }
     345    if (receiver->IsKeyDown(KEY_KEY_S) && dbtFile_r != NULL) {
     346      // rt_printf("stop play\n");
     347      close_hdfile(dbtFile_r);
     348      dbtFile_r = NULL;
     349      free(dbtbuf);
     350      // on resume les models
     351      for (size_t i = 0; i < models.size(); i++) {
     352        models.at(i)->pimpl_->Resume();
     353      }
     354    }
     355  }
     356
     357  receiver->SetCamera(NULL);
     358  receiver->SetModel(NULL);
     359}
     360
     361void Gui_impl::setWindowCaption(Object *object, int fps) {
     362  std::ostringstream text;
     363  text << "Cam: " << object->ObjectName().c_str()
     364       << ", Kbd: " << object->ObjectName().c_str() << ", FPS: " << fps;
     365
     366  device->setWindowCaption(stringw(text.str().c_str()).c_str());
     367}
     368
     369void Gui_impl::takeScreenshot(void) {
     370  static int cpt = 0;
     371  // get image from the last rendered frame
     372  IImage *const image = driver->createScreenShot();
     373  if (image) // should always be true, but you never know. ;)
     374  {
     375    // construct a filename, consisting of local time and file extension
     376    c8 filename[64];
     377    // snprintf(filename, 64, "screenshot_%u.png",
     378    // device->getTimer()->getRealTime());
     379    snprintf(filename, 64, "screenshot_%u.png", cpt);
     380    cpt++;
     381    // write screenshot to file
     382    if (!driver->writeImageToFile(image, filename))
     383      device->getLogger()->log(L"Failed to take screenshot.", ELL_WARNING);
     384
     385    // Don't forget to drop image since we don't need it anymore.
     386    image->drop();
     387  }
     388}
     389
     390size_t Gui_impl::dbtSize(std::vector<Model *> models) {
     391  size_t size = 0;
     392  for (size_t i = 0; i < models.size(); i++) {
     393    size += models.at(i)->dbtSize();
     394  }
     395
     396  return size;
     397}
     398#endif // GL

  • trunk/lib/FlairSimulator/src/Man.cpp

    r10 r15  
    3434using namespace flair::gui;
    3535
    36 namespace flair
    37 {
    38 namespace simulator
    39 {
     36namespace flair {
     37namespace simulator {
    4038
    41 Man::Man(const Simulator* parent,std::string name): Model(parent,name)
    42 {
    43     node = getGui()->getSceneManager()->addAnimatedMeshSceneNode(getGui()->getMesh("ninja.b3d"),getSceneNode(),-1,
    44                                                                  vector3df(0,0,0),vector3df(90,0,90),vector3df(10,10,10));
     39Man::Man(const Simulator *parent, std::string name) : Model(parent, name) {
     40  node = getGui()->getSceneManager()->addAnimatedMeshSceneNode(
     41      getGui()->getMesh("ninja.b3d"), getSceneNode(), -1, vector3df(0, 0, 0),
     42      vector3df(90, 0, 90), vector3df(10, 10, 10));
    4543
    46         node->setFrameLoop(0, 13);
    47         node->setAnimationSpeed(0);
    48         node->getMaterial(0).NormalizeNormals = true;
    49         node->getMaterial(0).Lighting = false;
     44  node->setFrameLoop(0, 13);
     45  node->setAnimationSpeed(0);
     46  node->getMaterial(0).NormalizeNormals = true;
     47  node->getMaterial(0).Lighting = false;
    5048
    51         getCamera()->setPositionOffset(vector3df(0,0,100));
    52     getCamera()->setTargetOffset(vector3df(0,0,100));
     49  getCamera()->setPositionOffset(vector3df(0, 0, 100));
     50  getCamera()->setTargetOffset(vector3df(0, 0, 100));
    5351
    54     setTriangleSelector(getGui()->getSceneManager()->createTriangleSelector(node));
    55     Box()->addInternalBox(node->getTransformedBoundingBox());
     52  setTriangleSelector(
     53      getGui()->getSceneManager()->createTriangleSelector(node));
     54  Box()->addInternalBox(node->getTransformedBoundingBox());
    5655
    57     Tab *setup_tab=new Tab(GetTabWidget(),"model");
    58         t_speed=new DoubleSpinBox(setup_tab->NewRow(),"translational speed (m/s):",0,5,0.1);
    59         r_speed=new DoubleSpinBox(setup_tab->NewRow(),"rotational speed (deg/s):",0,180,10);
     56  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     57  t_speed = new DoubleSpinBox(setup_tab->NewRow(), "translational speed (m/s):",
     58                              0, 5, 0.1);
     59  r_speed = new DoubleSpinBox(setup_tab->NewRow(), "rotational speed (deg/s):",
     60                              0, 180, 10);
    6061}
    6162
    62 Man::~Man()
    63 {
     63Man::~Man() {}
    6464
     65void Man::CalcModel(void) {
     66  // compute quaternion from W
     67  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     68  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     69
     70  // Quaternion integration
     71  state[0].Quat = state[-1].Quat + dQ * dT();
     72  state[0].Quat.Normalize();
     73
     74  Vector3D dir = state[0].Vel;
     75  dir.Rotate(state[0].Quat);
     76  state[0].Pos = state[-1].Pos + dT() * dir;
    6577}
    6678
    67 void Man::CalcModel(void)
    68 {
    69     // compute quaternion from W
    70     // Quaternion derivative: dQ = 0.5*(Q*Qw)
    71     Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
     79bool Man::OnEvent(const SEvent &event) {
     80  if (event.EventType != EET_KEY_INPUT_EVENT)
     81    return false;
    7282
    73     // Quaternion integration
    74     state[0].Quat = state[-1].Quat +dQ*dT();
    75     state[0].Quat.Normalize();
     83  if (event.KeyInput.PressedDown == false) {
     84    state[0].Vel.x = 0;
     85    state[0].W.z = 0;
     86    node->setAnimationSpeed(0);
     87  } else {
     88    switch (event.KeyInput.Key) {
     89    case KEY_UP:
     90      state[0].Vel.x = t_speed->Value();
     91      node->setAnimationSpeed(t_speed->Value() * 10.f);
     92      break;
     93    case KEY_DOWN:
     94      state[0].Vel.x = -t_speed->Value();
     95      node->setAnimationSpeed(-t_speed->Value() * 10.f);
     96      break;
     97    case KEY_LEFT:
     98      state[0].W.z = -Euler::ToRadian(r_speed->Value());
     99      node->setAnimationSpeed(r_speed->Value() * .15f);
     100      break;
     101    case KEY_RIGHT:
     102      state[0].W.z = Euler::ToRadian(r_speed->Value());
     103      node->setAnimationSpeed(-r_speed->Value() * .15f);
     104      break;
     105    default:
     106      return false;
     107      break;
     108    }
     109  }
    76110
    77     Vector3D dir=state[0].Vel;
    78     dir.Rotate(state[0].Quat);
    79     state[0].Pos=state[-1].Pos+dT()*dir;
     111  return true;
    80112}
    81113
    82 bool Man::OnEvent(const SEvent& event)
    83 {
    84     if (event.EventType != EET_KEY_INPUT_EVENT)
    85         return false;
     114size_t Man::dbtSize(void) const { return 6 * sizeof(float); }
    86115
    87     if(event.KeyInput.PressedDown==false)
    88     {
    89         state[0].Vel.x=0;
    90         state[0].W.z=0;
    91         node->setAnimationSpeed(0);
    92     }
    93     else
    94     {
    95         switch(event.KeyInput.Key)
    96         {
    97         case KEY_UP:
    98             state[0].Vel.x=t_speed->Value();
    99             node->setAnimationSpeed(t_speed->Value()*10.f);
    100             break;
    101         case KEY_DOWN:
    102             state[0].Vel.x=-t_speed->Value();
    103             node->setAnimationSpeed(-t_speed->Value()*10.f);
    104             break;
    105         case KEY_LEFT:
    106             state[0].W.z=-Euler::ToRadian(r_speed->Value());
    107             node->setAnimationSpeed(r_speed->Value()*.15f);
    108             break;
    109         case KEY_RIGHT:
    110             state[0].W.z=Euler::ToRadian(r_speed->Value());
    111             node->setAnimationSpeed(-r_speed->Value()*.15f);
    112             break;
    113         default:
    114             return false;
    115             break;
    116         }
    117     }
    118 
    119     return true;
     116void Man::WritedbtBuf(char *dbtbuf) { /*
     117                                         float *buf=(float*)dbtbuf;
     118                                         vector3df vect=node->getPosition();
     119                                         memcpy(buf,&vect.X,sizeof(float));
     120                                         buf++;
     121                                         memcpy(buf,&vect.Y,sizeof(float));
     122                                         buf++;
     123                                         memcpy(buf,&vect.Z,sizeof(float));
     124                                         buf++;
     125                                         vect=node->getRotation();
     126                                         memcpy(buf,&vect.X,sizeof(float));
     127                                         buf++;
     128                                         memcpy(buf,&vect.Y,sizeof(float));
     129                                         buf++;
     130                                         memcpy(buf,&vect.Z,sizeof(float));
     131                                         buf++;*/
    120132}
    121133
    122 size_t Man::dbtSize(void) const
    123 {
    124     return 6*sizeof(float);
    125 }
    126 
    127 void Man::WritedbtBuf(char* dbtbuf)
    128 {/*
    129     float *buf=(float*)dbtbuf;
    130     vector3df vect=node->getPosition();
    131     memcpy(buf,&vect.X,sizeof(float));
    132     buf++;
    133     memcpy(buf,&vect.Y,sizeof(float));
    134     buf++;
    135     memcpy(buf,&vect.Z,sizeof(float));
    136     buf++;
    137     vect=node->getRotation();
    138     memcpy(buf,&vect.X,sizeof(float));
    139     buf++;
    140     memcpy(buf,&vect.Y,sizeof(float));
    141     buf++;
    142     memcpy(buf,&vect.Z,sizeof(float));
    143     buf++;*/
    144 }
    145 
    146 void Man::ReaddbtBuf(char* dbtbuf)
    147 {/*
    148     float *buf=(float*)dbtbuf;
    149     vector3df vect;
    150     memcpy(&vect.X,buf,sizeof(float));
    151     buf++;
    152     memcpy(&vect.Y,buf,sizeof(float));
    153     buf++;
    154     memcpy(&vect.Z,buf,sizeof(float));
    155     buf++;
    156     node->setPosition(vect);
    157     memcpy(&vect.X,buf,sizeof(float));
    158     buf++;
    159     memcpy(&vect.Y,buf,sizeof(float));
    160     buf++;
    161     memcpy(&vect.Z,buf,sizeof(float));
    162     buf++;
    163     node->setRotation(vect);
    164     node->setAnimationSpeed(2.f);*/
     134void Man::ReaddbtBuf(char *dbtbuf) { /*
     135                                        float *buf=(float*)dbtbuf;
     136                                        vector3df vect;
     137                                        memcpy(&vect.X,buf,sizeof(float));
     138                                        buf++;
     139                                        memcpy(&vect.Y,buf,sizeof(float));
     140                                        buf++;
     141                                        memcpy(&vect.Z,buf,sizeof(float));
     142                                        buf++;
     143                                        node->setPosition(vect);
     144                                        memcpy(&vect.X,buf,sizeof(float));
     145                                        buf++;
     146                                        memcpy(&vect.Y,buf,sizeof(float));
     147                                        buf++;
     148                                        memcpy(&vect.Z,buf,sizeof(float));
     149                                        buf++;
     150                                        node->setRotation(vect);
     151                                        node->setAnimationSpeed(2.f);*/
    165152}
    166153
    167154} // end namespace simulator
    168155} // end namespace flair
    169 #endif //GL
     156#endif // GL

  • trunk/lib/FlairSimulator/src/Man.h

    r10 r15  
    2020#include <Model.h>
    2121
    22 namespace irr
    23 {
    24     namespace scene
    25     {
    26         class IAnimatedMeshSceneNode;
    27     }
     22namespace irr {
     23namespace scene {
     24class IAnimatedMeshSceneNode;
     25}
    2826}
    2927
    30 namespace flair
    31 {
    32     namespace gui
    33     {
    34         class DoubleSpinBox;
    35     }
     28namespace flair {
     29namespace gui {
     30class DoubleSpinBox;
     31}
    3632}
    3733
    38 namespace flair
    39 {
    40 namespace simulator
    41 {
    42     class Simulator;
     34namespace flair {
     35namespace simulator {
     36class Simulator;
    4337
    44     class Man: private Model
    45     {
    46         public:
    47             Man(const Simulator* parent,std::string name);
    48             ~Man();
     38class Man : private Model {
     39public:
     40  Man(const Simulator *parent, std::string name);
     41  ~Man();
    4942
    50         private:
    51             size_t dbtSize(void) const;
    52             void WritedbtBuf(char* dbtbuf);
    53             void ReaddbtBuf(char* dbtbuf);
    54             void CalcModel(void);
    55             void AnimateModel(void){};
    56             bool OnEvent(const irr::SEvent& event);
     43private:
     44  size_t dbtSize(void) const;
     45  void WritedbtBuf(char *dbtbuf);
     46  void ReaddbtBuf(char *dbtbuf);
     47  void CalcModel(void);
     48  void AnimateModel(void){};
     49  bool OnEvent(const irr::SEvent &event);
    5750
    58             irr::scene::IAnimatedMeshSceneNode* node;
    59             gui::DoubleSpinBox *t_speed,*r_speed;
    60     };
     51  irr::scene::IAnimatedMeshSceneNode *node;
     52  gui::DoubleSpinBox *t_speed, *r_speed;
     53};
    6154} // end namespace simulator
    6255} // end namespace flair

  • trunk/lib/FlairSimulator/src/MeshSceneNode.cpp

    r10 r15  
    2828using namespace flair::core;
    2929
    30 namespace flair
    31 {
    32 namespace simulator
    33 {
     30namespace flair {
     31namespace simulator {
    3432
    35 MeshSceneNode::MeshSceneNode(Model* parent,IMesh* mesh,
    36                         const vector3df& position ,const vector3df& rotation,
    37                         ITexture* texture, s32 id ): IMeshSceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,position,rotation)
    38 {
    39     Material.Wireframe = false;
    40     Material.Lighting = false;
     33MeshSceneNode::MeshSceneNode(Model *parent, IMesh *mesh,
     34                             const vector3df &position,
     35                             const vector3df &rotation, ITexture *texture,
     36                             s32 id)
     37    : IMeshSceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,
     38                     position, rotation) {
     39  Material.Wireframe = false;
     40  Material.Lighting = false;
    4141
    42     setMesh(mesh);
     42  setMesh(mesh);
    4343
    44     if(texture!=NULL)
    45     {
    46         setMaterialTexture(0,texture);
    47     }
     44  if (texture != NULL) {
     45    setMaterialTexture(0, texture);
     46  }
    4847
    49     parent->Box()->addInternalBox(getTransformedBoundingBox());
     48  parent->Box()->addInternalBox(getTransformedBoundingBox());
    5049}
    5150
     51void MeshSceneNode::OnRegisterSceneNode(void) {
     52  if (IsVisible)
     53    SceneManager->registerNodeForRendering(this);
    5254
    53 void MeshSceneNode::OnRegisterSceneNode(void)
    54 {
    55     if (IsVisible)
    56         SceneManager->registerNodeForRendering(this);
    57 
    58     ISceneNode::OnRegisterSceneNode();
     55  ISceneNode::OnRegisterSceneNode();
    5956}
    6057
    61 void MeshSceneNode::render(void)
    62 {
    63     IVideoDriver* driver = SceneManager->getVideoDriver();
     58void MeshSceneNode::render(void) {
     59  IVideoDriver *driver = SceneManager->getVideoDriver();
    6460
    65     driver->setMaterial(Material);
    66     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    67     driver->drawMeshBuffer(mesh->getMeshBuffer(0));
     61  driver->setMaterial(Material);
     62  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     63  driver->drawMeshBuffer(mesh->getMeshBuffer(0));
    6864}
    6965
     66SMaterial &MeshSceneNode::getMaterial(u32 i) { return Material; }
    7067
    71 SMaterial& MeshSceneNode::getMaterial(u32 i)
    72 {
    73     return Material;
     68void MeshSceneNode::setMesh(IMesh *ptr) {
     69  mesh = ptr;
     70  Box = mesh->getBoundingBox();
    7471}
    7572
    76 void MeshSceneNode::setMesh(IMesh* ptr)
    77 {
    78     mesh=ptr;
    79     Box=mesh->getBoundingBox();
    80 }
     73IMesh *MeshSceneNode::getMesh(void) { return mesh; }
    8174
    82 
    83 IMesh* MeshSceneNode::getMesh(void)
    84 {
    85     return mesh;
    86 }
    87 
    88 
    89 void MeshSceneNode::setReadOnlyMaterials(bool readonly)
    90 {
    91 
    92 }
     75void MeshSceneNode::setReadOnlyMaterials(bool readonly) {}
    9376
    9477} // end namespace simulator
    9578} // end namespace flair
    96 #endif //GL
     79#endif // GL

  • trunk/lib/FlairSimulator/src/MeshSceneNode.h

    r10 r15  
    2020#include <IMeshSceneNode.h>
    2121
    22 namespace flair
    23 {
    24 namespace simulator
    25 {
    26     class Model;
     22namespace flair {
     23namespace simulator {
     24class Model;
    2725
    28     class MeshSceneNode : public irr::scene::IMeshSceneNode
    29     {
    30         public:
    31             MeshSceneNode(Model* parent,irr::scene::IMesh* mesh,
    32                 const irr::core::vector3df& position = irr::core::vector3df(0,0,0),
    33                 const irr::core::vector3df& rotation = irr::core::vector3df(0,0,0),
    34                 irr::video::ITexture* texture=NULL, irr::s32 id=-1);
     26class MeshSceneNode : public irr::scene::IMeshSceneNode {
     27public:
     28  MeshSceneNode(
     29      Model *parent, irr::scene::IMesh *mesh,
     30      const irr::core::vector3df &position = irr::core::vector3df(0, 0, 0),
     31      const irr::core::vector3df &rotation = irr::core::vector3df(0, 0, 0),
     32      irr::video::ITexture *texture = NULL, irr::s32 id = -1);
    3533
    36             virtual void OnRegisterSceneNode(void);
    37             virtual void render(void);
    38             virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox() const
    39             {
    40                 return Box;
    41             }
    42             virtual irr::u32 getMaterialCount(void) const
    43             {
    44                 return 1;
    45             }
    46             virtual irr::video::SMaterial& getMaterial(irr::u32 i);
    47             virtual void setMesh( irr::scene::IMesh* ptr);
    48             virtual  irr::scene::IMesh* getMesh(void);
    49             virtual void setReadOnlyMaterials(bool readonly);
    50             virtual bool isReadOnlyMaterials(void) const
    51             {
    52                 return false;
    53             }
    54             virtual irr::scene::IShadowVolumeSceneNode* addShadowVolumeSceneNode(const irr::scene::IMesh* shadowMesh=0,
    55             irr::s32 id=-1, bool zfailmethod=true, irr::f32 infinity=1000.0f)
    56             {
    57                 return NULL;
    58             }
     34  virtual void OnRegisterSceneNode(void);
     35  virtual void render(void);
     36  virtual const irr::core::aabbox3d<irr::f32> &getBoundingBox() const {
     37    return Box;
     38  }
     39  virtual irr::u32 getMaterialCount(void) const { return 1; }
     40  virtual irr::video::SMaterial &getMaterial(irr::u32 i);
     41  virtual void setMesh(irr::scene::IMesh *ptr);
     42  virtual irr::scene::IMesh *getMesh(void);
     43  virtual void setReadOnlyMaterials(bool readonly);
     44  virtual bool isReadOnlyMaterials(void) const { return false; }
     45  virtual irr::scene::IShadowVolumeSceneNode *
     46  addShadowVolumeSceneNode(const irr::scene::IMesh *shadowMesh = 0,
     47                           irr::s32 id = -1, bool zfailmethod = true,
     48                           irr::f32 infinity = 1000.0f) {
     49    return NULL;
     50  }
    5951
    60         private:
    61             irr::scene::IMesh *mesh;
    62             irr::core::aabbox3d<irr::f32> Box;
    63             irr::video::SMaterial Material;
    64     };
     52private:
     53  irr::scene::IMesh *mesh;
     54  irr::core::aabbox3d<irr::f32> Box;
     55  irr::video::SMaterial Material;
     56};
    6557} // end namespace simulator
    6658} // end namespace flair

  • trunk/lib/FlairSimulator/src/Model.cpp

    r10 r15  
    3232using namespace flair::gui;
    3333
    34 namespace flair
    35 {
    36 namespace simulator
    37 {
     34namespace flair {
     35namespace simulator {
    3836
    39 Model::Model(const Simulator* parent,std::string name) : IODevice(parent,name)
    40 {
     37Model::Model(const Simulator *parent, std::string name)
     38    : IODevice(parent, name) {
    4139#ifdef GL
    42     pimpl_=new Model_impl(this,name,getGui()->getSceneManager(),parent->pimpl_);
     40  pimpl_ =
     41      new Model_impl(this, name, getGui()->getSceneManager(), parent->pimpl_);
    4342#else
    44     pimpl_=new Model_impl(this,name,parent->pimpl_);
     43  pimpl_ = new Model_impl(this, name, parent->pimpl_);
    4544#endif
    46     parent->pimpl_->models.push_back(this);
     45  parent->pimpl_->models.push_back(this);
    4746}
    4847
    49 Model::~Model()
    50 {
    51     delete pimpl_;
    52 }
     48Model::~Model() { delete pimpl_; }
    5349#ifdef GL
    54 ISceneNode* Model::getSceneNode() const
    55 {
    56     return pimpl_;
    57 }
     50ISceneNode *Model::getSceneNode() const { return pimpl_; }
    5851/*
    5952only used by dbt, to rewrite using conversion functions (irrlicht <-> simulator)
    … …  
    6457}
    6558*/
    66 void Model::setScale(float value)
    67 {
    68     pimpl_->setScale(vector3df(value,value,value));
     59void Model::setScale(float value) {
     60  pimpl_->setScale(vector3df(value, value, value));
    6961}
    7062
    71 aabbox3d<f32> *Model::Box() const
    72 {
    73     return &(pimpl_->box);
     63aabbox3d<f32> *Model::Box() const { return &(pimpl_->box); }
     64
     65AnimPoursuite *Model::getCamera(void) const { return pimpl_->animator; }
     66
     67void Model::setTriangleSelector(ITriangleSelector *selector) {
     68
     69  pimpl_->selector = selector;
     70  pimpl_->setTriangleSelector(selector);
    7471}
    7572
    76 AnimPoursuite* Model::getCamera(void) const
    77 {
    78     return pimpl_->animator;
    79 }
    80 
    81 void Model::setTriangleSelector(ITriangleSelector* selector) {
    82 
    83     pimpl_->selector=selector;
    84     pimpl_->setTriangleSelector(selector);
    85 }
    86 
    87 void Model::setCameraFarValue(float zf) {
    88     pimpl_->camera->setFarValue(zf);
    89 }
     73void Model::setCameraFarValue(float zf) { pimpl_->camera->setFarValue(zf); }
    9074#endif
    9175
    92 TabWidget* Model::GetTabWidget(void) const
    93 {
    94     return pimpl_->tabwidget;
    95 }
     76TabWidget *Model::GetTabWidget(void) const { return pimpl_->tabwidget; }
    9677
    97 float Model::dT(void) const
    98 {
    99     return pimpl_->dT->Value();
    100 }
     78float Model::dT(void) const { return pimpl_->dT->Value(); }
    10179
    10280} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Model.h

    r10 r15  
    2525#ifdef GL
    2626#include <aabbox3d.h>
    27 namespace irr
    28 {
    29     class SEvent;
    30     namespace scene
    31     {
    32         class ISceneManager;
    33         class ISceneNode;
    34         class ITriangleSelector;
    35     }
     27namespace irr {
     28class SEvent;
     29namespace scene {
     30class ISceneManager;
     31class ISceneNode;
     32class ITriangleSelector;
     33}
    3634}
    3735#endif
    3836
    39 namespace flair
    40 {
    41     namespace gui
    42     {
    43         class TabWidget;
    44     }
    45     namespace sensor
    46     {
    47         class SensorGL;
    48     }
     37namespace flair {
     38namespace gui {
     39class TabWidget;
     40}
     41namespace sensor {
     42class SensorGL;
     43}
    4944}
    5045
    … …  
    5348class Model_impl;
    5449
    55 namespace flair
    56 {
    57 namespace simulator
    58 {
    59     class Simulator;
    60     class AnimPoursuite;
     50namespace flair {
     51namespace simulator {
     52class Simulator;
     53class AnimPoursuite;
    6154
    62     class Model : public core::IODevice
    63     {
    64         friend class ::Gui_impl;
    65         friend class ::Simulator_impl;
    66         friend class ::Model_impl;
    67         friend class AnimPoursuite;
    68         friend class sensor::SensorGL;
     55class Model : public core::IODevice {
     56  friend class ::Gui_impl;
     57  friend class ::Simulator_impl;
     58  friend class ::Model_impl;
     59  friend class AnimPoursuite;
     60  friend class sensor::SensorGL;
    6961
    70         public:
    71             Model(const Simulator* parent,std::string name);
    72             virtual ~Model();
     62public:
     63  Model(const Simulator *parent, std::string name);
     64  virtual ~Model();
    7365
     66  typedef struct simu_state {
     67    core::Quaternion Quat;
     68    core::Vector3D W;
     69    core::Vector3D Pos;
     70    core::Vector3D Vel;
     71  } simu_state_t;
    7472
    75             typedef struct simu_state {
    76                 core::Quaternion Quat;
    77                 core::Vector3D W;
    78                 core::Vector3D Pos;
    79                 core::Vector3D Vel;
    80             } simu_state_t;
     73#ifdef GL
     74  irr::scene::ISceneNode *getSceneNode() const;
     75  irr::core::aabbox3d<irr::f32> *Box() const;
    8176
    82     #ifdef GL
    83             irr::scene::ISceneNode* getSceneNode() const;
    84             irr::core::aabbox3d<irr::f32>* Box() const;
     77  virtual size_t dbtSize(void) const = 0;
     78  virtual void Draw(void){};
     79  virtual void ExtraDraw(void){};
     80  virtual void WritedbtBuf(char *dbtbuf) = 0;
     81  virtual void ReaddbtBuf(char *dbtbuf) = 0;
     82  virtual bool OnEvent(const irr::SEvent &event) = 0;
    8583
    86             virtual size_t dbtSize(void) const =0;
    87             virtual void Draw(void){};
    88             virtual void ExtraDraw(void){};
    89             virtual void WritedbtBuf(char* dbtbuf)=0;
    90             virtual void ReaddbtBuf(char* dbtbuf)=0;
    91             virtual bool OnEvent(const irr::SEvent& event)=0;
     84  //! Sets the value of the camera's far clipping plane (default: 2000.0f)
     85  /** \param zf: New z far value. */
     86  void setCameraFarValue(float zf);
     87#endif
     88  gui::TabWidget *GetTabWidget(void) const;
    9289
    93             //! Sets the value of the camera's far clipping plane (default: 2000.0f)
    94             /** \param zf: New z far value. */
    95             void setCameraFarValue(float zf);
    96     #endif
    97             gui::TabWidget* GetTabWidget(void) const;
     90protected:
     91  DiscreteTimeVariable<simu_state_t, 3> state;
     92  float dT(void) const;
     93  virtual void CalcModel(void) = 0;
     94#ifdef GL
     95  AnimPoursuite *getCamera(void) const;
     96  virtual void AnimateModel(void) = 0;
     97  // void setPosition(core::Vector3D pos);
     98  void setScale(float value);
     99  void setTriangleSelector(irr::scene::ITriangleSelector *selector);
     100#endif
    98101
    99         protected:
    100             DiscreteTimeVariable<simu_state_t,3> state;
    101             float dT(void) const;
    102             virtual void CalcModel(void)=0;
    103     #ifdef GL
    104             AnimPoursuite* getCamera(void) const;
    105             virtual void AnimateModel(void)=0;
    106             //void setPosition(core::Vector3D pos);
    107             void setScale(float value);
    108             void setTriangleSelector(irr::scene::ITriangleSelector* selector);
    109     #endif
    110 
    111         private:
    112             void UpdateFrom(const core::io_data *data){};
    113             class Model_impl* pimpl_;
    114     };
     102private:
     103  void UpdateFrom(const core::io_data *data){};
     104  class Model_impl *pimpl_;
     105};
    115106} // end namespace simulator
    116107} // end namespace flair

  • trunk/lib/FlairSimulator/src/Model_impl.cpp

    r10 r15  
    5050
    5151#ifdef GL
    52 Model_impl::Model_impl(Model* self,std::string name,ISceneManager* scenemanager,vrpn_Connection_IP* vrpn)
    53     :  ISceneNode(scenemanager->getRootSceneNode(), scenemanager, -1),Thread(self,name,50),vrpn_Tracker( name.c_str(), vrpn )
     52Model_impl::Model_impl(Model *self, std::string name,
     53                       ISceneManager *scenemanager, vrpn_Connection_IP *vrpn)
     54    : ISceneNode(scenemanager->getRootSceneNode(), scenemanager, -1),
     55      Thread(self, name, 50), vrpn_Tracker(name.c_str(), vrpn)
    5456
    5557#else
    56 Model_impl::Model_impl(Model* self,std::string name,vrpn_Connection_IP* vrpn)
    57     : Thread(self,name,50),vrpn_Tracker( name.c_str(), vrpn )
     58Model_impl::Model_impl(Model *self, std::string name, vrpn_Connection_IP *vrpn)
     59    : Thread(self, name, 50), vrpn_Tracker(name.c_str(), vrpn)
    5860#endif
    5961{
    60     this->self=self;
    61 
    62 #ifdef GL
    63     //for sync with gui
    64     cond=new ConditionVariable(this,name);
    65     sync_count=0;
    66 
    67     //collisions
    68     collision_mutex=new Mutex(this);
    69     collision_occured=false;
    70 
    71     //selector for collisions
    72     selector = getSceneManager()->createTriangleSelectorFromBoundingBox(this);
    73     setTriangleSelector(selector);
    74     meta_selector = getSceneManager()->createMetaTriangleSelector();
    75 
    76     anim = getSceneManager()->createCollisionResponseAnimator(meta_selector, this,vector3df(1,1,1),vector3df(0,0,0), vector3df(0,0,0));
    77     addAnimator(anim);
    78 
    79     //camera
    80         camera =getSceneManager()->addCameraSceneNode();
    81     camera->setAspectRatio(getGui()->getAspectRatio());//on force a cause du view port
    82     camera->setUpVector(vector3df(0,0,1));
    83 
    84     animator=new AnimPoursuite(this);
    85     camera->addAnimator(animator);
    86     camera->setFarValue(8000);
    87 
    88     position_init=false;
    89 #endif
    90 
    91     //init user interface
    92     Tab* tab=new Tab(getSimulator()->GetTabWidget(),ObjectName());
    93     tabwidget=new TabWidget(tab->NewRow(),"tabs");
    94     Tab* sampl=new Tab(tabwidget,"sampling");
    95     dT=new DoubleSpinBox(sampl->NewRow(),"Tech (s):",0.001,1,0.001,3);
    96     Tab* layout=new Tab(tabwidget,"optitrack");
    97     enable_opti=new CheckBox(layout->NewRow(),"enabled");
    98     Tab* init=new Tab(tabwidget,"init");
    99     pos_init=new Vector3DSpinBox(init->NewRow(),"position",-50,50,1);
    100     yaw_init=new SpinBox(init->NewRow(),"yaw (deg):",-180,180,10);
    101 
    102     //modele
    103     states_mutex=new Mutex(this);
    104         self->state[0].Pos=pos_init->Value();
    105         self->state[0].Vel.x=0;
    106     self->state[0].Vel.y=0;
    107     self->state[0].Vel.z=0;
    108         self->state[0].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
    109     self->state[0].W.x=0;
    110     self->state[0].W.y=0;
    111     self->state[0].W.z=0;
    112 
    113     self->state[-1]=self->state[0];
    114     self->state[-2]=self->state[0];
    115 
    116     cvmatrix_descriptor* desc=new cvmatrix_descriptor(13,1);
    117         desc->SetElementName(0,0,"q0");
    118         desc->SetElementName(1,0,"q1");
    119         desc->SetElementName(2,0,"q2");
    120         desc->SetElementName(3,0,"q3");
    121         desc->SetElementName(4,0,"x");
    122         desc->SetElementName(5,0,"y");
    123         desc->SetElementName(6,0,"z");
    124         desc->SetElementName(7,0,"wx");
    125         desc->SetElementName(8,0,"wy");
    126         desc->SetElementName(9,0,"wz");
    127         desc->SetElementName(10,0,"vx");
    128         desc->SetElementName(11,0,"vy");
    129         desc->SetElementName(12,0,"vz");
    130     output=new cvmatrix(this,desc,floatType,"state");
    131 
    132     self->AddDataToLog(output);
    133 }
    134 
    135 Model_impl::~Model_impl()
    136 {
    137     SafeStop();
    138     Join();
    139 #ifdef GL
    140     remove();//remove ISceneNode
     62  this->self = self;
     63
     64#ifdef GL
     65  // for sync with gui
     66  cond = new ConditionVariable(this, name);
     67  sync_count = 0;
     68
     69  // collisions
     70  collision_mutex = new Mutex(this);
     71  collision_occured = false;
     72
     73  // selector for collisions
     74  selector = getSceneManager()->createTriangleSelectorFromBoundingBox(this);
     75  setTriangleSelector(selector);
     76  meta_selector = getSceneManager()->createMetaTriangleSelector();
     77
     78  anim = getSceneManager()->createCollisionResponseAnimator(
     79      meta_selector, this, vector3df(1, 1, 1), vector3df(0, 0, 0),
     80      vector3df(0, 0, 0));
     81  addAnimator(anim);
     82
     83  // camera
     84  camera = getSceneManager()->addCameraSceneNode();
     85  camera->setAspectRatio(
     86      getGui()->getAspectRatio()); // on force a cause du view port
     87  camera->setUpVector(vector3df(0, 0, 1));
     88
     89  animator = new AnimPoursuite(this);
     90  camera->addAnimator(animator);
     91  camera->setFarValue(8000);
     92
     93  position_init = false;
     94#endif
     95
     96  // init user interface
     97  Tab *tab = new Tab(getSimulator()->GetTabWidget(), ObjectName());
     98  tabwidget = new TabWidget(tab->NewRow(), "tabs");
     99  Tab *sampl = new Tab(tabwidget, "sampling");
     100  dT = new DoubleSpinBox(sampl->NewRow(), "Tech (s):", 0.001, 1, 0.001, 3);
     101  Tab *layout = new Tab(tabwidget, "optitrack");
     102  enable_opti = new CheckBox(layout->NewRow(), "enabled");
     103  Tab *init = new Tab(tabwidget, "init");
     104  pos_init = new Vector3DSpinBox(init->NewRow(), "position", -50, 50, 1);
     105  yaw_init = new SpinBox(init->NewRow(), "yaw (deg):", -180, 180, 10);
     106
     107  // modele
     108  states_mutex = new Mutex(this);
     109  self->state[0].Pos = pos_init->Value();
     110  self->state[0].Vel.x = 0;
     111  self->state[0].Vel.y = 0;
     112  self->state[0].Vel.z = 0;
     113  self->state[0].Quat = ComputeInitRotation(Quaternion(1, 0, 0, 0));
     114  self->state[0].W.x = 0;
     115  self->state[0].W.y = 0;
     116  self->state[0].W.z = 0;
     117
     118  self->state[-1] = self->state[0];
     119  self->state[-2] = self->state[0];
     120
     121  cvmatrix_descriptor *desc = new cvmatrix_descriptor(13, 1);
     122  desc->SetElementName(0, 0, "q0");
     123  desc->SetElementName(1, 0, "q1");
     124  desc->SetElementName(2, 0, "q2");
     125  desc->SetElementName(3, 0, "q3");
     126  desc->SetElementName(4, 0, "x");
     127  desc->SetElementName(5, 0, "y");
     128  desc->SetElementName(6, 0, "z");
     129  desc->SetElementName(7, 0, "wx");
     130  desc->SetElementName(8, 0, "wy");
     131  desc->SetElementName(9, 0, "wz");
     132  desc->SetElementName(10, 0, "vx");
     133  desc->SetElementName(11, 0, "vy");
     134  desc->SetElementName(12, 0, "vz");
     135  output = new cvmatrix(this, desc, floatType, "state");
     136
     137  self->AddDataToLog(output);
     138}
     139
     140Model_impl::~Model_impl() {
     141  SafeStop();
     142  Join();
     143#ifdef GL
     144  remove(); // remove ISceneNode
    141145#endif
    142146}
    143147
    144148Quaternion Model_impl::ComputeInitRotation(Quaternion quat_in) {
    145     Quaternion yaw_rot_quat;
    146     Euler yaw_rot_euler(0,0,Euler::ToRadian(yaw_init->Value()));
    147     yaw_rot_euler.ToQuaternion(yaw_rot_quat);
    148     return yaw_rot_quat*quat_in;
    149 }
    150 
    151 void Model_impl::mainloop(void)
    152 {
    153     if(enable_opti->Value()==false) return;
    154     vrpn_gettimeofday(&_timestamp, NULL);
    155     vrpn_Tracker::timestamp = _timestamp;
    156 
    157     //change to vrpn reference
     149  Quaternion yaw_rot_quat;
     150  Euler yaw_rot_euler(0, 0, Euler::ToRadian(yaw_init->Value()));
     151  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
     152  return yaw_rot_quat * quat_in;
     153}
     154
     155void Model_impl::mainloop(void) {
     156  if (enable_opti->Value() == false)
     157    return;
     158  vrpn_gettimeofday(&_timestamp, NULL);
     159  vrpn_Tracker::timestamp = _timestamp;
     160
     161  // change to vrpn reference
     162  states_mutex->GetMutex();
     163  Quaternion quat = getSimulator()->ToVRPNReference(self->state[0].Quat);
     164  Vector3D position = getSimulator()->ToVRPNReference(self->state[0].Pos);
     165  states_mutex->ReleaseMutex();
     166
     167  pos[0] = position.x;
     168  pos[1] = position.y;
     169  pos[2] = position.z;
     170  // warning: d_quat is defined as (qx,qy,qz,qw), which is different from
     171  // flair::core::Quaternion
     172  d_quat[0] = quat.q1;
     173  d_quat[1] = quat.q2;
     174  d_quat[2] = quat.q3;
     175  d_quat[3] = quat.q0;
     176
     177  char msgbuf[1000];
     178
     179  d_sensor = 0;
     180
     181  int len = vrpn_Tracker::encode_to(msgbuf);
     182
     183  if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id,
     184                                 msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
     185    fprintf(stderr, "can't write message: tossing\n");
     186  }
     187
     188  server_mainloop();
     189}
     190
     191#ifdef GL
     192ITriangleSelector *Model_impl::TriangleSelector(void) { return selector; }
     193
     194IMetaTriangleSelector *Model_impl::MetaTriangleSelector(void) {
     195  return meta_selector;
     196}
     197
     198void Model_impl::UpdatePos(void) {
     199  vector3df nodePosition;
     200  Quaternion nodeOrientation;
     201  Euler euler;
     202
     203  states_mutex->GetMutex();
     204  nodePosition = ToIrrlichtCoordinates(self->state[0].Pos);
     205  nodeOrientation = ToIrrlichtOrientation(self->state[0].Quat);
     206  states_mutex->ReleaseMutex();
     207
     208  setPosition(nodePosition);
     209
     210  nodeOrientation.ToEuler(euler);
     211  ISceneNode::setRotation(Euler::ToDegree(1) *
     212                          vector3df(euler.roll, euler.pitch, euler.yaw));
     213
     214  if (position_init == false) {
     215    anim->setTargetNode(this); // a faire pour se teleporter sans les collisions
     216    position_init = true;
     217  }
     218
     219  self->AnimateModel();
     220}
     221
     222void Model_impl::CheckCollision(void) {
     223  // TODO: setEllipsoidRadius should be called in Model::setScale
     224  // but we need to call recalculateBoundingBox
     225  anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
     226
     227  if (anim->collisionOccurred() == true) {
     228    vector3df pos;
     229    vector3df pos_rel;
     230    vector3df nodePosition;
     231    pos = anim->getCollisionPoint();
     232    nodePosition = getPosition();
     233    pos_rel = pos - nodePosition;
     234    // printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
     235    // printf("drone %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
     236    // printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
     237
     238    collision_mutex->GetMutex();
     239    collision_occured = true;
     240    collision_point = ToSimulatorCoordinates(nodePosition);
     241    collision_mutex->ReleaseMutex();
     242  }
     243}
     244
     245void Model_impl::CollisionHandler(void) {
     246  collision_mutex->GetMutex();
     247  if (collision_occured == true) {
     248    collision_occured = false;
    158249    states_mutex->GetMutex();
    159     Quaternion quat=getSimulator()->ToVRPNReference(self->state[0].Quat);
    160     Vector3D position=getSimulator()->ToVRPNReference(self->state[0].Pos);
     250    self->state[0].Pos = collision_point;
     251    self->state[-1].Pos = self->state[0].Pos;
     252    self->state[-2].Pos = self->state[0].Pos;
    161253    states_mutex->ReleaseMutex();
    162 
    163     pos[0]=position.x;
    164     pos[1]=position.y;
    165     pos[2]=position.z;
    166     //warning: d_quat is defined as (qx,qy,qz,qw), which is different from flair::core::Quaternion
    167     d_quat[0] = quat.q1;
    168     d_quat[1] = quat.q2;
    169     d_quat[2] = quat.q3;
    170     d_quat[3] = quat.q0;
    171 
    172     char msgbuf[1000];
    173 
    174     d_sensor = 0;
    175 
    176     int  len = vrpn_Tracker::encode_to(msgbuf);
    177 
    178     if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY))
    179     {
    180         fprintf(stderr,"can't write message: tossing\n");
     254  }
     255  collision_mutex->ReleaseMutex();
     256}
     257
     258void Model_impl::OnRegisterSceneNode(void) {
     259  if (IsVisible)
     260    SceneManager->registerNodeForRendering(this);
     261
     262  ISceneNode::OnRegisterSceneNode();
     263}
     264
     265void Model_impl::render(void) {
     266  IVideoDriver *driver = SceneManager->getVideoDriver();
     267  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     268}
     269
     270// le premier arrive attend l'autre
     271void Model_impl::SynchronizationPoint() {
     272  cond->GetMutex();
     273  sync_count++;
     274
     275  if (sync_count < 2) {
     276    cond->CondWait();
     277  } else {
     278    cond->CondSignal();
     279  }
     280
     281  cond->ReleaseMutex();
     282}
     283#endif // GL
     284
     285void Model_impl::Run(void) {
     286  // Ask Xenomai to warn us upon switches to secondary mode.
     287  WarnUponSwitches(true);
     288
     289#ifdef GL
     290  // synchronize with gui
     291  SynchronizationPoint();
     292#endif
     293
     294  SetPeriodMS(dT->Value() * 1000.);
     295
     296  while (!ToBeStopped()) {
     297    if (dT->ValueChanged())
     298      SetPeriodMS(dT->Value() * 1000.);
     299    WaitPeriod();
     300
     301#ifdef GL
     302    CollisionHandler();
     303#endif
     304    states_mutex->GetMutex();
     305    self->CalcModel();
     306
     307    output->GetMutex();
     308    output->SetValueNoMutex(0, 0, self->state[0].Quat.q0);
     309    output->SetValueNoMutex(1, 0, self->state[0].Quat.q1);
     310    output->SetValueNoMutex(2, 0, self->state[0].Quat.q2);
     311    output->SetValueNoMutex(3, 0, self->state[0].Quat.q3);
     312    output->SetValueNoMutex(4, 0, self->state[0].Pos.x);
     313    output->SetValueNoMutex(5, 0, self->state[0].Pos.y);
     314    output->SetValueNoMutex(6, 0, self->state[0].Pos.z);
     315    output->SetValueNoMutex(7, 0, self->state[0].W.x);
     316    output->SetValueNoMutex(8, 0, self->state[0].W.y);
     317    output->SetValueNoMutex(9, 0, self->state[0].W.z);
     318    output->SetValueNoMutex(10, 0, self->state[0].Vel.x);
     319    output->SetValueNoMutex(11, 0, self->state[0].Vel.y);
     320    output->SetValueNoMutex(12, 0, self->state[0].Vel.z);
     321    output->ReleaseMutex();
     322    output->SetDataTime(GetTime());
     323
     324    self->state.Update();
     325
     326    if (pos_init->ValueChanged() || yaw_init->ValueChanged()) {
     327      self->state[-1].Quat = ComputeInitRotation(Quaternion(1, 0, 0, 0));
     328      self->state[-2].Quat = ComputeInitRotation(Quaternion(1, 0, 0, 0));
     329      self->state[-1].Pos = pos_init->Value();
     330      self->state[-2].Pos = self->state[-1].Pos;
     331#ifdef GL
     332      position_init = false;
     333#endif
    181334    }
    182335
    183     server_mainloop();
    184 }
    185 
    186 #ifdef GL
    187 ITriangleSelector* Model_impl::TriangleSelector(void)
    188 {
    189     return selector;
    190 }
    191 
    192 IMetaTriangleSelector* Model_impl::MetaTriangleSelector(void)
    193 {
    194     return meta_selector;
    195 }
    196 
    197 void Model_impl::UpdatePos(void)
    198 {
    199     vector3df nodePosition;
    200     Quaternion nodeOrientation;
    201     Euler euler;
    202 
    203     states_mutex->GetMutex();
    204     nodePosition=ToIrrlichtCoordinates(self->state[0].Pos);
    205     nodeOrientation=ToIrrlichtOrientation(self->state[0].Quat);
    206336    states_mutex->ReleaseMutex();
    207337
    208     setPosition(nodePosition);
    209 
    210     nodeOrientation.ToEuler(euler);
    211     ISceneNode::setRotation(Euler::ToDegree(1)*vector3df(euler.roll,euler.pitch,euler.yaw));
    212 
    213     if(position_init==false)
    214     {
    215         anim->setTargetNode(this); // a faire pour se teleporter sans les collisions
    216         position_init=true;
    217     }
    218 
    219     self->AnimateModel();
    220 }
    221 
    222 void Model_impl::CheckCollision(void)
    223 {
    224     //TODO: setEllipsoidRadius should be called in Model::setScale
    225     //but we need to call recalculateBoundingBox
    226     anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
    227 
    228     if(anim->collisionOccurred()==true)
    229     {
    230         vector3df pos;
    231         vector3df pos_rel;
    232         vector3df nodePosition;
    233         pos= anim->getCollisionPoint();
    234         nodePosition=getPosition();
    235         pos_rel=pos-nodePosition;
    236         //printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
    237         //printf("drone %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
    238         //printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
    239 
    240         collision_mutex->GetMutex();
    241         collision_occured=true;
    242         collision_point=ToSimulatorCoordinates(nodePosition);
    243         collision_mutex->ReleaseMutex();
    244     }
    245 }
    246 
    247 void Model_impl::CollisionHandler(void)
    248 {
    249     collision_mutex->GetMutex();
    250     if(collision_occured==true)
    251     {
    252         collision_occured=false;
    253         states_mutex->GetMutex();
    254         self->state[0].Pos=collision_point;
    255         self->state[-1].Pos=self->state[0].Pos;
    256         self->state[-2].Pos=self->state[0].Pos;
    257         states_mutex->ReleaseMutex();
    258     }
    259     collision_mutex->ReleaseMutex();
    260 }
    261 
    262 void Model_impl::OnRegisterSceneNode(void)
    263 {
    264     if (IsVisible)
    265         SceneManager->registerNodeForRendering(this);
    266 
    267     ISceneNode::OnRegisterSceneNode();
    268 }
    269 
    270 void Model_impl::render(void)
    271 {
    272     IVideoDriver* driver = SceneManager->getVideoDriver();
    273     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    274 }
    275 
    276 //le premier arrive attend l'autre
    277 void Model_impl::SynchronizationPoint()
    278 {
    279     cond->GetMutex();
    280     sync_count++;
    281 
    282     if (sync_count < 2)
    283     {
    284          cond->CondWait();
    285     }
    286     else
    287     {
    288         cond->CondSignal();
    289     }
    290 
    291     cond->ReleaseMutex();
    292 }
    293 #endif //GL
    294 
    295 void Model_impl::Run(void)
    296 {
    297     // Ask Xenomai to warn us upon switches to secondary mode.
    298     WarnUponSwitches(true);
    299 
    300 #ifdef GL
    301     //synchronize with gui
    302     SynchronizationPoint();
    303 #endif
    304 
    305     SetPeriodMS(dT->Value()*1000.);
    306 
    307     while(!ToBeStopped())
    308     {
    309         if(dT->ValueChanged()) SetPeriodMS(dT->Value()*1000.);
    310         WaitPeriod();
    311 
    312 #ifdef GL
    313         CollisionHandler();
    314 #endif
    315         states_mutex->GetMutex();
    316         self->CalcModel();
    317 
    318         output->GetMutex();
    319         output->SetValueNoMutex(0,0,self->state[0].Quat.q0);
    320         output->SetValueNoMutex(1,0,self->state[0].Quat.q1);
    321         output->SetValueNoMutex(2,0,self->state[0].Quat.q2);
    322         output->SetValueNoMutex(3,0,self->state[0].Quat.q3);
    323         output->SetValueNoMutex(4,0,self->state[0].Pos.x);
    324         output->SetValueNoMutex(5,0,self->state[0].Pos.y);
    325         output->SetValueNoMutex(6,0,self->state[0].Pos.z);
    326         output->SetValueNoMutex(7,0,self->state[0].W.x);
    327         output->SetValueNoMutex(8,0,self->state[0].W.y);
    328         output->SetValueNoMutex(9,0,self->state[0].W.z);
    329         output->SetValueNoMutex(10,0,self->state[0].Vel.x);
    330         output->SetValueNoMutex(11,0,self->state[0].Vel.y);
    331         output->SetValueNoMutex(12,0,self->state[0].Vel.z);
    332         output->ReleaseMutex();
    333         output->SetDataTime(GetTime());
    334 
    335         self->state.Update();
    336 
    337         if(pos_init->ValueChanged() || yaw_init->ValueChanged())
    338         {
    339             self->state[-1].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
    340             self->state[-2].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
    341             self->state[-1].Pos=pos_init->Value();
    342             self->state[-2].Pos=self->state[-1].Pos;
    343 #ifdef GL
    344             position_init=false;
    345 #endif
    346         }
    347 
    348         states_mutex->ReleaseMutex();
    349 
    350         self->ProcessUpdate(output);
    351 
    352     }
    353 
    354     WarnUponSwitches(false);
    355 }
     338    self->ProcessUpdate(output);
     339  }
     340
     341  WarnUponSwitches(false);
     342}

  • trunk/lib/FlairSimulator/src/Parser.cpp

    r10 r15  
    2323#include "GenericObject.h"
    2424
    25 
    2625using namespace irr;
    2726using namespace irr::core;
    … …  
    3130using namespace flair::simulator;
    3231
    33 //todo: getMeshVect doit plutot donner point3D et euler
    34 //adaptr le xml et la dtd
    35 
    36 namespace flair
    37 {
    38 namespace simulator
    39 {
    40 Parser::Parser(Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path, std::string xmlFile): Gui(parent,"Parser",app_width,app_height,scene_width,scene_height, media_path)
    41 {
    42         this->media_path = media_path;
    43         this->parent = parent;
    44         xmlNode        *root_element = NULL;
    45         doc = xmlReadFile(xmlFile.c_str(), NULL, 0);
    46         xmlDtdPtr dtd = NULL;
    47         dtd = xmlParseDTD(NULL,  (const xmlChar *)(media_path.append("/scene.dtd").c_str()));
    48 
    49         if (dtd == NULL)
    50         {
    51                 //xmlGenericError(xmlGenericErrorContext,
    52                 Err("Could not parse DTD scene.dtd\n");
    53         }
    54         else
    55         {
    56                 xmlValidCtxt cvp;
    57                 cvp.userData = (void *) stderr;
    58                 cvp.error    = (xmlValidityErrorFunc) fprintf;
    59                 cvp.warning  = (xmlValidityWarningFunc) fprintf;
    60                 if (!xmlValidateDtd(&cvp, doc, dtd))
    61                 {
    62                         //xmlGenericError(xmlGenericErrorContext,
    63                         Err("Document does not validate against scene.dtd\n");
    64                 }
    65         }
    66 
    67         if (doc == NULL)
    68         {
    69                 Err("error: could not parse file %s\n", xmlFile.c_str());
    70         }
    71         else
    72         {
    73                 /*
    74                  * Get the root element node
    75                  */
    76                 root_element = xmlDocGetRootElement(doc);
    77                 processElements(root_element);
    78     }
    79 
    80 }
    81 
    82 Parser::~Parser()
    83 {
    84         if(doc!=NULL)
    85         {
    86                 /*
    87                  * free the document
    88                  */
    89                 xmlFreeDoc(doc);;
    90         }
    91         /*
    92          *Free the global variables that may
    93          *have been allocated by the parser.
    94          */
    95         xmlCleanupParser();
     32// todo: getMeshVect doit plutot donner point3D et euler
     33// adaptr le xml et la dtd
     34
     35namespace flair {
     36namespace simulator {
     37Parser::Parser(Simulator *parent, int app_width, int app_height,
     38               int scene_width, int scene_height, std::string media_path,
     39               std::string xmlFile)
     40    : Gui(parent, "Parser", app_width, app_height, scene_width, scene_height,
     41          media_path) {
     42  this->media_path = media_path;
     43  this->parent = parent;
     44  xmlNode *root_element = NULL;
     45  doc = xmlReadFile(xmlFile.c_str(), NULL, 0);
     46  xmlDtdPtr dtd = NULL;
     47  dtd = xmlParseDTD(NULL,
     48                    (const xmlChar *)(media_path.append("/scene.dtd").c_str()));
     49
     50  if (dtd == NULL) {
     51    // xmlGenericError(xmlGenericErrorContext,
     52    Err("Could not parse DTD scene.dtd\n");
     53  } else {
     54    xmlValidCtxt cvp;
     55    cvp.userData = (void *)stderr;
     56    cvp.error = (xmlValidityErrorFunc)fprintf;
     57    cvp.warning = (xmlValidityWarningFunc)fprintf;
     58    if (!xmlValidateDtd(&cvp, doc, dtd)) {
     59      // xmlGenericError(xmlGenericErrorContext,
     60      Err("Document does not validate against scene.dtd\n");
     61    }
     62  }
     63
     64  if (doc == NULL) {
     65    Err("error: could not parse file %s\n", xmlFile.c_str());
     66  } else {
     67    /*
     68     * Get the root element node
     69     */
     70    root_element = xmlDocGetRootElement(doc);
     71    processElements(root_element);
     72  }
     73}
     74
     75Parser::~Parser() {
     76  if (doc != NULL) {
     77    /*
     78     * free the document
     79     */
     80    xmlFreeDoc(doc);
     81    ;
     82  }
     83  /*
     84   *Free the global variables that may
     85   *have been allocated by the parser.
     86   */
     87  xmlCleanupParser();
    9688}
    9789
    9890/* Recursive function that prints the XML structure */
    99 void Parser::processElements(xmlNode * a_node)
    100 {
    101         xmlNode *cur_node = NULL;
    102         for (cur_node = a_node; cur_node; cur_node = cur_node->next)
    103         {
    104                 if(cur_node->type == XML_ELEMENT_NODE)
    105                 {
    106                         if(xmlStrEqual(cur_node->name, (xmlChar*) "params"))
    107                         {
    108                                 processParams(cur_node->children);
    109                         }
    110                         else if(xmlStrEqual(cur_node->name, (xmlChar*) "objects"))
    111                         {
    112                                 processObjects(cur_node->children);
    113                         }
    114                         else
    115                         {
    116                                 processElements(cur_node->children);
    117                         }
    118                 }
    119         }
    120 }
    121 
    122 void Parser::processObjects(xmlNode * a_node)
    123 {
    124         FILE* fp;
    125         std::string fileName;
    126         xmlNode *cur_node = NULL;
    127 
    128         const IGeometryCreator *geo;
    129 
    130     geo=getGui()->getSceneManager()->getGeometryCreator();
    131 
    132         for (cur_node = a_node; cur_node; cur_node = cur_node->next)
    133         {
    134                 if(xmlStrEqual(cur_node->name, (xmlChar*) "mesh"))
    135                 {
    136                         fileName=this->media_path;
    137                         fp = NULL;
    138                         fp = fopen( fileName.append((char*)xmlGetProp(cur_node,(xmlChar*)"model")).c_str(), "rb" );
    139                         if(fp!=NULL){
    140                                 GenericObject* object = new GenericObject(parent,"Object", getSceneManager());
    141                                 object->setMesh(getGui()->getMesh((char*)xmlGetProp(cur_node,(xmlChar*)"model")));
    142                                 object->setPosition(getMeshVect(cur_node->children,(xmlChar*)"position"));
    143                                 object->setRotation(getMeshVect(cur_node->children,(xmlChar*)"rotation"));
    144                                 object->setScale(getMeshVect(cur_node->children,(xmlChar*)"scale"));
    145                                 object->render();
    146                         }else{
    147                                 Err("FATAL ERROR : File %s doesn't exist !\r\n",(char*)xmlGetProp(cur_node,(xmlChar*)"model"));
    148                         }
    149                 }else if(xmlStrEqual(cur_node->name, (xmlChar*) "cylinder")){
    150             GenericObject* object = new GenericObject(parent,"Object", getSceneManager());
    151             object->setMesh(geo->createCylinderMesh(ToIrrlichtScale(atof((char*)xmlGetProp(cur_node,(xmlChar*)"radius"))),
    152                                                     ToIrrlichtScale(atof((char*)xmlGetProp(cur_node,(xmlChar*)"length"))),
    153                                                     atof((char*)xmlGetProp(cur_node,(xmlChar*)"tesselation")),
    154                                                     SColor(100, 255, 100, 100)));
    155             object->setPosition(getMeshVect(cur_node->children,(xmlChar*)"position"));
    156             object->setRotation(getMeshVect(cur_node->children,(xmlChar*)"rotation"));
    157             object->setScale(getMeshVect(cur_node->children,(xmlChar*)"scale"));
    158             //object->setMaterialTexture(0,getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
    159                     object->setMaterialType( video::EMT_SOLID );
    160                     object->render();
    161                 }else if(xmlStrEqual(cur_node->name, (xmlChar*) "eight")){
    162             GenericObject* object = new GenericObject(parent,"Object", getSceneManager());
    163             object->setMesh(geo->createCubeMesh(vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"length")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"width")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"eight")))));
    164             object->setPosition(getMeshVect(cur_node->children,(xmlChar*)"position"));
    165             object->setRotation(getMeshVect(cur_node->children,(xmlChar*)"rotation"));
    166             object->setScale(getMeshVect(cur_node->children,(xmlChar*)"scale"));
    167                     object->setMaterialType( video::EMT_TRANSPARENT_ALPHA_CHANNEL );
    168                     //object->getMaterial(0).Textures[0] = getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg");
    169                     object->setMaterialFlag(EMF_LIGHTING, false);
    170                     object->render();
    171         }
    172         }
    173 }
    174 
    175 void Parser::processParams(xmlNode * a_node)
    176 {
    177 
    178         xmlNode *cur_node = NULL;
    179         for (cur_node = a_node; cur_node; cur_node = cur_node->next)
    180         {
    181                 if(xmlStrEqual(cur_node->name, (xmlChar*) "archive"))
    182                 {
    183                         std::string file=media_path + "/" + (char*)xmlGetProp(cur_node,(xmlChar*)"path");
    184                         getDevice()->getFileSystem()->addFileArchive(file.c_str());
    185                 }
    186                 else if(xmlStrEqual(cur_node->name, (xmlChar*) "mesh"))
    187                 {
    188                         setMesh((char*)xmlGetProp(cur_node,(xmlChar*)"model"),getSceneVect(cur_node->children,(xmlChar*)"position"),getSceneVect(cur_node->children,(xmlChar*)"rotation"),getSceneVect(cur_node->children,(xmlChar*)"scale",true));
    189 
    190                 }
    191         }
    192 }
    193 //todo rendre un vector3D framework
    194 //retirer irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df vect);
    195 vector3df Parser::getMeshVect(xmlNode * mesh_node, xmlChar * param)
    196 {
    197         xmlNode *cur_node = NULL;
    198         for (cur_node = mesh_node; cur_node; cur_node = cur_node->next)
    199         {
    200                 if(xmlStrEqual(cur_node->name, param)){
    201                         return vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"x")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"y")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"z")));
    202                 }
    203         }
    204         return vector3df(0,0,0);
    205 }
    206 
    207 vector3df Parser::getSceneVect(xmlNode * mesh_node, xmlChar * param, bool isScale)
    208 {
    209         xmlNode *cur_node = NULL;
    210         for (cur_node = mesh_node; cur_node; cur_node = cur_node->next)
    211         {
    212                 if(xmlStrEqual(cur_node->name, param)){
    213                         if(isScale){
    214                                 return vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"x")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"z")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"y")));
    215                         }
    216                         return vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"x")),-atof((char*)xmlGetProp(cur_node,(xmlChar*)"z")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"y")));
    217                 }
    218         }
    219         return vector3df(0,0,0);
     91void Parser::processElements(xmlNode *a_node) {
     92  xmlNode *cur_node = NULL;
     93  for (cur_node = a_node; cur_node; cur_node = cur_node->next) {
     94    if (cur_node->type == XML_ELEMENT_NODE) {
     95      if (xmlStrEqual(cur_node->name, (xmlChar *)"params")) {
     96        processParams(cur_node->children);
     97      } else if (xmlStrEqual(cur_node->name, (xmlChar *)"objects")) {
     98        processObjects(cur_node->children);
     99      } else {
     100        processElements(cur_node->children);
     101      }
     102    }
     103  }
     104}
     105
     106void Parser::processObjects(xmlNode *a_node) {
     107  FILE *fp;
     108  std::string fileName;
     109  xmlNode *cur_node = NULL;
     110
     111  const IGeometryCreator *geo;
     112
     113  geo = getGui()->getSceneManager()->getGeometryCreator();
     114
     115  for (cur_node = a_node; cur_node; cur_node = cur_node->next) {
     116    if (xmlStrEqual(cur_node->name, (xmlChar *)"mesh")) {
     117      fileName = this->media_path;
     118      fp = NULL;
     119      fp = fopen(fileName.append((char *)xmlGetProp(
     120                                     cur_node, (xmlChar *)"model")).c_str(),
     121                 "rb");
     122      if (fp != NULL) {
     123        GenericObject *object =
     124            new GenericObject(parent, "Object", getSceneManager());
     125        object->setMesh(getGui()->getMesh(
     126            (char *)xmlGetProp(cur_node, (xmlChar *)"model")));
     127        object->setPosition(
     128            getMeshVect(cur_node->children, (xmlChar *)"position"));
     129        object->setRotation(
     130            getMeshVect(cur_node->children, (xmlChar *)"rotation"));
     131        object->setScale(getMeshVect(cur_node->children, (xmlChar *)"scale"));
     132        object->render();
     133      } else {
     134        Err("FATAL ERROR : File %s doesn't exist !\r\n",
     135            (char *)xmlGetProp(cur_node, (xmlChar *)"model"));
     136      }
     137    } else if (xmlStrEqual(cur_node->name, (xmlChar *)"cylinder")) {
     138      GenericObject *object =
     139          new GenericObject(parent, "Object", getSceneManager());
     140      object->setMesh(geo->createCylinderMesh(
     141          ToIrrlichtScale(
     142              atof((char *)xmlGetProp(cur_node, (xmlChar *)"radius"))),
     143          ToIrrlichtScale(
     144              atof((char *)xmlGetProp(cur_node, (xmlChar *)"length"))),
     145          atof((char *)xmlGetProp(cur_node, (xmlChar *)"tesselation")),
     146          SColor(100, 255, 100, 100)));
     147      object->setPosition(
     148          getMeshVect(cur_node->children, (xmlChar *)"position"));
     149      object->setRotation(
     150          getMeshVect(cur_node->children, (xmlChar *)"rotation"));
     151      object->setScale(getMeshVect(cur_node->children, (xmlChar *)"scale"));
     152      // object->setMaterialTexture(0,getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
     153      object->setMaterialType(video::EMT_SOLID);
     154      object->render();
     155    } else if (xmlStrEqual(cur_node->name, (xmlChar *)"eight")) {
     156      GenericObject *object =
     157          new GenericObject(parent, "Object", getSceneManager());
     158      object->setMesh(geo->createCubeMesh(
     159          vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"length")),
     160                    atof((char *)xmlGetProp(cur_node, (xmlChar *)"width")),
     161                    atof((char *)xmlGetProp(cur_node, (xmlChar *)"eight")))));
     162      object->setPosition(
     163          getMeshVect(cur_node->children, (xmlChar *)"position"));
     164      object->setRotation(
     165          getMeshVect(cur_node->children, (xmlChar *)"rotation"));
     166      object->setScale(getMeshVect(cur_node->children, (xmlChar *)"scale"));
     167      object->setMaterialType(video::EMT_TRANSPARENT_ALPHA_CHANNEL);
     168      // object->getMaterial(0).Textures[0] =
     169      // getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg");
     170      object->setMaterialFlag(EMF_LIGHTING, false);
     171      object->render();
     172    }
     173  }
     174}
     175
     176void Parser::processParams(xmlNode *a_node) {
     177
     178  xmlNode *cur_node = NULL;
     179  for (cur_node = a_node; cur_node; cur_node = cur_node->next) {
     180    if (xmlStrEqual(cur_node->name, (xmlChar *)"archive")) {
     181      std::string file =
     182          media_path + "/" + (char *)xmlGetProp(cur_node, (xmlChar *)"path");
     183      getDevice()->getFileSystem()->addFileArchive(file.c_str());
     184    } else if (xmlStrEqual(cur_node->name, (xmlChar *)"mesh")) {
     185      setMesh((char *)xmlGetProp(cur_node, (xmlChar *)"model"),
     186              getSceneVect(cur_node->children, (xmlChar *)"position"),
     187              getSceneVect(cur_node->children, (xmlChar *)"rotation"),
     188              getSceneVect(cur_node->children, (xmlChar *)"scale", true));
     189    }
     190  }
     191}
     192// todo rendre un vector3D framework
     193// retirer irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df
     194// vect);
     195vector3df Parser::getMeshVect(xmlNode *mesh_node, xmlChar *param) {
     196  xmlNode *cur_node = NULL;
     197  for (cur_node = mesh_node; cur_node; cur_node = cur_node->next) {
     198    if (xmlStrEqual(cur_node->name, param)) {
     199      return vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"x")),
     200                       atof((char *)xmlGetProp(cur_node, (xmlChar *)"y")),
     201                       atof((char *)xmlGetProp(cur_node, (xmlChar *)"z")));
     202    }
     203  }
     204  return vector3df(0, 0, 0);
     205}
     206
     207vector3df Parser::getSceneVect(xmlNode *mesh_node, xmlChar *param,
     208                               bool isScale) {
     209  xmlNode *cur_node = NULL;
     210  for (cur_node = mesh_node; cur_node; cur_node = cur_node->next) {
     211    if (xmlStrEqual(cur_node->name, param)) {
     212      if (isScale) {
     213        return vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"x")),
     214                         atof((char *)xmlGetProp(cur_node, (xmlChar *)"z")),
     215                         atof((char *)xmlGetProp(cur_node, (xmlChar *)"y")));
     216      }
     217      return vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"x")),
     218                       -atof((char *)xmlGetProp(cur_node, (xmlChar *)"z")),
     219                       atof((char *)xmlGetProp(cur_node, (xmlChar *)"y")));
     220    }
     221  }
     222  return vector3df(0, 0, 0);
    220223}
    221224
    222225} // end namespace simulator
    223226} // end namespace flair
    224 #endif //GL
     227#endif // GL

  • trunk/lib/FlairSimulator/src/Parser.h

    r10 r15  
    2222#include <libxml/tree.h>
    2323
    24 namespace flair
    25 {
    26 namespace simulator
    27 {
    28 class Parser:public Gui
    29 {
     24namespace flair {
     25namespace simulator {
     26class Parser : public Gui {
    3027
    31 /*can create:
    32 - cylinders: in y axis
     28  /*can create:
     29  - cylinders: in y axis
    3330
    34 */
    35     public:
    36         Parser(Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path, std::string xmlFile);
    37         ~Parser();
     31  */
     32public:
     33  Parser(Simulator *parent, int app_width, int app_height, int scene_width,
     34         int scene_height, std::string media_path, std::string xmlFile);
     35  ~Parser();
    3836
    39 
    40     private:
    41         xmlDoc *doc;
    42         std::string media_path;
    43         Simulator* parent;
    44         void processElements(xmlNode * a_node);
    45         void processObjects(xmlNode * a_node);
    46         void processParams(xmlNode * a_node);
    47         irr::core::vector3df getMeshVect(xmlNode * mesh_node, xmlChar * param);
    48         irr::core::vector3df getSceneVect(xmlNode * mesh_node, xmlChar * param, bool isScale=false);
     37private:
     38  xmlDoc *doc;
     39  std::string media_path;
     40  Simulator *parent;
     41  void processElements(xmlNode *a_node);
     42  void processObjects(xmlNode *a_node);
     43  void processParams(xmlNode *a_node);
     44  irr::core::vector3df getMeshVect(xmlNode *mesh_node, xmlChar *param);
     45  irr::core::vector3df getSceneVect(xmlNode *mesh_node, xmlChar *param,
     46                                    bool isScale = false);
    4947};
    5048}

  • trunk/lib/FlairSimulator/src/SensorGL.cpp

    r10 r15  
    2525using namespace flair::simulator;
    2626
    27 namespace flair
    28 {
    29 namespace sensor
    30 {
     27namespace flair {
     28namespace sensor {
    3129
    32 SensorGL::SensorGL(const Model *parent)
    33 {
    34     collMan =getGui()->getSceneManager()->getSceneCollisionManager();
    35     node=parent->Model::pimpl_;
     30SensorGL::SensorGL(const Model *parent) {
     31  collMan = getGui()->getSceneManager()->getSceneCollisionManager();
     32  node = parent->Model::pimpl_;
    3633}
    3734
    38 SensorGL::~SensorGL()
    39 {
    40 }
     35SensorGL::~SensorGL() {}
    4136
    42 ISceneCollisionManager* SensorGL::CollMan(void) const
    43 {
    44     return collMan;
    45 }
     37ISceneCollisionManager *SensorGL::CollMan(void) const { return collMan; }
    4638
    47 ISceneNode* SensorGL::Node(void) const
    48 {
    49     return node;
    50 }
     39ISceneNode *SensorGL::Node(void) const { return node; }
    5140
    5241} // end namespace simulator

  • trunk/lib/FlairSimulator/src/SensorGL.h

    r10 r15  
    1818#define SENSORGL_H
    1919
    20 namespace irr
    21 {
    22     namespace scene
    23     {
    24         class ISceneNode;
    25         class ISceneCollisionManager;
    26     }
     20namespace irr {
     21namespace scene {
     22class ISceneNode;
     23class ISceneCollisionManager;
     24}
    2725}
    2826
    29 namespace flair
    30 {
    31     namespace simulator
    32     {
    33         class Model;
    34     }
     27namespace flair {
     28namespace simulator {
     29class Model;
     30}
    3531}
    3632
    37 namespace flair
    38 {
    39 namespace sensor
    40 {
    41     class SensorGL
    42     {
    43         public:
    44             SensorGL(const simulator::Model *parent);
    45             virtual ~SensorGL()=0;
     33namespace flair {
     34namespace sensor {
     35class SensorGL {
     36public:
     37  SensorGL(const simulator::Model *parent);
     38  virtual ~SensorGL() = 0;
    4639
    47         protected:
    48     #ifdef GL
    49             irr::scene::ISceneCollisionManager* CollMan(void) const;
    50             irr::scene::ISceneNode* Node(void) const;
    51     #endif
    52         private:
    53     #ifdef GL
    54             irr::scene::ISceneCollisionManager* collMan;
    55             irr::scene::ISceneNode* node;
    56     #endif
    57     };
     40protected:
     41#ifdef GL
     42  irr::scene::ISceneCollisionManager *CollMan(void) const;
     43  irr::scene::ISceneNode *Node(void) const;
     44#endif
     45private:
     46#ifdef GL
     47  irr::scene::ISceneCollisionManager *collMan;
     48  irr::scene::ISceneNode *node;
     49#endif
     50};
    5851} // end namespace sensor
    5952} // end namespace flair

  • trunk/lib/FlairSimulator/src/SimuCameraGL.cpp

    r10 r15  
    4141using namespace flair::simulator;
    4242
    43 namespace flair
    44 {
    45 namespace sensor
    46 {
    47 
    48 SimuCameraGL::SimuCameraGL(const Model* parent,std::string name,int width,int height,int x,int y,int dev_id) :SimuCamera(parent,name,width,height,3,dev_id),SensorGL(parent)
    49 {
    50     smgr=getGui()->getSceneManager();
    51     camera=smgr->addCameraSceneNode();
    52     camera->addAnimator(this);
    53     camera->setAspectRatio(4.0f/3.0f);//on force a cause du view port
    54 
    55     this->width=width;
    56     this->height=height;
    57     this->x=x;
    58     this->y=y;
    59 
    60     index = 0;
    61 
    62     buffer=(char*)malloc(width*height*3);
    63 
    64     //user interface
    65     Tab* setup_tab=new Tab(parent->GetTabWidget(),name);
    66         position=new Vector3DSpinBox(setup_tab->NewRow(),"position",-2,2,.01);
    67         direction=new Vector3DSpinBox(setup_tab->NewRow(),"direction",-2,2,.01);
    68         up=new Vector3DSpinBox(setup_tab->NewRow(),"up",-2,2,.01);
    69         fov=new DoubleSpinBox(setup_tab->NewRow(),"fov:",0,180,5);
    70 
    71     if(strcmp((char*)glGetString(GL_VENDOR),"Intel Open Source Technology Center")==0) {
    72         Thread::Warn("disabling cameras output for Intel card (bug with PBO)\n");
    73         disable_output=true;
    74     } else {
    75         disable_output=false;
     43namespace flair {
     44namespace sensor {
     45
     46SimuCameraGL::SimuCameraGL(const Model *parent, std::string name, int width,
     47                           int height, int x, int y, int dev_id)
     48    : SimuCamera(parent, name, width, height, 3, dev_id), SensorGL(parent) {
     49  smgr = getGui()->getSceneManager();
     50  camera = smgr->addCameraSceneNode();
     51  camera->addAnimator(this);
     52  camera->setAspectRatio(4.0f / 3.0f); // on force a cause du view port
     53
     54  this->width = width;
     55  this->height = height;
     56  this->x = x;
     57  this->y = y;
     58
     59  index = 0;
     60
     61  buffer = (char *)malloc(width * height * 3);
     62
     63  // user interface
     64  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
     65  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
     66  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
     67  up = new Vector3DSpinBox(setup_tab->NewRow(), "up", -2, 2, .01);
     68  fov = new DoubleSpinBox(setup_tab->NewRow(), "fov:", 0, 180, 5);
     69
     70  if (strcmp((char *)glGetString(GL_VENDOR),
     71             "Intel Open Source Technology Center") == 0) {
     72    Thread::Warn("disabling cameras output for Intel card (bug with PBO)\n");
     73    disable_output = true;
     74  } else {
     75    disable_output = false;
     76  }
     77
     78  if (isGlExtensionSupported("GL_ARB_pixel_buffer_object")) {
     79    use_pbo = true;
     80    // create 2 pixel buffer objects, you need to delete them when program
     81    // exits.
     82    // glBufferDataARB with NULL pointer reserves only memory space.
     83    pboIds = (GLuint *)malloc(PBO_COUNT * sizeof(GLuint));
     84    glGenBuffersARB(PBO_COUNT, pboIds);
     85    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[0]);
     86    glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width * height * 3, 0,
     87                    GL_STREAM_READ_ARB);
     88    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[1]);
     89    glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width * height * 3, 0,
     90                    GL_STREAM_READ_ARB);
     91
     92    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
     93  } else {
     94    use_pbo = false;
     95    Thread::Warn("GL_ARB_pixel_buffer_object is not suppoorted\n");
     96  }
     97  if (isGlExtensionSupported("GL_PACK_INVERT_MESA")) {
     98    invert_pixel = false;
     99    Thread::Warn("GL_PACK_INVERT_MESA is suppoorted\n");
     100  } else {
     101    invert_pixel = true;
     102  }
     103}
     104
     105SimuCameraGL::~SimuCameraGL() {
     106  free(buffer);
     107
     108  if (use_pbo) {
     109    glDeleteBuffersARB(PBO_COUNT, pboIds);
     110    free(pboIds);
     111  }
     112}
     113
     114void SimuCameraGL::setNearValue(float zn) { camera->setNearValue(zn); }
     115
     116void SimuCameraGL::setFarValue(float zf) { camera->setFarValue(zf); }
     117
     118void SimuCameraGL::UpdateFrom(const io_data *data) {
     119  if (noGui() == false && data == NULL) {
     120    smgr->setActiveCamera(camera);
     121    smgr->getVideoDriver()->setViewPort(rect<s32>(x, y, x + width, y + height));
     122    smgr->drawAll();
     123    // use_pbo=false;
     124    getImage();
     125  }
     126}
     127
     128void SimuCameraGL::getImage(void) {
     129  if (disable_output)
     130    return;
     131  // convert from irrlicht top left origin to gl bottom left origin
     132  int y = smgr->getVideoDriver()->getScreenSize().Height - height - this->y;
     133
     134  // We want to read the front buffer to get the latest render finished.
     135  Time a = GetTime();
     136  glReadBuffer(GL_FRONT);
     137  if (use_pbo) // with PBO
     138  {
     139    // increment current index first then get the next index
     140    // "index" is used to read pixels from a framebuffer to a PBO
     141    // "nextIndex" is used to process pixels in the other PBO
     142    index = (index + 1) % PBO_COUNT;
     143    int nextIndex = (index + 1) % PBO_COUNT;
     144
     145    // copy pixels from framebuffer to PBO
     146    // Use offset instead of pointer.
     147    // OpenGL should perform asynch DMA transfer, so glReadPixels() will return
     148    // immediately.
     149    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[index]);
     150    glReadPixels(x, y, width, height, GL_BGR, GL_UNSIGNED_BYTE, 0);
     151
     152    // map the PBO that contain framebuffer pixels before processing it
     153    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[nextIndex]);
     154    GLubyte *src = (GLubyte *)glMapBufferARB(
     155        GL_PIXEL_PACK_BUFFER_ARB, GL_READ_WRITE_ARB); // GL_READ_ONLY_ARB);
     156    if (src) {
     157      putImage((char *)src);
     158      glUnmapBufferARB(
     159          GL_PIXEL_PACK_BUFFER_ARB); // release pointer to the mapped buffer
    76160    }
    77 
    78     if(isGlExtensionSupported("GL_ARB_pixel_buffer_object"))
    79     {
    80         use_pbo=true;
    81          // create 2 pixel buffer objects, you need to delete them when program exits.
    82         // glBufferDataARB with NULL pointer reserves only memory space.
    83         pboIds=(GLuint*)malloc(PBO_COUNT*sizeof(GLuint));
    84         glGenBuffersARB(PBO_COUNT, pboIds);
    85         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[0]);
    86         glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width*height*3, 0, GL_STREAM_READ_ARB);
    87         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[1]);
    88         glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width*height*3, 0, GL_STREAM_READ_ARB);
    89 
    90         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
     161    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
     162  } else {
     163    glReadPixels(x, y, width, height, GL_BGR, GL_UNSIGNED_BYTE, buffer);
     164    putImage(buffer);
     165  }
     166
     167  glReadBuffer(GL_BACK);
     168  Time b = GetTime();
     169  Time c = b - a;
     170  // printf("%s %f\n",Thread::ObjectName().c_str(),(float)(c/1000000.));
     171}
     172
     173void SimuCameraGL::putImage(char *buf) {
     174  if (invert_pixel == true) {
     175    // opengl images are horizontally flipped, so we have to fix that here.
     176    const s32 pitch = width * 3;
     177    char *pixels = buf;
     178    char *p2 = pixels + (height - 1) * pitch;
     179    char *tmpBuffer = new char[pitch];
     180    for (int i = 0; i < height; i += 2) {
     181      memcpy(tmpBuffer, pixels, pitch);
     182      memcpy(pixels, p2, pitch);
     183      memcpy(p2, tmpBuffer, pitch);
     184      pixels += pitch;
     185      p2 -= pitch;
    91186    }
    92     else
    93     {
    94         use_pbo=false;
    95         Thread::Warn("GL_ARB_pixel_buffer_object is not suppoorted\n");
    96     }
    97     if(isGlExtensionSupported("GL_PACK_INVERT_MESA"))
    98     {
    99         invert_pixel=false;
    100         Thread::Warn("GL_PACK_INVERT_MESA is suppoorted\n");
    101     }
    102     else
    103     {
    104         invert_pixel=true;
    105     }
    106 }
    107 
    108 SimuCameraGL::~SimuCameraGL()
    109 {
    110     free(buffer);
    111 
    112     if(use_pbo)
    113     {
    114         glDeleteBuffersARB(PBO_COUNT, pboIds);
    115         free(pboIds);
    116     }
    117 }
    118 
    119 void SimuCameraGL::setNearValue(float zn) {
    120     camera->setNearValue(zn);
    121 }
    122 
    123 
    124 void SimuCameraGL::setFarValue(float zf) {
    125     camera->setFarValue(zf);
    126 }
    127 
    128 
    129 void SimuCameraGL::UpdateFrom(const io_data *data)
    130 {
    131     if(noGui()==false && data==NULL)
    132     {
    133         smgr->setActiveCamera(camera);
    134         smgr->getVideoDriver()->setViewPort(rect<s32>(x,y,x+width,y+height));
    135         smgr->drawAll();
    136         //use_pbo=false;
    137         getImage();
    138     }
    139 }
    140 
    141 void SimuCameraGL::getImage(void)
    142 {
    143     if(disable_output) return;
    144     //convert from irrlicht top left origin to gl bottom left origin
    145     int y=smgr->getVideoDriver()->getScreenSize().Height-height-this->y;
    146 
    147         // We want to read the front buffer to get the latest render finished.
    148         Time a=GetTime();
    149         glReadBuffer(GL_FRONT);
    150         if(use_pbo) // with PBO
    151     {
    152         // increment current index first then get the next index
    153         // "index" is used to read pixels from a framebuffer to a PBO
    154         // "nextIndex" is used to process pixels in the other PBO
    155         index = (index + 1) % PBO_COUNT;
    156         int nextIndex = (index + 1) % PBO_COUNT;
    157 
    158         // copy pixels from framebuffer to PBO
    159         // Use offset instead of pointer.
    160         // OpenGL should perform asynch DMA transfer, so glReadPixels() will return immediately.
    161         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[index]);
    162         glReadPixels(x,y,width,height, GL_BGR, GL_UNSIGNED_BYTE, 0);
    163 
    164         // map the PBO that contain framebuffer pixels before processing it
    165         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[nextIndex]);
    166         GLubyte* src = (GLubyte*)glMapBufferARB(GL_PIXEL_PACK_BUFFER_ARB,GL_READ_WRITE_ARB);//GL_READ_ONLY_ARB);
    167         if(src)
    168         {
    169             putImage((char*)src);
    170             glUnmapBufferARB(GL_PIXEL_PACK_BUFFER_ARB);     // release pointer to the mapped buffer
    171         }
    172         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
    173     }
    174     else
    175     {
    176         glReadPixels(x,y,width,height, GL_BGR, GL_UNSIGNED_BYTE, buffer);
    177         putImage(buffer);
    178     }
    179 
    180         glReadBuffer(GL_BACK);
    181         Time b=GetTime();
    182         Time c=b-a;
    183         //printf("%s %f\n",Thread::ObjectName().c_str(),(float)(c/1000000.));
    184 }
    185 
    186 void SimuCameraGL::putImage(char* buf)
    187 {
    188     if(invert_pixel==true)
    189     {
    190         // opengl images are horizontally flipped, so we have to fix that here.
    191         const s32 pitch=width*3;
    192         char* pixels = buf;
    193         char* p2 = pixels + (height - 1) * pitch;
    194         char* tmpBuffer = new char[pitch];
    195         for (int i=0; i < height; i += 2)
    196         {
    197             memcpy(tmpBuffer, pixels, pitch);
    198             memcpy(pixels, p2, pitch);
    199             memcpy(p2, tmpBuffer, pitch);
    200             pixels += pitch;
    201             p2 -= pitch;
    202         }
    203         delete [] tmpBuffer;
    204     }
    205 
    206     shmem->Write(buf,width*height*3);
    207 }
    208 
    209 void SimuCameraGL::animateNode(ISceneNode* node, u32 timeMs)
    210 {
    211     ICameraSceneNode* camera = static_cast<ICameraSceneNode*>(node);
    212 
    213     matrix4 m;
    214     m.setRotationDegrees(Node()->getRotation());
    215 
    216     // transform forward vector of camera
    217     vector3df frv =ToIrrlichtCoordinates(direction->Value());
    218     m.transformVect(frv);
    219 
    220     // transform upvector of camera
    221     vector3df upv =ToIrrlichtCoordinates(up->Value());
    222     m.transformVect(upv);
    223 
    224     // transform camera offset (thanks to Zeuss for finding it was missing)
    225     vector3df offset = ToIrrlichtCoordinates(position->Value());
    226     m.transformVect(offset);
    227 
    228     // set camera
    229     camera->setPosition(Node()->getPosition() + offset); //position camera in front of the ship
    230     camera->setUpVector(upv); //set up vector of camera >> Zeuss - tested with +node->getPostion() and it didnt work, but this works fine.
    231     camera->setTarget(Node()->getPosition() + offset+frv); //set target of camera (look at point) >> Zeuss - Dont forget to add the node positiob
    232 
    233     camera->setFOV(Euler::ToRadian(fov->Value()));
    234 }
    235 
    236 ISceneNodeAnimator* SimuCameraGL::createClone(ISceneNode* node,
    237             ISceneManager* newManager)
    238 {
    239     return NULL;
     187    delete[] tmpBuffer;
     188  }
     189
     190  shmem->Write(buf, width * height * 3);
     191}
     192
     193void SimuCameraGL::animateNode(ISceneNode *node, u32 timeMs) {
     194  ICameraSceneNode *camera = static_cast<ICameraSceneNode *>(node);
     195
     196  matrix4 m;
     197  m.setRotationDegrees(Node()->getRotation());
     198
     199  // transform forward vector of camera
     200  vector3df frv = ToIrrlichtCoordinates(direction->Value());
     201  m.transformVect(frv);
     202
     203  // transform upvector of camera
     204  vector3df upv = ToIrrlichtCoordinates(up->Value());
     205  m.transformVect(upv);
     206
     207  // transform camera offset (thanks to Zeuss for finding it was missing)
     208  vector3df offset = ToIrrlichtCoordinates(position->Value());
     209  m.transformVect(offset);
     210
     211  // set camera
     212  camera->setPosition(Node()->getPosition() +
     213                      offset); // position camera in front of the ship
     214  camera->setUpVector(upv);    // set up vector of camera >> Zeuss - tested with
     215  // +node->getPostion() and it didnt work, but this
     216  // works fine.
     217  camera->setTarget(Node()->getPosition() + offset + frv); // set target of
     218                                                           // camera (look at
     219                                                           // point) >> Zeuss -
     220                                                           // Dont forget to add
     221                                                           // the node positiob
     222
     223  camera->setFOV(Euler::ToRadian(fov->Value()));
     224}
     225
     226ISceneNodeAnimator *SimuCameraGL::createClone(ISceneNode *node,
     227                                              ISceneManager *newManager) {
     228  return NULL;
    240229}
    241230

  • trunk/lib/FlairSimulator/src/SimuCameraGL.h

    r10 r15  
    1818#include <ISceneNodeAnimator.h>
    1919
    20 // in order to get function prototypes from glext.h, define GL_GLEXT_PROTOTYPES before including glext.h
     20// in order to get function prototypes from glext.h, define GL_GLEXT_PROTOTYPES
     21// before including glext.h
    2122#define GL_GLEXT_PROTOTYPES
    2223#include <GL/gl.h>
    2324
    24 namespace irr
    25 {
    26     namespace scene
    27     {
    28         class ICameraSceneNode;
    29     }
     25namespace irr {
     26namespace scene {
     27class ICameraSceneNode;
     28}
    3029}
    3130
    32 namespace flair
    33 {
    34     namespace gui
    35     {
    36         class DoubleSpinBox;
    37         class Vector3DSpinBox;
    38     }
    39     namespace simulator
    40     {
    41         class Model;
    42     }
     31namespace flair {
     32namespace gui {
     33class DoubleSpinBox;
     34class Vector3DSpinBox;
     35}
     36namespace simulator {
     37class Model;
     38}
    4339}
    4440
    45 namespace flair
    46 {
    47 namespace sensor
    48 {
    49     /*! \class SimuCameraGL
    50     * \brief Class for a simulation camera
    51     *
    52     */
    53     class SimuCameraGL : public SimuCamera, public SensorGL, public irr::scene::ISceneNodeAnimator
    54     {
    55         public:
    56             //top left origin
    57             SimuCameraGL(const simulator::Model* parent,std::string name,int width,int height,int x,int y,int dev_id);
    58             ~SimuCameraGL();
    59             //! Sets the value of the near clipping plane. (default: 1.0f)
    60             /** \param zn: New z near value. */
    61             void setNearValue(float zn);
     41namespace flair {
     42namespace sensor {
     43/*! \class SimuCameraGL
     44* \brief Class for a simulation camera
     45*
     46*/
     47class SimuCameraGL : public SimuCamera,
     48                     public SensorGL,
     49                     public irr::scene::ISceneNodeAnimator {
     50public:
     51  // top left origin
     52  SimuCameraGL(const simulator::Model *parent, std::string name, int width,
     53               int height, int x, int y, int dev_id);
     54  ~SimuCameraGL();
     55  //! Sets the value of the near clipping plane. (default: 1.0f)
     56  /** \param zn: New z near value. */
     57  void setNearValue(float zn);
    6258
    63             //! Sets the value of the far clipping plane (default: 2000.0f)
    64             /** \param zf: New z far value. */
    65             void setFarValue(float zf);
     59  //! Sets the value of the far clipping plane (default: 2000.0f)
     60  /** \param zf: New z far value. */
     61  void setFarValue(float zf);
    6662
    67         private:
    68             void UpdateFrom(const core::io_data *data);
    69             void animateNode(irr::scene::ISceneNode* node, irr::u32 timeMs);
    70             ISceneNodeAnimator* createClone(irr::scene::ISceneNode* node,irr::scene::ISceneManager* newManager=0);
    71             void getImage(void);
    72             void putImage(char* pixels);
    73             irr::scene::ICameraSceneNode* camera;
    74             irr::scene::ISceneManager* smgr;
    75             gui::Vector3DSpinBox *position,*direction,*up;
    76             gui::DoubleSpinBox *fov;
    77             int width,height,x,y;
    78             char* buffer;
    79             bool use_pbo,invert_pixel,disable_output;
    80             GLuint *pboIds;
    81             int index ;
    82     };
     63private:
     64  void UpdateFrom(const core::io_data *data);
     65  void animateNode(irr::scene::ISceneNode *node, irr::u32 timeMs);
     66  ISceneNodeAnimator *createClone(irr::scene::ISceneNode *node,
     67                                  irr::scene::ISceneManager *newManager = 0);
     68  void getImage(void);
     69  void putImage(char *pixels);
     70  irr::scene::ICameraSceneNode *camera;
     71  irr::scene::ISceneManager *smgr;
     72  gui::Vector3DSpinBox *position, *direction, *up;
     73  gui::DoubleSpinBox *fov;
     74  int width, height, x, y;
     75  char *buffer;
     76  bool use_pbo, invert_pixel, disable_output;
     77  GLuint *pboIds;
     78  int index;
     79};
    8380} // end namespace simulator
    8481} // end namespace flair

  • trunk/lib/FlairSimulator/src/SimuLaserGL.cpp

    r10 r15  
    3636using namespace flair::simulator;
    3737
    38 namespace flair
    39 {
    40 namespace sensor
    41 {
    42 SimuLaserGL::SimuLaserGL(const Model* parent,std::string name,int dev_id) :SimuLaser(parent,name,dev_id),SensorGL(parent)
    43 {
    44     Tab* setup_tab=new Tab(parent->GetTabWidget(),name);
    45             position=new Vector3DSpinBox(setup_tab->NewRow(),"position",-2,2,.01);
    46             direction=new Vector3DSpinBox(setup_tab->NewRow(),"direction",-2,2,.01);
    47             range=new DoubleSpinBox(setup_tab->NewRow(),"range:",0,30,1);
     38namespace flair {
     39namespace sensor {
     40SimuLaserGL::SimuLaserGL(const Model *parent, std::string name, int dev_id)
     41    : SimuLaser(parent, name, dev_id), SensorGL(parent) {
     42  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
     43  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
     44  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
     45  range = new DoubleSpinBox(setup_tab->NewRow(), "range:", 0, 30, 1);
    4846}
    4947
    50 SimuLaserGL::~SimuLaserGL()
    51 {
     48SimuLaserGL::~SimuLaserGL() {}
    5249
    53 }
     50void SimuLaserGL::UpdateFrom(const io_data *data) {
     51  float value[360];
    5452
    55 void SimuLaserGL::UpdateFrom(const io_data *data)
    56 {
    57     float value[360];
     53  if (noGui() == false && data == NULL) {
     54    for (int i = 0; i < 360; i++) {
     55      line3d<f32> ray_laser;         // rayon provenant de l'ultra son
     56      vector3df intersection_laser;  // point intersection us avec le sol
     57      triangle3df hitTriangle_laser; // triangle intersection us avec le sol
    5858
    59     if(noGui()==false && data==NULL)
    60     {
    61         for (int i=0; i<360; i++)
    62         {
    63             line3d<f32> ray_laser;//rayon provenant de l'ultra son
    64             vector3df intersection_laser;//point intersection us avec le sol
    65             triangle3df hitTriangle_laser;//triangle intersection us avec le sol
     59      // get rotation matrix of node - Zeuss must be getRotation not
     60      // getRelativeTransformation
     61      matrix4 m;
     62      matrix4 M;
     63      m.setRotationDegrees(Node()->getRotation());
    6664
    67              //get rotation matrix of node - Zeuss must be getRotation not getRelativeTransformation
    68             matrix4 m;
    69             matrix4 M;
    70             m.setRotationDegrees(Node()->getRotation());
     65      // Matrice de rotation pour balayage du laser, angle i
     66      M.setRotationDegrees(vector3df(0, 0, i));
     67      // transform forward vector of us
     68      vector3df frv = ToIrrlichtCoordinates(direction->Value());
     69      M.transformVect(frv);
     70      m.transformVect(frv);
     71      frv.normalize();
    7172
    72             //Matrice de rotation pour balayage du laser, angle i
    73             M.setRotationDegrees(vector3df(0,0,i));
    74             // transform forward vector of us
    75             vector3df frv =ToIrrlichtCoordinates(direction->Value());
    76             M.transformVect(frv);
    77             m.transformVect(frv);
    78             frv.normalize();
     73      // transform pos vector of us
     74      vector3df pos = ToIrrlichtCoordinates(position->Value());
     75      m.transformVect(pos);
    7976
     77      ray_laser.start = Node()->getPosition() + pos;
     78      ray_laser.end = ray_laser.start + ToIrrlichtScale(range->Value()) * frv;
    8079
    81              // transform pos vector of us
    82             vector3df pos =ToIrrlichtCoordinates(position->Value());
    83             m.transformVect(pos);
    84 
    85             ray_laser.start =Node()->getPosition() + pos;
    86             ray_laser.end = ray_laser.start + ToIrrlichtScale(range->Value())*frv;
    87 
    88             scene::ISceneNode * selectedSceneNode =
    89                 CollMan()->getSceneNodeAndCollisionPointFromRay(ray_laser,intersection_laser,hitTriangle_laser);
    90     // //////////////////////////////////////////
    91             if(selectedSceneNode) //
    92             {
    93                 value[i]=ToSimulatorScale(ray_laser.start.getDistanceFrom(intersection_laser));
    94             }
    95             else
    96             {
    97                 value[i]=-1;
    98             }
    99 
    100         }
    101         shmem->Write((char*)value,360*sizeof(float));
     80      scene::ISceneNode *selectedSceneNode =
     81          CollMan()->getSceneNodeAndCollisionPointFromRay(
     82              ray_laser, intersection_laser, hitTriangle_laser);
     83      // //////////////////////////////////////////
     84      if (selectedSceneNode) //
     85      {
     86        value[i] = ToSimulatorScale(
     87            ray_laser.start.getDistanceFrom(intersection_laser));
     88      } else {
     89        value[i] = -1;
     90      }
    10291    }
     92    shmem->Write((char *)value, 360 * sizeof(float));
     93  }
    10394}
    10495

  • trunk/lib/FlairSimulator/src/SimuLaserGL.h

    r10 r15  
    1717#include <SensorGL.h>
    1818
    19 namespace flair
    20 {
    21     namespace gui
    22     {
    23         class DoubleSpinBox;
    24         class Vector3DSpinBox;
    25     }
     19namespace flair {
     20namespace gui {
     21class DoubleSpinBox;
     22class Vector3DSpinBox;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30     namespace simulator
    31     {
    32         class Model;
    33     }
     26namespace flair {
     27namespace simulator {
     28class Model;
     29}
    3430}
    3531
    36 namespace flair
    37 {
    38 namespace sensor
    39 {
    40     /*! \class SimuUsGL
    41     * \brief Class for a simulation us
    42     *
    43     */
    44     class SimuLaserGL : public SimuLaser, public SensorGL
    45     {
    46         public:
    47             SimuLaserGL(const simulator::Model* parent,std::string name,int dev_id);
    48             ~SimuLaserGL();
     32namespace flair {
     33namespace sensor {
     34/*! \class SimuUsGL
     35* \brief Class for a simulation us
     36*
     37*/
     38class SimuLaserGL : public SimuLaser, public SensorGL {
     39public:
     40  SimuLaserGL(const simulator::Model *parent, std::string name, int dev_id);
     41  ~SimuLaserGL();
    4942
    50         private:
    51             void UpdateFrom(const core::io_data *data);
    52             gui::DoubleSpinBox *range;
    53             gui::Vector3DSpinBox *position,*direction;
    54     };
     43private:
     44  void UpdateFrom(const core::io_data *data);
     45  gui::DoubleSpinBox *range;
     46  gui::Vector3DSpinBox *position, *direction;
     47};
    5548} // end namespace sensor
    5649} // end namespace flair

  • trunk/lib/FlairSimulator/src/SimuUsGL.cpp

    r10 r15  
    3636using namespace flair::simulator;
    3737
    38 namespace flair
    39 {
    40 namespace sensor
    41 {
    42 SimuUsGL::SimuUsGL(const Model* parent,std::string name,int dev_id) :SimuUs(parent,name,dev_id),SensorGL(parent)
    43 {
    44     Tab* setup_tab=new Tab(parent->GetTabWidget(),name);
    45             position=new Vector3DSpinBox(setup_tab->NewRow(),"position",-2,2,.01);
    46             direction=new Vector3DSpinBox(setup_tab->NewRow(),"direction",-2,2,.01);
    47             range=new DoubleSpinBox(setup_tab->NewRow(),"range:",0,6,1);
     38namespace flair {
     39namespace sensor {
     40SimuUsGL::SimuUsGL(const Model *parent, std::string name, int dev_id)
     41    : SimuUs(parent, name, dev_id), SensorGL(parent) {
     42  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
     43  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
     44  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
     45  range = new DoubleSpinBox(setup_tab->NewRow(), "range:", 0, 6, 1);
    4846}
    4947
    50 SimuUsGL::~SimuUsGL()
    51 {
     48SimuUsGL::~SimuUsGL() {}
    5249
    53 }
     50void SimuUsGL::UpdateFrom(const io_data *data) {
     51  float value;
     52#ifdef GL
     53  if (noGui() == true) {
     54#endif
     55    // todo: utiliser le placement de l'us dans le drone et sa portée
     56    cvmatrix *input = (cvmatrix *)data;
     57    value = input->Value(9, 0);
     58    shmem->Write((char *)&value, sizeof(float));
     59#ifdef GL
     60  }
    5461
    55 void SimuUsGL::UpdateFrom(const io_data *data)
    56 {
    57     float value;
    58 #ifdef GL
    59     if(noGui()==true)
    60     {
    61 #endif
    62         //todo: utiliser le placement de l'us dans le drone et sa portée
    63         cvmatrix *input=(cvmatrix*)data;
    64         value=input->Value(9,0);
    65         shmem->Write((char*)&value,sizeof(float));
    66 #ifdef GL
     62  if (noGui() == false && data == NULL) {
     63    line3d<f32> ray_us;         // rayon provenant de l'ultra son
     64    vector3df intersection_us;  // point intersection us avec le sol
     65    triangle3df hitTriangle_us; // triangle intersection us avec le sol
     66
     67    // get rotation matrix of node - Zeuss must be getRotation not
     68    // getRelativeTransformation
     69    matrix4 m;
     70    m.setRotationDegrees(Node()->getRotation());
     71
     72    // transform forward vector of us
     73    vector3df frv = ToIrrlichtCoordinates(direction->Value());
     74    m.transformVect(frv);
     75    frv.normalize();
     76
     77    // transform pos vector of us
     78    vector3df pos = ToIrrlichtCoordinates(position->Value());
     79    m.transformVect(pos);
     80
     81    ray_us.start = Node()->getPosition() + pos;
     82    ray_us.end = ray_us.start + ToIrrlichtScale(range->Value()) * frv;
     83
     84    scene::ISceneNode *selectedSceneNode =
     85        CollMan()->getSceneNodeAndCollisionPointFromRay(ray_us, intersection_us,
     86                                                        hitTriangle_us);
     87
     88    if (selectedSceneNode) {
     89      float value =
     90          ToSimulatorScale(ray_us.start.getDistanceFrom(intersection_us));
     91      shmem->Write((char *)&value, sizeof(float));
    6792    }
    68 
    69     if(noGui()==false && data==NULL)
    70     {
    71         line3d<f32> ray_us;//rayon provenant de l'ultra son
    72         vector3df intersection_us;//point intersection us avec le sol
    73         triangle3df hitTriangle_us;//triangle intersection us avec le sol
    74 
    75          //get rotation matrix of node - Zeuss must be getRotation not getRelativeTransformation
    76         matrix4 m;
    77         m.setRotationDegrees(Node()->getRotation());
    78 
    79         // transform forward vector of us
    80         vector3df frv =ToIrrlichtCoordinates(direction->Value());
    81         m.transformVect(frv);
    82         frv.normalize();
    83 
    84          // transform pos vector of us
    85         vector3df pos =ToIrrlichtCoordinates(position->Value());
    86         m.transformVect(pos);
    87 
    88         ray_us.start =Node()->getPosition() +pos;
    89         ray_us.end = ray_us.start + ToIrrlichtScale(range->Value())*frv;
    90 
    91         scene::ISceneNode * selectedSceneNode =
    92             CollMan()->getSceneNodeAndCollisionPointFromRay(ray_us,intersection_us,hitTriangle_us);
    93 
    94         if(selectedSceneNode)
    95         {
    96             float value=ToSimulatorScale(ray_us.start.getDistanceFrom(intersection_us));
    97             shmem->Write((char*)&value,sizeof(float));
    98         }
    99     }
     93  }
    10094#endif
    10195}

  • trunk/lib/FlairSimulator/src/SimuUsGL.h

    r10 r15  
    1717#include <SensorGL.h>
    1818
    19 namespace flair
    20 {
    21     namespace gui
    22     {
    23         class DoubleSpinBox;
    24         class Vector3DSpinBox;
    25     }
     19namespace flair {
     20namespace gui {
     21class DoubleSpinBox;
     22class Vector3DSpinBox;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30     namespace simulator
    31     {
    32         class Model;
    33     }
     26namespace flair {
     27namespace simulator {
     28class Model;
     29}
    3430}
    3531
    36 namespace flair
    37 {
    38 namespace sensor
    39 {
    40     /*! \class SimuUsGL
    41     * \brief Class for a simulation us
    42     *
    43     */
    44     class SimuUsGL : public SimuUs, public SensorGL
    45     {
    46         public:
    47             SimuUsGL(const simulator::Model* parent,std::string name,int dev_id);
    48             ~SimuUsGL();
     32namespace flair {
     33namespace sensor {
     34/*! \class SimuUsGL
     35* \brief Class for a simulation us
     36*
     37*/
     38class SimuUsGL : public SimuUs, public SensorGL {
     39public:
     40  SimuUsGL(const simulator::Model *parent, std::string name, int dev_id);
     41  ~SimuUsGL();
    4942
    50         private:
    51             void UpdateFrom(const core::io_data *data);
    52             gui::DoubleSpinBox *range;
    53             gui::Vector3DSpinBox *position,*direction;
    54     };
     43private:
     44  void UpdateFrom(const core::io_data *data);
     45  gui::DoubleSpinBox *range;
     46  gui::Vector3DSpinBox *position, *direction;
     47};
    5548} // end namespace sensor
    5649} // end namespace flair

  • trunk/lib/FlairSimulator/src/Simulator.cpp

    r10 r15  
    2727using namespace flair::core;
    2828
    29 namespace
    30 {
    31     flair::simulator::Simulator* simu=NULL;
     29namespace {
     30flair::simulator::Simulator *simu = NULL;
    3231}
    3332
    34 namespace flair
    35 {
    36 namespace simulator
    37 {
     33namespace flair {
     34namespace simulator {
    3835
    39 Simulator* getSimulator(void) {
    40     return simu;
     36Simulator *getSimulator(void) { return simu; }
     37
     38Simulator::Simulator(string name, int optitrack_mstime, float yaw_deg)
     39    : FrameworkManager(name) {
     40  if (simu != NULL)
     41    Err("Simulator should be instanced only one time\n");
     42
     43  pimpl_ = new Simulator_impl(this, optitrack_mstime, yaw_deg);
     44  simu = this;
    4145}
    4246
    43 Simulator::Simulator(string name,int optitrack_mstime,float yaw_deg): FrameworkManager(name)
    44 {
    45     if(simu!=NULL) Err("Simulator should be instanced only one time\n");
    46 
    47     pimpl_=new Simulator_impl(this,optitrack_mstime,yaw_deg);
    48     simu=this;
    49 }
    50 
    51 Simulator::~Simulator()
    52 {
    53     delete pimpl_;
    54 }
     47Simulator::~Simulator() { delete pimpl_; }
    5548
    5649Quaternion Simulator::ToVRPNReference(Quaternion quat_in) {
    57     Quaternion yaw_rot_quat;
    58     Euler yaw_rot_euler(0,0,-pimpl_->yaw_rad);//yaw_rad is vrpn rotation in earth reference
    59     yaw_rot_euler.ToQuaternion(yaw_rot_quat);
     50  Quaternion yaw_rot_quat;
     51  Euler yaw_rot_euler(
     52      0, 0, -pimpl_->yaw_rad); // yaw_rad is vrpn rotation in earth reference
     53  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
    6054
    61     return yaw_rot_quat*quat_in;
     55  return yaw_rot_quat * quat_in;
    6256}
    6357
    6458Vector3D Simulator::ToVRPNReference(Vector3D point_in) {
    65     Quaternion yaw_rot_quat;
    66     Euler yaw_rot_euler(0,0,-pimpl_->yaw_rad);//yaw_rad is vrpn rotation in earth reference
    67     yaw_rot_euler.ToQuaternion(yaw_rot_quat);
    68     point_in.Rotate(yaw_rot_quat);
     59  Quaternion yaw_rot_quat;
     60  Euler yaw_rot_euler(
     61      0, 0, -pimpl_->yaw_rad); // yaw_rad is vrpn rotation in earth reference
     62  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
     63  point_in.Rotate(yaw_rot_quat);
    6964
    70     return point_in;
     65  return point_in;
    7166}
    7267
    73 float Simulator::Yaw(void) const
    74 {
    75     return pimpl_->yaw_rad;
    76 }
     68float Simulator::Yaw(void) const { return pimpl_->yaw_rad; }
    7769
    78 void Simulator::RunSimu(void)
    79 {
    80     pimpl_->RunSimu();
    81 }
     70void Simulator::RunSimu(void) { pimpl_->RunSimu(); }
    8271
    8372} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Simulator.h

    r10 r15  
    2424
    2525namespace flair {
    26     namespace core {
    27         class Quaternion;
    28         class Vector3D;
    29     }
     26namespace core {
     27class Quaternion;
     28class Vector3D;
     29}
    3030}
    3131
    32 namespace flair
    33 {
    34 namespace simulator
    35 {
    36     class Model;
    37     class Gui;
     32namespace flair {
     33namespace simulator {
     34class Model;
     35class Gui;
    3836
    39     class Simulator: public core::FrameworkManager
    40     {
    41         friend class Model;
    42         friend class Gui;
    43         friend class GenericObject;
     37class Simulator : public core::FrameworkManager {
     38  friend class Model;
     39  friend class Gui;
     40  friend class GenericObject;
    4441
    45         public:
    46             //yaw_deg: rotation of the vrpn coordinate with respect to the earth coordinate, around z axis
    47             Simulator(std::string name,int optitrack_mstime=10,float yaw_deg=30);
    48             ~Simulator();
    49             void RunSimu(void);
    50             //get rotation of the vrpn coordinate with respect to the earth coordinate, around z axis; in radians
    51             float Yaw(void) const;
    52             //compute rotation of the vrpn coordinate with respect to the earth coordinate, around z axis
    53             core::Quaternion ToVRPNReference(core::Quaternion quat_in);
    54             core::Vector3D ToVRPNReference(core::Vector3D point_in);
     42public:
     43  // yaw_deg: rotation of the vrpn coordinate with respect to the earth
     44  // coordinate, around z axis
     45  Simulator(std::string name, int optitrack_mstime = 10, float yaw_deg = 30);
     46  ~Simulator();
     47  void RunSimu(void);
     48  // get rotation of the vrpn coordinate with respect to the earth coordinate,
     49  // around z axis; in radians
     50  float Yaw(void) const;
     51  // compute rotation of the vrpn coordinate with respect to the earth
     52  // coordinate, around z axis
     53  core::Quaternion ToVRPNReference(core::Quaternion quat_in);
     54  core::Vector3D ToVRPNReference(core::Vector3D point_in);
    5555
    56         private:
    57             Simulator_impl* pimpl_;
    58     };
     56private:
     57  Simulator_impl *pimpl_;
     58};
    5959
    60     /*!
    61     * \brief get Simulator
    62     *
    63     * \return the Simulator
    64     */
    65     Simulator* getSimulator(void);
     60/*!
     61* \brief get Simulator
     62*
     63* \return the Simulator
     64*/
     65Simulator *getSimulator(void);
    6666
    6767} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Simulator_impl.cpp

    r10 r15  
    2929using namespace flair::simulator;
    3030
    31 Simulator_impl::Simulator_impl(Simulator* self,int optitrack_mstime,float yaw_deg): vrpn_Connection_IP(), Thread(self,"simulator",1)
    32 {
    33     this->self=self;
    34     this->optitrack_mstime=optitrack_mstime;
    35     yaw_rad=Euler::ToRadian(yaw_deg);
     31Simulator_impl::Simulator_impl(Simulator *self, int optitrack_mstime,
     32                               float yaw_deg)
     33    : vrpn_Connection_IP(), Thread(self, "simulator", 1) {
     34  this->self = self;
     35  this->optitrack_mstime = optitrack_mstime;
     36  yaw_rad = Euler::ToRadian(yaw_deg);
    3637}
    3738
    38 Simulator_impl::~Simulator_impl()
    39 {
    40     //printf("del Simulator_impl\n");
     39Simulator_impl::~Simulator_impl() {
     40  // printf("del Simulator_impl\n");
    4141
    42     SafeStop();
    43     Join();
     42  SafeStop();
     43  Join();
    4444
    45     for(size_t i=0;i<models.size();i++)
    46     {
    47         models.at(i)->pimpl_->SafeStop();
    48         models.at(i)->pimpl_->Join();
    49         delete models.at(i);
    50     }
     45  for (size_t i = 0; i < models.size(); i++) {
     46    models.at(i)->pimpl_->SafeStop();
     47    models.at(i)->pimpl_->Join();
     48    delete models.at(i);
     49  }
    5150
    5251#ifdef GL
    53     if(getGui()!=NULL) delete getGui();
     52  if (getGui() != NULL)
     53    delete getGui();
    5454#endif
    5555
    56      //printf("del Simulator_impl ok\n");
     56  // printf("del Simulator_impl ok\n");
    5757}
    5858
    59 void Simulator_impl::Run(void)
    60 {
    61     SetPeriodMS(optitrack_mstime);
     59void Simulator_impl::Run(void) {
     60  SetPeriodMS(optitrack_mstime);
    6261
    63     while(ToBeStopped()==false)
    64         {
    65             WaitPeriod();
    66 //printf("%lld\n",GetTime());
    67         mainloop();
    68         for(size_t i=0;i<models.size();i++)
    69         {
    70             models.at(i)->pimpl_->mainloop();
    71         }
    72         }
     62  while (ToBeStopped() == false) {
     63    WaitPeriod();
     64    // printf("%lld\n",GetTime());
     65    mainloop();
     66    for (size_t i = 0; i < models.size(); i++) {
     67      models.at(i)->pimpl_->mainloop();
     68    }
     69  }
    7370}
    7471
     72void Simulator_impl::RunSimu(void) {
     73  if (models.size() == 0) {
     74    self->Err("No model to run\n");
     75    return;
     76  }
    7577
    76 void Simulator_impl::RunSimu(void)
    77 {
    78     if(models.size()==0)
    79     {
    80         self->Err("No model to run\n");
    81         return;
    82     }
     78  for (size_t i = 0; i < models.size(); i++) {
     79    models.at(i)->pimpl_->Start();
     80  }
    8381
    84     for(size_t i=0;i<models.size();i++)
    85     {
    86         models.at(i)->pimpl_->Start();
    87     }
    88 
    89     Start();
     82  Start();
    9083
    9184#ifdef GL
    92     if(getGui()!=NULL)
    93     {
    94         getGui()->pimpl_->RunGui(models,objects);
    95     }
    96     else
     85  if (getGui() != NULL) {
     86    getGui()->pimpl_->RunGui(models, objects);
     87  } else
    9788#endif
    98     {
    99         models.at(0)->pimpl_->Join();
    100     }
     89  {
     90    models.at(0)->pimpl_->Join();
     91  }
    10192
    102     SafeStop();
    103     Join();
     93  SafeStop();
     94  Join();
    10495}

  • trunk/lib/FlairSimulator/src/X4.cpp

    r10 r15  
    3131#endif
    3232
    33 #define K_MOT 0.4f //blade animation
    34 #define G (float)9.81 //gravity ( N/(m/s²) )
     33#define K_MOT 0.4f    // blade animation
     34#define G (float)9.81 // gravity ( N/(m/s²) )
    3535
    3636#ifdef GL
    … …  
    4343using namespace flair::actuator;
    4444
    45 namespace flair
    46 {
    47 namespace simulator
    48 {
    49 
    50 X4::X4(const Simulator* parent,std::string name, int dev_id): Model(parent,name)
    51 {
    52     Tab *setup_tab=new Tab(GetTabWidget(),"model");
    53         m=new DoubleSpinBox(setup_tab->NewRow(),"mass (kg):",0,20,0.1);
    54         arm_length=new DoubleSpinBox(setup_tab->LastRowLastCol(),"arm length (m):",0,2,0.1);
    55         //l_cg=new DoubleSpinBox(setup_tab,"position G (m):",0,2,-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
    56         k_mot=new DoubleSpinBox(setup_tab->NewRow(),"k_mot:",0,1,0.001,3);// vitesse rotation² (unité arbitraire) -> force (N)
    57         c_mot=new DoubleSpinBox(setup_tab->LastRowLastCol(),"c_mot:",0,1,0.001,3);// vitesse rotation moteur -> couple (N.m/unité arbitraire)
    58         f_air_vert=new DoubleSpinBox(setup_tab->NewRow(),"f_air_vert:",0,10,1);//frottements air depl. vertical, aussi utilisé pour les rotations ( N/(m/s) ) (du aux helices en rotation)
    59         f_air_lat=new DoubleSpinBox(setup_tab->LastRowLastCol(),"f_air_lat:",0,10,1);//frottements air deplacements lateraux ( N/(m/s) )
    60         j_roll=new DoubleSpinBox(setup_tab->NewRow(),"j_roll:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    61         j_pitch=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_pitch:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    62         j_yaw=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_yaw:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    63 
    64     motors=new SimuBldc(this,name,4,dev_id);
    65 }
    66 
    67 X4::~X4()
    68 {
    69     //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
    70 }
    71 
    72 
    73 #ifdef GL
    74 
    75 void X4::Draw(void)
    76 {
    77     //create unite (1m=100cm) UAV; scale will be adapted according to arm_length parameter
    78     //note that the frame used is irrlicht one:
    79     //left handed, North East Up
    80     const IGeometryCreator *geo;
    81     geo=getGui()->getSceneManager()->getGeometryCreator();
    82 
    83     //cylinders are aligned with y axis
    84     red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
    85     black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
    86     motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
    87     //geo->drop();
    88 
    89     ITexture* texture=getGui()->getTexture("carbone.jpg");
    90     fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
    91     fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
    92     rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
    93     rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
    94 
    95     texture=getGui()->getTexture("metal047.jpg");
    96     fl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
    97     fr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
    98     rl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
    99     rr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
    100 
    101     fl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
    102     fr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
    103     rl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
    104     rr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
    105 
    106     motor_speed_mutex=new Mutex(this);
    107     for(int i=0;i<4;i++) motor_speed[i]=0;
    108     ExtraDraw();
    109 }
    110 
    111 void X4::AnimateModel(void)
    112 {
    113     motor_speed_mutex->GetMutex();
    114     fl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
    115     fr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
    116     rl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
    117     rr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
    118     motor_speed_mutex->ReleaseMutex();
    119 
    120     //adapt UAV size
    121     if(arm_length->ValueChanged()==true)
    122     {
    123         setScale(arm_length->Value());
    124     }
    125 }
    126 
    127 size_t X4::dbtSize(void) const
    128 {
    129     return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
    130 }
    131 
    132 void X4::WritedbtBuf(char* dbtbuf)
    133 {/*
    134     float *buf=(float*)dbtbuf;
    135     vector3df vect=getPosition();
    136     memcpy(buf,&vect.X,sizeof(float));
    137     buf++;
    138     memcpy(buf,&vect.Y,sizeof(float));
    139     buf++;
    140     memcpy(buf,&vect.Z,sizeof(float));
    141     buf++;
    142     vect=getRotation();
    143     memcpy(buf,&vect.X,sizeof(float));
    144     buf++;
    145     memcpy(buf,&vect.Y,sizeof(float));
    146     buf++;
    147     memcpy(buf,&vect.Z,sizeof(float));
    148     buf++;
    149     memcpy(buf,&motors,sizeof(rtsimu_motors));*/
    150 }
    151 
    152 void X4::ReaddbtBuf(char* dbtbuf)
    153 {/*
    154     float *buf=(float*)dbtbuf;
    155     vector3df vect;
    156     memcpy(&vect.X,buf,sizeof(float));
    157     buf++;
    158     memcpy(&vect.Y,buf,sizeof(float));
    159     buf++;
    160     memcpy(&vect.Z,buf,sizeof(float));
    161     buf++;
    162     setPosition(vect);
    163     memcpy(&vect.X,buf,sizeof(float));
    164     buf++;
    165     memcpy(&vect.Y,buf,sizeof(float));
    166     buf++;
    167     memcpy(&vect.Z,buf,sizeof(float));
    168     buf++;
    169     ((ISceneNode*)(this))->setRotation(vect);
    170     memcpy(&motors,buf,sizeof(rtsimu_motors));
    171     AnimateModele();*/
    172 }
    173 #endif //GL
    174 
    175 //states are computed on fixed frame NED
    176 //x north
    177 //y east
    178 //z down
    179 void X4::CalcModel(void)
    180 {
    181     float fl_speed,fr_speed,rl_speed,rr_speed;
    182     float u_roll,u_pitch,u_yaw,u_thrust;
    183 #ifdef GL
    184     motor_speed_mutex->GetMutex();
    185 #endif //GL
    186     motors->GetSpeeds(motor_speed);
    187 #ifdef GL
    188     motor_speed_mutex->ReleaseMutex();
    189 #endif //GL
    190         fl_speed=motor_speed[0];
    191         fr_speed=motor_speed[1];
    192         rl_speed=motor_speed[2];
    193         rr_speed=motor_speed[3];
    194 
    195     /*
    196         ** ===================================================================
    197         **    u roll: roll torque
    198         **
    199         ** ===================================================================
    200         */
    201         u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    202 
    203     /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed of the quadri in the body frame). It is a discrete integrator
    204         state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z + u_roll) +state[-1].W.x;
    205 
    206         //u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    207         //state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
    208 
    209         /*
    210         ** ===================================================================
    211         **   u pitch : pitch torque
    212         **
    213         ** ===================================================================
    214         */
    215         u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    216 
    217     /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed of the quadri in the body frame). It is a discrete integrator
    218         state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z + u_pitch)+state[-1].W.y;
    219 
    220         //u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    221         //state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
    222 
    223         /*
    224         ** ===================================================================
    225         **    u yaw : yaw torque
    226         **
    227         ** ===================================================================
    228         */
    229         u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
    230 
    231     /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed of the quadri in the body frame). It is a discrete integrator
    232         state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;
    233 
    234         //u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
    235         //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
    236 
    237     // compute quaternion from W
    238     // Quaternion derivative: dQ = 0.5*(Q*Qw)
    239     Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
    240 
    241     // Quaternion integration
    242     state[0].Quat = state[-1].Quat +dQ*dT();
    243     state[0].Quat.Normalize();
    244 
    245     // Calculation of the thrust from the reference speed of motors
    246         u_thrust=k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed+rl_speed*rl_speed+rr_speed*rr_speed);
    247     Vector3D vect(0,0,-u_thrust);
    248     vect.Rotate(state[0].Quat);
    249 
    250     /*
    251         ** ===================================================================
    252         **     x double integrator
    253         **
    254         ** ===================================================================
    255         */
    256         state[0].Pos.x=(dT()*dT()/m->Value())*(vect.x-f_air_lat->Value()*(state[-1].Pos.x-state[-2].Pos.x)/dT())+2*state[-1].Pos.x-state[-2].Pos.x;
    257     state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
    258 
    259         /*
    260         ** ===================================================================
    261         **     y double integrator
    262         **
    263         ** ===================================================================
    264         */
    265         state[0].Pos.y=(dT()*dT()/m->Value())*(vect.y-f_air_lat->Value()*(state[-1].Pos.y-state[-2].Pos.y)/dT())+2*state[-1].Pos.y-state[-2].Pos.y;
    266     state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
    267 
    268         /*
    269         ** ===================================================================
    270         **     z double integrator
    271         **
    272         ** ===================================================================
    273         */
    274         state[0].Pos.z=(dT()*dT()/m->Value())*(vect.z+f_air_vert->Value()*(state[-1].Pos.z-state[-2].Pos.z)/dT()+m->Value()*G)+2*state[-1].Pos.z-state[-2].Pos.z;
    275     state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
     45namespace flair {
     46namespace simulator {
     47
     48X4::X4(const Simulator *parent, std::string name, int dev_id)
     49    : Model(parent, name) {
     50  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     51  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
     52  arm_length = new DoubleSpinBox(setup_tab->LastRowLastCol(), "arm length (m):",
     53                                 0, 2, 0.1);
     54  // l_cg=new DoubleSpinBox(setup_tab,"position G
     55  // (m):",0,2,-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
     56  k_mot =
     57      new DoubleSpinBox(setup_tab->NewRow(), "k_mot:", 0, 1, 0.001,
     58                        3); // vitesse rotation² (unité arbitraire) -> force (N)
     59  c_mot = new DoubleSpinBox(
     60      setup_tab->LastRowLastCol(), "c_mot:", 0, 1, 0.001,
     61      3); // vitesse rotation moteur -> couple (N.m/unité arbitraire)
     62  f_air_vert = new DoubleSpinBox(setup_tab->NewRow(), "f_air_vert:", 0, 10,
     63                                 1); // frottements air depl. vertical, aussi
     64                                     // utilisé pour les rotations ( N/(m/s) )
     65                                     // (du aux helices en rotation)
     66  f_air_lat =
     67      new DoubleSpinBox(setup_tab->LastRowLastCol(), "f_air_lat:", 0, 10,
     68                        1); // frottements air deplacements lateraux ( N/(m/s) )
     69  j_roll = new DoubleSpinBox(setup_tab->NewRow(), "j_roll:", 0, 1, 0.001,
     70                             5); // moment d'inertie d'un axe (N.m.s²/rad)
     71  j_pitch =
     72      new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_pitch:", 0, 1, 0.001,
     73                        5); // moment d'inertie d'un axe (N.m.s²/rad)
     74  j_yaw = new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_yaw:", 0, 1, 0.001,
     75                            5); // moment d'inertie d'un axe (N.m.s²/rad)
     76
     77  motors = new SimuBldc(this, name, 4, dev_id);
     78}
     79
     80X4::~X4() {
     81  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
     82  // pales) par parenté
     83}
     84
     85#ifdef GL
     86
     87void X4::Draw(void) {
     88  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
     89  // parameter
     90  // note that the frame used is irrlicht one:
     91  // left handed, North East Up
     92  const IGeometryCreator *geo;
     93  geo = getGui()->getSceneManager()->getGeometryCreator();
     94
     95  // cylinders are aligned with y axis
     96  red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
     97  black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
     98  motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
     99  // geo->drop();
     100
     101  ITexture *texture = getGui()->getTexture("carbone.jpg");
     102  fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     103                             vector3df(0, 0, -135));
     104  fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     105                             vector3df(0, 0, -45));
     106  rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     107                             vector3df(0, 0, 135), texture);
     108  rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     109                             vector3df(0, 0, 45), texture);
     110
     111  texture = getGui()->getTexture("metal047.jpg");
     112  fl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
     113                               vector3df(90, 0, 0), texture);
     114  fr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
     115                               vector3df(90, 0, 0), texture);
     116  rl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
     117                               vector3df(90, 0, 0), texture);
     118  rr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
     119                               vector3df(90, 0, 0), texture);
     120
     121  fl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
     122  fr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);
     123  rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);
     124  rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
     125
     126  motor_speed_mutex = new Mutex(this);
     127  for (int i = 0; i < 4; i++)
     128    motor_speed[i] = 0;
     129  ExtraDraw();
     130}
     131
     132void X4::AnimateModel(void) {
     133  motor_speed_mutex->GetMutex();
     134  fl_blade->SetRotationSpeed(K_MOT * motor_speed[0]);
     135  fr_blade->SetRotationSpeed(-K_MOT * motor_speed[1]);
     136  rl_blade->SetRotationSpeed(-K_MOT * motor_speed[2]);
     137  rr_blade->SetRotationSpeed(K_MOT * motor_speed[3]);
     138  motor_speed_mutex->ReleaseMutex();
     139
     140  // adapt UAV size
     141  if (arm_length->ValueChanged() == true) {
     142    setScale(arm_length->Value());
     143  }
     144}
     145
     146size_t X4::dbtSize(void) const {
     147  return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices
     148}
     149
     150void X4::WritedbtBuf(
     151    char *dbtbuf) { /*
     152                       float *buf=(float*)dbtbuf;
     153                       vector3df vect=getPosition();
     154                       memcpy(buf,&vect.X,sizeof(float));
     155                       buf++;
     156                       memcpy(buf,&vect.Y,sizeof(float));
     157                       buf++;
     158                       memcpy(buf,&vect.Z,sizeof(float));
     159                       buf++;
     160                       vect=getRotation();
     161                       memcpy(buf,&vect.X,sizeof(float));
     162                       buf++;
     163                       memcpy(buf,&vect.Y,sizeof(float));
     164                       buf++;
     165                       memcpy(buf,&vect.Z,sizeof(float));
     166                       buf++;
     167                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
     168}
     169
     170void X4::ReaddbtBuf(
     171    char *dbtbuf) { /*
     172                       float *buf=(float*)dbtbuf;
     173                       vector3df vect;
     174                       memcpy(&vect.X,buf,sizeof(float));
     175                       buf++;
     176                       memcpy(&vect.Y,buf,sizeof(float));
     177                       buf++;
     178                       memcpy(&vect.Z,buf,sizeof(float));
     179                       buf++;
     180                       setPosition(vect);
     181                       memcpy(&vect.X,buf,sizeof(float));
     182                       buf++;
     183                       memcpy(&vect.Y,buf,sizeof(float));
     184                       buf++;
     185                       memcpy(&vect.Z,buf,sizeof(float));
     186                       buf++;
     187                       ((ISceneNode*)(this))->setRotation(vect);
     188                       memcpy(&motors,buf,sizeof(rtsimu_motors));
     189                       AnimateModele();*/
     190}
     191#endif // GL
     192
     193// states are computed on fixed frame NED
     194// x north
     195// y east
     196// z down
     197void X4::CalcModel(void) {
     198  float fl_speed, fr_speed, rl_speed, rr_speed;
     199  float u_roll, u_pitch, u_yaw, u_thrust;
     200#ifdef GL
     201  motor_speed_mutex->GetMutex();
     202#endif // GL
     203  motors->GetSpeeds(motor_speed);
     204#ifdef GL
     205  motor_speed_mutex->ReleaseMutex();
     206#endif // GL
     207  fl_speed = motor_speed[0];
     208  fr_speed = motor_speed[1];
     209  rl_speed = motor_speed[2];
     210  rr_speed = motor_speed[3];
     211
     212  /*
     213      ** ===================================================================
     214      **    u roll: roll torque
     215      **
     216      ** ===================================================================
     217      */
     218  u_roll = arm_length->Value() * k_mot->Value() *
     219           (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
     220            rr_speed * rr_speed) *
     221           sqrtf(2) / 2;
     222
     223  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
     224  /// of the quadri in the body frame). It is a discrete integrator
     225  state[0].W.x =
     226      (dT() / j_roll->Value()) *
     227          ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
     228           u_roll) +
     229      state[-1].W.x;
     230
     231  // u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
     232  // state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
     233
     234  /*
     235  ** ===================================================================
     236  **   u pitch : pitch torque
     237  **
     238  ** ===================================================================
     239  */
     240  u_pitch = arm_length->Value() * k_mot->Value() *
     241            (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
     242             rr_speed * rr_speed) *
     243            sqrtf(2) / 2;
     244
     245  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
     246  /// of the quadri in the body frame). It is a discrete integrator
     247  state[0].W.y =
     248      (dT() / j_pitch->Value()) *
     249          ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
     250           u_pitch) +
     251      state[-1].W.y;
     252
     253  // u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
     254  // state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
     255
     256  /*
     257  ** ===================================================================
     258  **    u yaw : yaw torque
     259  **
     260  ** ===================================================================
     261  */
     262  u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
     263                            fr_speed * fr_speed - rl_speed * rl_speed);
     264
     265  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
     266  /// of the quadri in the body frame). It is a discrete integrator
     267  state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
     268
     269  // u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
     270  // state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
     271
     272  // compute quaternion from W
     273  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     274  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     275
     276  // Quaternion integration
     277  state[0].Quat = state[-1].Quat + dQ * dT();
     278  state[0].Quat.Normalize();
     279
     280  // Calculation of the thrust from the reference speed of motors
     281  u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
     282                               rl_speed * rl_speed + rr_speed * rr_speed);
     283  Vector3D vect(0, 0, -u_thrust);
     284  vect.Rotate(state[0].Quat);
     285
     286  /*
     287      ** ===================================================================
     288      **     x double integrator
     289      **
     290      ** ===================================================================
     291      */
     292  state[0].Pos.x =
     293      (dT() * dT() / m->Value()) *
     294          (vect.x -
     295           f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
     296      2 * state[-1].Pos.x - state[-2].Pos.x;
     297  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
     298
     299  /*
     300  ** ===================================================================
     301  **     y double integrator
     302  **
     303  ** ===================================================================
     304  */
     305  state[0].Pos.y =
     306      (dT() * dT() / m->Value()) *
     307          (vect.y -
     308           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
     309      2 * state[-1].Pos.y - state[-2].Pos.y;
     310  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
     311
     312  /*
     313  ** ===================================================================
     314  **     z double integrator
     315  **
     316  ** ===================================================================
     317  */
     318  state[0].Pos.z =
     319      (dT() * dT() / m->Value()) *
     320          (vect.z +
     321           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
     322           m->Value() * G) +
     323      2 * state[-1].Pos.z - state[-2].Pos.z;
     324  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    276325
    277326#ifndef GL
    278     if(state[0].Pos.z<0) state[0].Pos.z=0;
     327  if (state[0].Pos.z < 0)
     328    state[0].Pos.z = 0;
    279329#endif
    280 
    281330}
    282331

  • trunk/lib/FlairSimulator/src/X4.h

    r10 r15  
    2222#include <stdio.h>
    2323
    24 namespace flair
    25 {
    26     namespace core
    27     {
    28         class Mutex;
    29     }
    30     namespace gui
    31     {
    32         class DoubleSpinBox;
    33     }
    34     namespace actuator
    35     {
    36         class SimuBldc;
    37     }
     24namespace flair {
     25namespace core {
     26class Mutex;
     27}
     28namespace gui {
     29class DoubleSpinBox;
     30}
     31namespace actuator {
     32class SimuBldc;
     33}
    3834}
    3935
    4036#ifdef GL
    41 namespace irr
    42 {
    43     namespace scene
    44     {
    45         class IMesh;
    46     }
     37namespace irr {
     38namespace scene {
     39class IMesh;
     40}
    4741}
    4842#endif
    4943
    50 namespace flair
    51 {
    52 namespace simulator
    53 {
    54     class Simulator;
    55     class Blade;
    56     class MeshSceneNode;
     44namespace flair {
     45namespace simulator {
     46class Simulator;
     47class Blade;
     48class MeshSceneNode;
    5749
    58     class X4 : public Model
    59     {
    60         public:
    61             X4(const Simulator* parent,std::string name, int dev_id);
    62             ~X4();
     50class X4 : public Model {
     51public:
     52  X4(const Simulator *parent, std::string name, int dev_id);
     53  ~X4();
    6354#ifdef GL
    64             virtual void Draw(void);
    65             virtual void ExtraDraw(void){};
     55  virtual void Draw(void);
     56  virtual void ExtraDraw(void){};
    6657
    67         protected:
    68                     irr::scene::IMesh *red_arm,*black_arm,*motor;
    69                     MeshSceneNode *fl_arm,*fr_arm,*rl_arm,*rr_arm;
    70                     MeshSceneNode *fl_motor,*fr_motor,*rl_motor,*rr_motor;
    71                     Blade *fl_blade,*fr_blade,*rl_blade,*rr_blade;
    72                     core::Mutex *motor_speed_mutex;
     58protected:
     59  irr::scene::IMesh *red_arm, *black_arm, *motor;
     60  MeshSceneNode *fl_arm, *fr_arm, *rl_arm, *rr_arm;
     61  MeshSceneNode *fl_motor, *fr_motor, *rl_motor, *rr_motor;
     62  Blade *fl_blade, *fr_blade, *rl_blade, *rr_blade;
     63  core::Mutex *motor_speed_mutex;
    7364#endif
    74         private:
    75             void CalcModel(void);
    76     #ifdef GL
    77             bool OnEvent(const irr::SEvent& event){};
    78             void AnimateModel(void);
    79             size_t dbtSize(void) const;
    80             void WritedbtBuf(char* dbtbuf);
    81             void ReaddbtBuf(char* dbtbuf);
    82     #endif
     65private:
     66  void CalcModel(void);
     67#ifdef GL
     68  bool OnEvent(const irr::SEvent &event){};
     69  void AnimateModel(void);
     70  size_t dbtSize(void) const;
     71  void WritedbtBuf(char *dbtbuf);
     72  void ReaddbtBuf(char *dbtbuf);
     73#endif
    8374
    84             actuator::SimuBldc *motors;
    85             float motor_speed[4];
    86             gui::DoubleSpinBox *m,*arm_length,*l_cg;
    87             gui::DoubleSpinBox *k_mot,*c_mot;
    88             gui::DoubleSpinBox *f_air_vert,*f_air_lat;
    89             gui::DoubleSpinBox *j_roll,*j_pitch,*j_yaw;
    90     };
     75  actuator::SimuBldc *motors;
     76  float motor_speed[4];
     77  gui::DoubleSpinBox *m, *arm_length, *l_cg;
     78  gui::DoubleSpinBox *k_mot, *c_mot;
     79  gui::DoubleSpinBox *f_air_vert, *f_air_lat;
     80  gui::DoubleSpinBox *j_roll, *j_pitch, *j_yaw;
     81};
    9182} // end namespace simulator
    9283} // end namespace flair

  • trunk/lib/FlairSimulator/src/X8.cpp

    r10 r15  
    3131#endif
    3232
    33 #define K_MOT 0.4f //blade animation
    34 #define G (float)9.81 //gravity ( N/(m/s²) )
     33#define K_MOT 0.4f    // blade animation
     34#define G (float)9.81 // gravity ( N/(m/s²) )
    3535
    3636#ifdef GL
    … …  
    4343using namespace flair::actuator;
    4444
    45 namespace flair
    46 {
    47 namespace simulator
    48 {
    49 
    50 X8::X8(const Simulator* parent,std::string name, int dev_id): Model(parent,name)
    51 {
    52     Tab *setup_tab=new Tab(GetTabWidget(),"model");
    53         m=new DoubleSpinBox(setup_tab->NewRow(),"mass (kg):",0,20,0.1);
    54         arm_length=new DoubleSpinBox(setup_tab->LastRowLastCol(),"arm length (m):",0,2,0.1);
    55         l_cg=new DoubleSpinBox(setup_tab->LastRowLastCol(),"position G (m):",-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
    56         k_mot=new DoubleSpinBox(setup_tab->NewRow(),"k_mot:",0,1,0.001,3);// vitesse rotation² (unité arbitraire) -> force (N)
    57         c_mot=new DoubleSpinBox(setup_tab->LastRowLastCol(),"c_mot:",0,1,0.001,3);// vitesse rotation moteur -> couple (N.m/unité arbitraire)
    58         f_air_vert=new DoubleSpinBox(setup_tab->NewRow(),"f_air_vert:",0,10,1);//frottements air depl. vertical, aussi utilisé pour les rotations ( N/(m/s) ) (du aux helices en rotation)
    59         f_air_lat=new DoubleSpinBox(setup_tab->LastRowLastCol(),"f_air_lat:",0,10,1);//frottements air deplacements lateraux ( N/(m/s) )
    60         j_roll=new DoubleSpinBox(setup_tab->NewRow(),"j_roll:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    61         j_pitch=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_pitch:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    62         j_yaw=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_yaw:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    63         j_r=new DoubleSpinBox(setup_tab->NewRow(),"j_r:",0,1,0.001);// moment des helices (N.m.s²/rad)
    64         sigma=new DoubleSpinBox(setup_tab->LastRowLastCol(),"sigma:",0,1,0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
    65         S=new DoubleSpinBox(setup_tab->LastRowLastCol(),"S:",1,2,0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
    66 
    67     motors=new SimuBldc(this,name,8,dev_id);
    68 }
    69 
    70 void X8::Draw(){
    71 #ifdef GL
    72 
    73         //create unite (1m=100cm) UAV; scale will be adapted according to arm_length parameter
    74     //note that the frame used is irrlicht one:
    75     //left handed, North East Up
    76 
    77     const IGeometryCreator *geo;
    78     geo=getGui()->getSceneManager()->getGeometryCreator();
    79 
    80     //cylinders are aligned with y axis
    81     red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
    82     black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
    83     motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
    84     //geo->drop();
    85 
    86     ITexture* texture=getGui()->getTexture("carbone.jpg");
    87     fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
    88     fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
    89     rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
    90     rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
    91 
    92     texture=getGui()->getTexture("metal047.jpg");
    93     tfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
    94     tfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
    95     trl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
    96     trr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
    97 
    98     bfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,-17.5),vector3df(90,0,0),texture);
    99     bfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,-17.5),vector3df(90,0,0),texture);
    100     brl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,-17.5),vector3df(90,0,0),texture);
    101     brr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,-17.5),vector3df(90,0,0),texture);
    102 
    103     tfl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
    104     tfr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
    105     trl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
    106     trr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
    107 
    108     bfl_blade=new Blade(this, vector3df(70.71,-70.71,-17.5));
    109     bfr_blade=new Blade(this, vector3df(70.71,70.71,-17.5),true);
    110     brl_blade=new Blade(this, vector3df(-70.71,-70.71,-17.5),true);
    111     brr_blade=new Blade(this, vector3df(-70.71,70.71,-17.5));
    112 
    113     motor_speed_mutex=new Mutex(this);
    114     for(int i=0;i<8;i++) motor_speed[i]=0;
    115     ExtraDraw();
    116     #endif
    117 }
    118 
    119 X8::~X8()
    120 {
    121     //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
    122 }
    123 
    124 #ifdef GL
    125 void X8::AnimateModel(void)
    126 {
    127     motor_speed_mutex->GetMutex();
    128     tfl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
    129     tfr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
    130     trl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
    131     trr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
    132 
    133     bfl_blade->SetRotationSpeed(-K_MOT*motor_speed[4]);
    134     bfr_blade->SetRotationSpeed(K_MOT*motor_speed[5]);
    135     brl_blade->SetRotationSpeed(K_MOT*motor_speed[6]);
    136     brr_blade->SetRotationSpeed(-K_MOT*motor_speed[7]);
    137     motor_speed_mutex->ReleaseMutex();
    138 
    139     //adapt UAV size
    140     if(arm_length->ValueChanged()==true)
    141     {
    142         setScale(arm_length->Value());
    143     }
    144 }
    145 
    146 size_t X8::dbtSize(void) const
    147 {
    148     return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
    149 }
    150 
    151 void X8::WritedbtBuf(char* dbtbuf)
    152 {/*
    153     float *buf=(float*)dbtbuf;
    154     vector3df vect=getPosition();
    155     memcpy(buf,&vect.X,sizeof(float));
    156     buf++;
    157     memcpy(buf,&vect.Y,sizeof(float));
    158     buf++;
    159     memcpy(buf,&vect.Z,sizeof(float));
    160     buf++;
    161     vect=getRotation();
    162     memcpy(buf,&vect.X,sizeof(float));
    163     buf++;
    164     memcpy(buf,&vect.Y,sizeof(float));
    165     buf++;
    166     memcpy(buf,&vect.Z,sizeof(float));
    167     buf++;
    168     memcpy(buf,&motors,sizeof(rtsimu_motors));*/
    169 }
    170 
    171 void X8::ReaddbtBuf(char* dbtbuf)
    172 {/*
    173     float *buf=(float*)dbtbuf;
    174     vector3df vect;
    175     memcpy(&vect.X,buf,sizeof(float));
    176     buf++;
    177     memcpy(&vect.Y,buf,sizeof(float));
    178     buf++;
    179     memcpy(&vect.Z,buf,sizeof(float));
    180     buf++;
    181     setPosition(vect);
    182     memcpy(&vect.X,buf,sizeof(float));
    183     buf++;
    184     memcpy(&vect.Y,buf,sizeof(float));
    185     buf++;
    186     memcpy(&vect.Z,buf,sizeof(float));
    187     buf++;
    188     ((ISceneNode*)(this))->setRotation(vect);
    189     memcpy(&motors,buf,sizeof(rtsimu_motors));
    190     AnimateModele();*/
    191 }
    192 #endif //GL
    193 
    194 //states are computed on fixed frame NED
    195 //x north
    196 //y east
    197 //z down
    198 void X8::CalcModel(void)
    199 {
    200     float tfl_speed,tfr_speed,trl_speed,trr_speed;
    201     float bfl_speed,bfr_speed,brl_speed,brr_speed;
    202     float u_roll,u_pitch,u_yaw,u_thrust;
    203     float omega;
    204 #ifdef GL
    205     motor_speed_mutex->GetMutex();
    206 #endif //GL
    207     motors->GetSpeeds(motor_speed);
    208 #ifdef GL
    209     motor_speed_mutex->ReleaseMutex();
    210 #endif //GL
    211         tfl_speed=motor_speed[0];
    212         tfr_speed=motor_speed[1];
    213         trl_speed=motor_speed[2];
    214         trr_speed=motor_speed[3];
    215         bfl_speed=motor_speed[4];
    216         bfr_speed=motor_speed[5];
    217         brl_speed=motor_speed[6];
    218         brr_speed=motor_speed[7];
    219 
    220         omega=tfl_speed+brl_speed+trr_speed+bfr_speed-bfl_speed-trl_speed-brr_speed-tfr_speed;
    221 
    222 
    223     /*
    224         ** ===================================================================
    225         **    u roll: roll torque
    226         **
    227         ** ===================================================================
    228         */
    229 
    230     u_roll=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
    231                                                 +sigma->Value()*trl_speed*trl_speed+brl_speed*brl_speed
    232                                                 -sigma->Value()*tfr_speed*tfr_speed-bfr_speed*bfr_speed
    233                                                 -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
    234 
    235     /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed of the quadri in the body frame). It is a discrete integrator
    236         //state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega + u_roll) +state[-1].W.x;//Osamah
    237     state[0].W.x=(dT()/j_roll->Value())*((j_pitch->Value()-j_yaw->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega + u_roll) +state[-1].W.x;//Majd
    238 
    239         //state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
    240 
    241         /*
    242         ** ===================================================================
    243         **   u pitch : pitch torque
    244         **
    245         ** ===================================================================
    246         */
    247         u_pitch=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
    248                                              +sigma->Value()*tfr_speed*tfr_speed+bfr_speed*bfr_speed
    249                                             -sigma->Value()*trl_speed*trl_speed-brl_speed*brl_speed
    250                                              -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
    251 
    252     /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed of the quadri in the body frame). It is a discrete integrator
    253         //state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega + u_pitch)+state[-1].W.y;//Osamah
    254     state[0].W.y=(dT()/j_pitch->Value())*((j_yaw->Value()-j_roll->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega + u_pitch)+state[-1].W.y;//Majd
    255 
    256    //state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
    257 
    258         /*
    259         ** ===================================================================
    260         **    u yaw : yaw torque
    261         **
    262         ** ===================================================================
    263         */
    264         u_yaw=c_mot->Value()*(tfl_speed*tfl_speed-bfl_speed*bfl_speed
    265                         +trr_speed*trr_speed-brr_speed*brr_speed
    266                         -tfr_speed*tfr_speed+bfr_speed*bfr_speed
    267                         -trl_speed*trl_speed+brl_speed*brl_speed);
    268 
    269     /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed of the quadri in the body frame). It is a discrete integrator
    270         //state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
    271         state[0].W.z=(dT()/j_yaw->Value())*((j_roll->Value()-j_pitch->Value())*state[-1].W.x*state[-1].W.y+u_yaw )+state[-1].W.z;//Majd
    272 
    273         //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
    274 
    275     // compute quaternion from W
    276     // Quaternion derivative: dQ = 0.5*(Q*Qw)
    277     Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
    278 
    279     // Quaternion integration
    280     state[0].Quat =state[-1].Quat +dQ*dT();
    281     state[0].Quat.Normalize();
    282 
    283     // Calculation of the thrust from the reference speed of motors
    284     u_thrust=k_mot->Value()*S->Value()*
    285        (sigma->Value()*tfl_speed*tfl_speed+sigma->Value()*tfr_speed*tfr_speed+sigma->Value()*trl_speed*trl_speed+sigma->Value()*trr_speed*trr_speed
    286        +bfl_speed*bfl_speed+bfr_speed*bfr_speed+brl_speed*brl_speed+brr_speed*brr_speed);
    287     Vector3D vect(0,0,-u_thrust);
    288     vect.Rotate(state[0].Quat);
    289 
    290     /*
    291         ** ===================================================================
    292         **     x double integrator
    293         **
    294         ** ===================================================================
    295         */
    296         state[0].Pos.x=(dT()*dT()/m->Value())*(vect.x-f_air_lat->Value()*(state[-1].Pos.x-state[-2].Pos.x)/dT())+2*state[-1].Pos.x-state[-2].Pos.x;
    297     state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
    298 
    299     /*
    300         ** ===================================================================
    301         **     y double integrator
    302         **
    303         ** ===================================================================
    304         */
    305         state[0].Pos.y=(dT()*dT()/m->Value())*(vect.y-f_air_lat->Value()*(state[-1].Pos.y-state[-2].Pos.y)/dT())+2*state[-1].Pos.y-state[-2].Pos.y;
    306     state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
    307 
    308         /*
    309         ** ===================================================================
    310         **     z double integrator
    311         **
    312         ** ===================================================================
    313         */
    314         state[0].Pos.z=(dT()*dT()/m->Value())*(vect.z+f_air_vert->Value()*(state[-1].Pos.z-state[-2].Pos.z)/dT()+m->Value()*G)+2*state[-1].Pos.z-state[-2].Pos.z;
    315     state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
     45namespace flair {
     46namespace simulator {
     47
     48X8::X8(const Simulator *parent, std::string name, int dev_id)
     49    : Model(parent, name) {
     50  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     51  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
     52  arm_length = new DoubleSpinBox(setup_tab->LastRowLastCol(), "arm length (m):",
     53                                 0, 2, 0.1);
     54  l_cg = new DoubleSpinBox(
     55      setup_tab->LastRowLastCol(), "position G (m):", -0.5, 0.5,
     56      0.02); // position du centre de gravité/centre de poussé
     57  k_mot =
     58      new DoubleSpinBox(setup_tab->NewRow(), "k_mot:", 0, 1, 0.001,
     59                        3); // vitesse rotation² (unité arbitraire) -> force (N)
     60  c_mot = new DoubleSpinBox(
     61      setup_tab->LastRowLastCol(), "c_mot:", 0, 1, 0.001,
     62      3); // vitesse rotation moteur -> couple (N.m/unité arbitraire)
     63  f_air_vert = new DoubleSpinBox(setup_tab->NewRow(), "f_air_vert:", 0, 10,
     64                                 1); // frottements air depl. vertical, aussi
     65                                     // utilisé pour les rotations ( N/(m/s) )
     66                                     // (du aux helices en rotation)
     67  f_air_lat =
     68      new DoubleSpinBox(setup_tab->LastRowLastCol(), "f_air_lat:", 0, 10,
     69                        1); // frottements air deplacements lateraux ( N/(m/s) )
     70  j_roll = new DoubleSpinBox(setup_tab->NewRow(), "j_roll:", 0, 1, 0.001,
     71                             5); // moment d'inertie d'un axe (N.m.s²/rad)
     72  j_pitch =
     73      new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_pitch:", 0, 1, 0.001,
     74                        5); // moment d'inertie d'un axe (N.m.s²/rad)
     75  j_yaw = new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_yaw:", 0, 1, 0.001,
     76                            5); // moment d'inertie d'un axe (N.m.s²/rad)
     77  j_r = new DoubleSpinBox(setup_tab->NewRow(), "j_r:", 0, 1,
     78                          0.001); // moment des helices (N.m.s²/rad)
     79  sigma = new DoubleSpinBox(
     80      setup_tab->LastRowLastCol(), "sigma:", 0, 1,
     81      0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
     82  S = new DoubleSpinBox(
     83      setup_tab->LastRowLastCol(), "S:", 1, 2,
     84      0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
     85
     86  motors = new SimuBldc(this, name, 8, dev_id);
     87}
     88
     89void X8::Draw() {
     90#ifdef GL
     91
     92  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
     93  // parameter
     94  // note that the frame used is irrlicht one:
     95  // left handed, North East Up
     96
     97  const IGeometryCreator *geo;
     98  geo = getGui()->getSceneManager()->getGeometryCreator();
     99
     100  // cylinders are aligned with y axis
     101  red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
     102  black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
     103  motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
     104  // geo->drop();
     105
     106  ITexture *texture = getGui()->getTexture("carbone.jpg");
     107  fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     108                             vector3df(0, 0, -135));
     109  fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     110                             vector3df(0, 0, -45));
     111  rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     112                             vector3df(0, 0, 135), texture);
     113  rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     114                             vector3df(0, 0, 45), texture);
     115
     116  texture = getGui()->getTexture("metal047.jpg");
     117  tfl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
     118                                vector3df(90, 0, 0), texture);
     119  tfr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
     120                                vector3df(90, 0, 0), texture);
     121  trl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
     122                                vector3df(90, 0, 0), texture);
     123  trr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
     124                                vector3df(90, 0, 0), texture);
     125
     126  bfl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, -17.5),
     127                                vector3df(90, 0, 0), texture);
     128  bfr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, -17.5),
     129                                vector3df(90, 0, 0), texture);
     130  brl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, -17.5),
     131                                vector3df(90, 0, 0), texture);
     132  brr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, -17.5),
     133                                vector3df(90, 0, 0), texture);
     134
     135  tfl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
     136  tfr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);
     137  trl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);
     138  trr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
     139
     140  bfl_blade = new Blade(this, vector3df(70.71, -70.71, -17.5));
     141  bfr_blade = new Blade(this, vector3df(70.71, 70.71, -17.5), true);
     142  brl_blade = new Blade(this, vector3df(-70.71, -70.71, -17.5), true);
     143  brr_blade = new Blade(this, vector3df(-70.71, 70.71, -17.5));
     144
     145  motor_speed_mutex = new Mutex(this);
     146  for (int i = 0; i < 8; i++)
     147    motor_speed[i] = 0;
     148  ExtraDraw();
     149#endif
     150}
     151
     152X8::~X8() {
     153  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
     154  // pales) par parenté
     155}
     156
     157#ifdef GL
     158void X8::AnimateModel(void) {
     159  motor_speed_mutex->GetMutex();
     160  tfl_blade->SetRotationSpeed(K_MOT * motor_speed[0]);
     161  tfr_blade->SetRotationSpeed(-K_MOT * motor_speed[1]);
     162  trl_blade->SetRotationSpeed(-K_MOT * motor_speed[2]);
     163  trr_blade->SetRotationSpeed(K_MOT * motor_speed[3]);
     164
     165  bfl_blade->SetRotationSpeed(-K_MOT * motor_speed[4]);
     166  bfr_blade->SetRotationSpeed(K_MOT * motor_speed[5]);
     167  brl_blade->SetRotationSpeed(K_MOT * motor_speed[6]);
     168  brr_blade->SetRotationSpeed(-K_MOT * motor_speed[7]);
     169  motor_speed_mutex->ReleaseMutex();
     170
     171  // adapt UAV size
     172  if (arm_length->ValueChanged() == true) {
     173    setScale(arm_length->Value());
     174  }
     175}
     176
     177size_t X8::dbtSize(void) const {
     178  return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices
     179}
     180
     181void X8::WritedbtBuf(
     182    char *dbtbuf) { /*
     183                       float *buf=(float*)dbtbuf;
     184                       vector3df vect=getPosition();
     185                       memcpy(buf,&vect.X,sizeof(float));
     186                       buf++;
     187                       memcpy(buf,&vect.Y,sizeof(float));
     188                       buf++;
     189                       memcpy(buf,&vect.Z,sizeof(float));
     190                       buf++;
     191                       vect=getRotation();
     192                       memcpy(buf,&vect.X,sizeof(float));
     193                       buf++;
     194                       memcpy(buf,&vect.Y,sizeof(float));
     195                       buf++;
     196                       memcpy(buf,&vect.Z,sizeof(float));
     197                       buf++;
     198                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
     199}
     200
     201void X8::ReaddbtBuf(
     202    char *dbtbuf) { /*
     203                       float *buf=(float*)dbtbuf;
     204                       vector3df vect;
     205                       memcpy(&vect.X,buf,sizeof(float));
     206                       buf++;
     207                       memcpy(&vect.Y,buf,sizeof(float));
     208                       buf++;
     209                       memcpy(&vect.Z,buf,sizeof(float));
     210                       buf++;
     211                       setPosition(vect);
     212                       memcpy(&vect.X,buf,sizeof(float));
     213                       buf++;
     214                       memcpy(&vect.Y,buf,sizeof(float));
     215                       buf++;
     216                       memcpy(&vect.Z,buf,sizeof(float));
     217                       buf++;
     218                       ((ISceneNode*)(this))->setRotation(vect);
     219                       memcpy(&motors,buf,sizeof(rtsimu_motors));
     220                       AnimateModele();*/
     221}
     222#endif // GL
     223
     224// states are computed on fixed frame NED
     225// x north
     226// y east
     227// z down
     228void X8::CalcModel(void) {
     229  float tfl_speed, tfr_speed, trl_speed, trr_speed;
     230  float bfl_speed, bfr_speed, brl_speed, brr_speed;
     231  float u_roll, u_pitch, u_yaw, u_thrust;
     232  float omega;
     233#ifdef GL
     234  motor_speed_mutex->GetMutex();
     235#endif // GL
     236  motors->GetSpeeds(motor_speed);
     237#ifdef GL
     238  motor_speed_mutex->ReleaseMutex();
     239#endif // GL
     240  tfl_speed = motor_speed[0];
     241  tfr_speed = motor_speed[1];
     242  trl_speed = motor_speed[2];
     243  trr_speed = motor_speed[3];
     244  bfl_speed = motor_speed[4];
     245  bfr_speed = motor_speed[5];
     246  brl_speed = motor_speed[6];
     247  brr_speed = motor_speed[7];
     248
     249  omega = tfl_speed + brl_speed + trr_speed + bfr_speed - bfl_speed -
     250          trl_speed - brr_speed - tfr_speed;
     251
     252  /*
     253      ** ===================================================================
     254      **    u roll: roll torque
     255      **
     256      ** ===================================================================
     257      */
     258
     259  u_roll = arm_length->Value() * k_mot->Value() *
     260           (sigma->Value() * tfl_speed * tfl_speed + bfl_speed * bfl_speed +
     261            sigma->Value() * trl_speed * trl_speed + brl_speed * brl_speed -
     262            sigma->Value() * tfr_speed * tfr_speed - bfr_speed * bfr_speed -
     263            sigma->Value() * trr_speed * trr_speed - brr_speed * brr_speed) *
     264           sqrtf(2) / 2;
     265
     266  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
     267  /// of the quadri in the body frame). It is a discrete integrator
     268  // state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega
     269  // + u_roll) +state[-1].W.x;//Osamah
     270  state[0].W.x =
     271      (dT() / j_roll->Value()) *
     272          ((j_pitch->Value() - j_yaw->Value()) * state[-1].W.y * state[-1].W.z -
     273           j_r->Value() * state[-1].W.y * omega + u_roll) +
     274      state[-1].W.x; // Majd
     275
     276  // state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
     277
     278  /*
     279  ** ===================================================================
     280  **   u pitch : pitch torque
     281  **
     282  ** ===================================================================
     283  */
     284  u_pitch = arm_length->Value() * k_mot->Value() *
     285            (sigma->Value() * tfl_speed * tfl_speed + bfl_speed * bfl_speed +
     286             sigma->Value() * tfr_speed * tfr_speed + bfr_speed * bfr_speed -
     287             sigma->Value() * trl_speed * trl_speed - brl_speed * brl_speed -
     288             sigma->Value() * trr_speed * trr_speed - brr_speed * brr_speed) *
     289            sqrtf(2) / 2;
     290
     291  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
     292  /// of the quadri in the body frame). It is a discrete integrator
     293  // state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega
     294  // + u_pitch)+state[-1].W.y;//Osamah
     295  state[0].W.y =
     296      (dT() / j_pitch->Value()) *
     297          ((j_yaw->Value() - j_roll->Value()) * state[-1].W.x * state[-1].W.z -
     298           j_r->Value() * state[-1].W.x * omega + u_pitch) +
     299      state[-1].W.y; // Majd
     300
     301  // state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
     302
     303  /*
     304  ** ===================================================================
     305  **    u yaw : yaw torque
     306  **
     307  ** ===================================================================
     308  */
     309  u_yaw = c_mot->Value() * (tfl_speed * tfl_speed - bfl_speed * bfl_speed +
     310                            trr_speed * trr_speed - brr_speed * brr_speed -
     311                            tfr_speed * tfr_speed + bfr_speed * bfr_speed -
     312                            trl_speed * trl_speed + brl_speed * brl_speed);
     313
     314  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
     315  /// of the quadri in the body frame). It is a discrete integrator
     316  // state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
     317  state[0].W.z =
     318      (dT() / j_yaw->Value()) * ((j_roll->Value() - j_pitch->Value()) *
     319                                     state[-1].W.x * state[-1].W.y +
     320                                 u_yaw) +
     321      state[-1].W.z; // Majd
     322
     323  // state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
     324
     325  // compute quaternion from W
     326  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     327  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     328
     329  // Quaternion integration
     330  state[0].Quat = state[-1].Quat + dQ * dT();
     331  state[0].Quat.Normalize();
     332
     333  // Calculation of the thrust from the reference speed of motors
     334  u_thrust =
     335      k_mot->Value() * S->Value() *
     336      (sigma->Value() * tfl_speed * tfl_speed +
     337       sigma->Value() * tfr_speed * tfr_speed +
     338       sigma->Value() * trl_speed * trl_speed +
     339       sigma->Value() * trr_speed * trr_speed + bfl_speed * bfl_speed +
     340       bfr_speed * bfr_speed + brl_speed * brl_speed + brr_speed * brr_speed);
     341  Vector3D vect(0, 0, -u_thrust);
     342  vect.Rotate(state[0].Quat);
     343
     344  /*
     345      ** ===================================================================
     346      **     x double integrator
     347      **
     348      ** ===================================================================
     349      */
     350  state[0].Pos.x =
     351      (dT() * dT() / m->Value()) *
     352          (vect.x -
     353           f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
     354      2 * state[-1].Pos.x - state[-2].Pos.x;
     355  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
     356
     357  /*
     358      ** ===================================================================
     359      **     y double integrator
     360      **
     361      ** ===================================================================
     362      */
     363  state[0].Pos.y =
     364      (dT() * dT() / m->Value()) *
     365          (vect.y -
     366           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
     367      2 * state[-1].Pos.y - state[-2].Pos.y;
     368  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
     369
     370  /*
     371  ** ===================================================================
     372  **     z double integrator
     373  **
     374  ** ===================================================================
     375  */
     376  state[0].Pos.z =
     377      (dT() * dT() / m->Value()) *
     378          (vect.z +
     379           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
     380           m->Value() * G) +
     381      2 * state[-1].Pos.z - state[-2].Pos.z;
     382  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    316383
    317384#ifndef GL
    318     if(state[0].Pos.z<0) state[0].Pos.z=0;
     385  if (state[0].Pos.z < 0)
     386    state[0].Pos.z = 0;
    319387#endif
    320 
    321388}
    322389

  • trunk/lib/FlairSimulator/src/X8.h

    r10 r15  
    2121#include <stdint.h>
    2222
    23 namespace flair
    24 {
    25     namespace core
    26     {
    27         class Mutex;
    28     }
    29     namespace gui
    30     {
    31         class DoubleSpinBox;
    32     }
    33     namespace actuator
    34     {
    35         class SimuBldc;
    36     }
     23namespace flair {
     24namespace core {
     25class Mutex;
     26}
     27namespace gui {
     28class DoubleSpinBox;
     29}
     30namespace actuator {
     31class SimuBldc;
     32}
    3733}
    3834
    3935#ifdef GL
    40 namespace irr
    41 {
    42     namespace scene
    43     {
    44         class IMesh;
    45     }
     36namespace irr {
     37namespace scene {
     38class IMesh;
     39}
    4640}
    4741#endif
    4842
    49 namespace flair
    50 {
    51 namespace simulator
    52 {
    53     class Simulator;
    54     class Blade;
    55     class MeshSceneNode;
     43namespace flair {
     44namespace simulator {
     45class Simulator;
     46class Blade;
     47class MeshSceneNode;
    5648
    57     class X8 : public Model
    58     {
    59         public:
    60             X8(const Simulator* parent,std::string name, int dev_id);
    61             ~X8();
    62             virtual void Draw(void);
    63                         virtual void ExtraDraw(void){};
    64         protected:
     49class X8 : public Model {
     50public:
     51  X8(const Simulator *parent, std::string name, int dev_id);
     52  ~X8();
     53  virtual void Draw(void);
     54  virtual void ExtraDraw(void){};
     55
     56protected:
    6557#ifdef GL
    66             irr::scene::IMesh *red_arm,*black_arm,*motor;
    67                         MeshSceneNode *fl_arm,*fr_arm,*rl_arm,*rr_arm;
    68                         MeshSceneNode *tfl_motor,*tfr_motor,*trl_motor,*trr_motor;
    69                         MeshSceneNode *bfl_motor,*bfr_motor,*brl_motor,*brr_motor;
    70                         Blade *tfl_blade,*tfr_blade,*trl_blade,*trr_blade;
    71                         Blade *bfl_blade,*bfr_blade,*brl_blade,*brr_blade;
     58  irr::scene::IMesh *red_arm, *black_arm, *motor;
     59  MeshSceneNode *fl_arm, *fr_arm, *rl_arm, *rr_arm;
     60  MeshSceneNode *tfl_motor, *tfr_motor, *trl_motor, *trr_motor;
     61  MeshSceneNode *bfl_motor, *bfr_motor, *brl_motor, *brr_motor;
     62  Blade *tfl_blade, *tfr_blade, *trl_blade, *trr_blade;
     63  Blade *bfl_blade, *bfr_blade, *brl_blade, *brr_blade;
    7264#endif
    73         private:
    74             void CalcModel(void);
    75     #ifdef GL
    76             bool OnEvent(const irr::SEvent& event){};
    77             void AnimateModel(void);
    78             size_t dbtSize(void) const;
    79             void WritedbtBuf(char* dbtbuf);
    80             void ReaddbtBuf(char* dbtbuf);
    81             core::Mutex *motor_speed_mutex;
    82     #endif
     65private:
     66  void CalcModel(void);
     67#ifdef GL
     68  bool OnEvent(const irr::SEvent &event){};
     69  void AnimateModel(void);
     70  size_t dbtSize(void) const;
     71  void WritedbtBuf(char *dbtbuf);
     72  void ReaddbtBuf(char *dbtbuf);
     73  core::Mutex *motor_speed_mutex;
     74#endif
    8375
    84             actuator::SimuBldc *motors;
    85             float motor_speed[8];
    86             gui::DoubleSpinBox *m,*arm_length,*l_cg;
    87             gui::DoubleSpinBox *k_mot,*c_mot;
    88             gui::DoubleSpinBox *f_air_vert,*f_air_lat;
    89             gui::DoubleSpinBox *j_roll,*j_pitch,*j_yaw;
    90             gui::DoubleSpinBox *j_r,*sigma,*S;
    91     };
     76  actuator::SimuBldc *motors;
     77  float motor_speed[8];
     78  gui::DoubleSpinBox *m, *arm_length, *l_cg;
     79  gui::DoubleSpinBox *k_mot, *c_mot;
     80  gui::DoubleSpinBox *f_air_vert, *f_air_lat;
     81  gui::DoubleSpinBox *j_roll, *j_pitch, *j_yaw;
     82  gui::DoubleSpinBox *j_r, *sigma, *S;
     83};
    9284} // end namespace simulator
    9385} // end namespace flair

  • trunk/lib/FlairSimulator/src/unexported/AnimPoursuite.h

    r10 r15  
    2222#include <vector3d.h>
    2323
    24 namespace flair
    25 {
    26 namespace simulator
    27 {
     24namespace flair {
     25namespace simulator {
    2826
    29     class AnimPoursuite : public irr::scene::ISceneNodeAnimator
    30     {
    31         public:
    32             AnimPoursuite(const irr::scene::ISceneNode* parent,float rotateSpeed = -500.0f, float zoomSpeed = 4.0f);
    33             ~AnimPoursuite();
     27class AnimPoursuite : public irr::scene::ISceneNodeAnimator {
     28public:
     29  AnimPoursuite(const irr::scene::ISceneNode *parent,
     30                float rotateSpeed = -500.0f, float zoomSpeed = 4.0f);
     31  ~AnimPoursuite();
    3432
    35             void animateNode(irr::scene::ISceneNode* node, irr::u32 timeMs);
    36             ISceneNodeAnimator* createClone(irr::scene::ISceneNode* node,irr::scene::ISceneManager* newManager=0);
    37             bool MouseMoved(const irr::SEvent& event,irr::core::position2df MousePos);
    38             void setPositionOffset(irr::core::vector3df newpos);
    39             void setTargetOffset(irr::core::vector3df newpos);
     33  void animateNode(irr::scene::ISceneNode *node, irr::u32 timeMs);
     34  ISceneNodeAnimator *createClone(irr::scene::ISceneNode *node,
     35                                  irr::scene::ISceneManager *newManager = 0);
     36  bool MouseMoved(const irr::SEvent &event, irr::core::position2df MousePos);
     37  void setPositionOffset(irr::core::vector3df newpos);
     38  void setTargetOffset(irr::core::vector3df newpos);
    4039
    41         private:
    42             virtual bool isEventReceiverEnabled(void) const
    43             {
    44                 return true;
    45             }
     40private:
     41  virtual bool isEventReceiverEnabled(void) const { return true; }
    4642
    47             irr::core::vector3df pos_offset,target_offset;
    48             bool LMouseKey;
    49             irr::core::position2df RotateStart;
    50             irr::core::position2df MousePos;
    51             const irr::scene::ISceneNode* parent;
    52             bool Rotating;
    53             float RotY,RotZ;
    54             float rotateSpeed;
    55             float zoomSpeed;
    56             float currentZoom;
    57             float sat(float value);
    58     };
     43  irr::core::vector3df pos_offset, target_offset;
     44  bool LMouseKey;
     45  irr::core::position2df RotateStart;
     46  irr::core::position2df MousePos;
     47  const irr::scene::ISceneNode *parent;
     48  bool Rotating;
     49  float RotY, RotZ;
     50  float rotateSpeed;
     51  float zoomSpeed;
     52  float currentZoom;
     53  float sat(float value);
     54};
    5955
    6056} // end namespace simulator

  • trunk/lib/FlairSimulator/src/unexported/GenericObject.h

    r10 r15  
    3535#include <Vector3D.h>
    3636
    37 
    3837#include <aabbox3d.h>
    3938#include <IMeshSceneNode.h>
    4039
    41 namespace irr
    42 {
    43     class SEvent;
    44     namespace scene
    45     {
    46         class IMesh;
    47         class ISceneManager;
    48         class ITriangleSelector;
    49         class IMetaTriangleSelector;
    50         class ISceneNodeAnimatorCollisionResponse;
    51     }
     40namespace irr {
     41class SEvent;
     42namespace scene {
     43class IMesh;
     44class ISceneManager;
     45class ITriangleSelector;
     46class IMetaTriangleSelector;
     47class ISceneNodeAnimatorCollisionResponse;
     48}
    5249}
    5350
    … …  
    5552class Simulator_impl;
    5653
    57 namespace flair
    58 {
    59 namespace core
    60 {
     54namespace flair {
     55namespace core {
    6156class ConditionVariable;
    6257}
    63 namespace simulator
    64 {
     58namespace simulator {
    6559class Simulator;
    6660class AnimPoursuite;
    6761
    68 class GenericObject : public irr::scene::IMeshSceneNode
    69 {
    70     friend class ::Simulator_impl;
     62class GenericObject : public irr::scene::IMeshSceneNode {
     63  friend class ::Simulator_impl;
    7164
    7265public:
    73     GenericObject(Simulator* parent,std::string name, irr::scene::ISceneManager* sceneManager);
    74     virtual ~GenericObject();
     66  GenericObject(Simulator *parent, std::string name,
     67                irr::scene::ISceneManager *sceneManager);
     68  virtual ~GenericObject();
    7569
    76     //FROM IMPL
    77     irr::scene::ITriangleSelector* TriangleSelector(void);
    78     irr::core::aabbox3d<irr::f32> box;
    79     //END FROM IMPL
     70  // FROM IMPL
     71  irr::scene::ITriangleSelector *TriangleSelector(void);
     72  irr::core::aabbox3d<irr::f32> box;
     73  // END FROM IMPL
    8074
    81     irr::scene::ISceneNode* getSceneNode();
    82     virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox(void) const
    83     {
    84         return box;
    85     }
     75  irr::scene::ISceneNode *getSceneNode();
     76  virtual const irr::core::aabbox3d<irr::f32> &getBoundingBox(void) const {
     77    return box;
     78  }
    8679
    87     void setPosition(irr::core::vector3df position);
    88     void setScale(float value);
    89     void setScale(irr::core::vector3df scale);
    90     void setRotation(irr::core::vector3df rotation);
    91     void OnRegisterSceneNode(void);
    92     void setMesh(irr::scene::IMesh* mesh);
    93     irr::scene::IMesh* getMesh(void);
    94     void render(void);
    95     virtual void setReadOnlyMaterials(bool readonly){};
    96     virtual bool isReadOnlyMaterials(void) const
    97     {
    98         return false;
    99     }
    100     virtual irr::scene::IShadowVolumeSceneNode* addShadowVolumeSceneNode(const irr::scene::IMesh* shadowMesh=0,
    101             irr::s32 id=-1, bool zfailmethod=true, irr::f32 infinity=1000.0f)
    102     {
    103         return NULL;
    104     }
     80  void setPosition(irr::core::vector3df position);
     81  void setScale(float value);
     82  void setScale(irr::core::vector3df scale);
     83  void setRotation(irr::core::vector3df rotation);
     84  void OnRegisterSceneNode(void);
     85  void setMesh(irr::scene::IMesh *mesh);
     86  irr::scene::IMesh *getMesh(void);
     87  void render(void);
     88  virtual void setReadOnlyMaterials(bool readonly){};
     89  virtual bool isReadOnlyMaterials(void) const { return false; }
     90  virtual irr::scene::IShadowVolumeSceneNode *
     91  addShadowVolumeSceneNode(const irr::scene::IMesh *shadowMesh = 0,
     92                           irr::s32 id = -1, bool zfailmethod = true,
     93                           irr::f32 infinity = 1000.0f) {
     94    return NULL;
     95  }
    10596
    10697private:
    107     void UpdateFrom(core::io_data *data) {};
    108     irr::scene::IMesh *mesh;
    109     irr::scene::ITriangleSelector* selector;
    110     flair::core::ConditionVariable* cond;
    111     irr::video::SMaterial Material;
    112 
     98  void UpdateFrom(core::io_data *data){};
     99  irr::scene::IMesh *mesh;
     100  irr::scene::ITriangleSelector *selector;
     101  flair::core::ConditionVariable *cond;
     102  irr::video::SMaterial Material;
    113103};
    114104} // end namespace simulator

  • trunk/lib/FlairSimulator/src/unexported/Gui_impl.h

    r10 r15  
    2424#include <vector3d.h>
    2525
    26 namespace irr
    27 {
    28     class IrrlichtDevice;
    29     namespace scene
    30     {
    31         class ISceneManager;
    32         class IMeshSceneNode;
    33         class ITriangleSelector;
    34     }
    35     namespace video
    36     {
    37         class IVideoDriver;
    38     }
    39     namespace gui
    40     {
    41         class IGUIFont;
    42         class IGUIEnvironment;
    43     }
     26namespace irr {
     27class IrrlichtDevice;
     28namespace scene {
     29class ISceneManager;
     30class IMeshSceneNode;
     31class ITriangleSelector;
     32}
     33namespace video {
     34class IVideoDriver;
     35}
     36namespace gui {
     37class IGUIFont;
     38class IGUIEnvironment;
     39}
    4440}
    4541
    46 namespace flair
    47 {
    48     namespace core
    49     {
    50         class Object;
    51     }
    52     namespace simulator
    53     {
    54         class Model;
    55         class GenericObject;
    56         class Gui;
    57     }
     42namespace flair {
     43namespace core {
     44class Object;
     45}
     46namespace simulator {
     47class Model;
     48class GenericObject;
     49class Gui;
     50}
    5851}
    5952
    6053class MyEventReceiver;
    6154
    62 class Gui_impl
    63 {
    64     public:
    65         Gui_impl(flair::simulator::Gui* self,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,irr::video::E_DRIVER_TYPE driver_type=irr::video::EDT_OPENGL);
    66         ~Gui_impl();
    67         void RunGui(std::vector<flair::simulator::Model*> modeles,std::vector<flair::simulator::GenericObject*> objects);
    68         void setMesh(std::string file,irr::core::vector3df position = irr::core::vector3df(0,0,0),irr::core::vector3df rotation= irr::core::vector3df(0,0,0),irr::core::vector3df scale= irr::core::vector3df(1,1,1));
     55class Gui_impl {
     56public:
     57  Gui_impl(flair::simulator::Gui *self, int app_width, int app_height,
     58           int scene_width, int scene_height, std::string media_path,
     59           irr::video::E_DRIVER_TYPE driver_type = irr::video::EDT_OPENGL);
     60  ~Gui_impl();
     61  void RunGui(std::vector<flair::simulator::Model *> modeles,
     62              std::vector<flair::simulator::GenericObject *> objects);
     63  void setMesh(std::string file,
     64               irr::core::vector3df position = irr::core::vector3df(0, 0, 0),
     65               irr::core::vector3df rotation = irr::core::vector3df(0, 0, 0),
     66               irr::core::vector3df scale = irr::core::vector3df(1, 1, 1));
    6967
    70         irr::scene::IMeshSceneNode* node;
    71         irr::IrrlichtDevice *device;
    72         std::string media_path;
    73         irr::video::IVideoDriver* driver;
    74         irr::scene::ISceneManager* smgr;
    75         int scene_width,scene_height;
     68  irr::scene::IMeshSceneNode *node;
     69  irr::IrrlichtDevice *device;
     70  std::string media_path;
     71  irr::video::IVideoDriver *driver;
     72  irr::scene::ISceneManager *smgr;
     73  int scene_width, scene_height;
    7674
    77     private:
    78         MyEventReceiver *receiver;
    79         irr::scene::ITriangleSelector* selector;
    80         irr::gui::IGUIFont* font;
    81         irr::gui::IGUIEnvironment* env;
    82         void setWindowCaption(flair::core::Object* object, int fps);
    83         void takeScreenshot(void);
    84         hdfile_t *dbtFile_r,*dbtFile_w;
    85         size_t dbtSize(std::vector<flair::simulator::Model*> modeles);
    86         char* dbtbuf;
    87         flair::simulator::Gui *self;
     75private:
     76  MyEventReceiver *receiver;
     77  irr::scene::ITriangleSelector *selector;
     78  irr::gui::IGUIFont *font;
     79  irr::gui::IGUIEnvironment *env;
     80  void setWindowCaption(flair::core::Object *object, int fps);
     81  void takeScreenshot(void);
     82  hdfile_t *dbtFile_r, *dbtFile_w;
     83  size_t dbtSize(std::vector<flair::simulator::Model *> modeles);
     84  char *dbtbuf;
     85  flair::simulator::Gui *self;
    8886};
    8987

  • trunk/lib/FlairSimulator/src/unexported/Model_impl.h

    r10 r15  
    2323#include "Quaternion.h"
    2424
    25 namespace flair
    26 {
    27     namespace core
    28     {
    29         class cvmatrix;
    30         class Mutex;
    31         class ConditionVariable;
    32     }
    33     namespace gui
    34     {
    35         class TabWidget;
    36         class CheckBox;
    37         class DoubleSpinBox;
    38         class SpinBox;
    39         class Vector3DSpinBox;
    40     }
    41     namespace simulator
    42     {
    43         class Simulator;
    44         class AnimPoursuite;
    45     }
     25namespace flair {
     26namespace core {
     27class cvmatrix;
     28class Mutex;
     29class ConditionVariable;
     30}
     31namespace gui {
     32class TabWidget;
     33class CheckBox;
     34class DoubleSpinBox;
     35class SpinBox;
     36class Vector3DSpinBox;
     37}
     38namespace simulator {
     39class Simulator;
     40class AnimPoursuite;
     41}
    4642}
    4743
    … …  
    5046#include <IMeshSceneNode.h>
    5147
    52 namespace irr
    53 {
    54     namespace scene
    55     {
    56         class ISceneManager;
    57         class ITriangleSelector;
    58         class IMetaTriangleSelector;
    59         class ISceneNodeAnimatorCollisionResponse;
    60         class ICameraSceneNode;
    61     }
     48namespace irr {
     49namespace scene {
     50class ISceneManager;
     51class ITriangleSelector;
     52class IMetaTriangleSelector;
     53class ISceneNodeAnimatorCollisionResponse;
     54class ICameraSceneNode;
     55}
    6256}
    6357#endif
    6458
    6559#ifdef GL
    66 class Model_impl : public irr::scene::ISceneNode,public flair::core::Thread,private vrpn_Tracker
     60class Model_impl : public irr::scene::ISceneNode,
     61                   public flair::core::Thread,
     62                   private vrpn_Tracker
    6763#else
    68 class Model_impl : public flair::core::Thread, private vrpn_Tracker
     64class Model_impl : public flair::core::Thread,
     65                   private vrpn_Tracker
    6966#endif
    70 {
    71     public:
     67                   {
     68public:
    7269#ifdef GL
    73         Model_impl(flair::simulator::Model* self,std::string name,irr::scene::ISceneManager* scenemanager,vrpn_Connection_IP* vrpn);
     70  Model_impl(flair::simulator::Model *self, std::string name,
     71             irr::scene::ISceneManager *scenemanager, vrpn_Connection_IP *vrpn);
    7472#else
    75         Model_impl(flair::simulator::Model* self,std::string name,vrpn_Connection_IP* vrpn);
     73  Model_impl(flair::simulator::Model *self, std::string name,
     74             vrpn_Connection_IP *vrpn);
    7675#endif
    77         ~Model_impl();
     76  ~Model_impl();
    7877
    7978#ifdef GL
    80         void OnRegisterSceneNode(void);
    81         void render(void);
    82         void Draw(void){printf("CA MARCHE PAS PUNAISE\r\n");ExtraDraw();};
    83         void ExtraDraw(void){printf("nope\r\n");};
     79  void OnRegisterSceneNode(void);
     80  void render(void);
     81  void Draw(void) {
     82    printf("CA MARCHE PAS PUNAISE\r\n");
     83    ExtraDraw();
     84  };
     85  void ExtraDraw(void) { printf("nope\r\n"); };
    8486
    85         const irr::core::aabbox3d<irr::f32>& getBoundingBox(void) const
    86         {
    87             return box;
    88         }
    89         void UpdatePos(void);
    90         void CheckCollision(void);
    91         irr::scene::ITriangleSelector* TriangleSelector(void);
    92         irr::scene::IMetaTriangleSelector* MetaTriangleSelector(void);
    93         irr::core::aabbox3d<irr::f32> box;
    94         void SynchronizationPoint();
    95         irr::scene::ICameraSceneNode* camera;
    96         flair::simulator::AnimPoursuite* animator;
    97         irr::scene::ITriangleSelector* selector;
     87  const irr::core::aabbox3d<irr::f32> &getBoundingBox(void) const {
     88    return box;
     89  }
     90  void UpdatePos(void);
     91  void CheckCollision(void);
     92  irr::scene::ITriangleSelector *TriangleSelector(void);
     93  irr::scene::IMetaTriangleSelector *MetaTriangleSelector(void);
     94  irr::core::aabbox3d<irr::f32> box;
     95  void SynchronizationPoint();
     96  irr::scene::ICameraSceneNode *camera;
     97  flair::simulator::AnimPoursuite *animator;
     98  irr::scene::ITriangleSelector *selector;
    9899#endif
    99         void mainloop(void);
     100  void mainloop(void);
    100101
    101         flair::gui::TabWidget *tabwidget;
    102         flair::gui::DoubleSpinBox *dT;
     102  flair::gui::TabWidget *tabwidget;
     103  flair::gui::DoubleSpinBox *dT;
    103104
    104     private:
    105         flair::gui::Vector3DSpinBox* pos_init;
    106         flair::gui::SpinBox *yaw_init;
    107         flair::gui::CheckBox *enable_opti;
    108         flair::simulator::Model* self;
    109         flair::core::cvmatrix* output;
    110         flair::core::Mutex *states_mutex;
     105private:
     106  flair::gui::Vector3DSpinBox *pos_init;
     107  flair::gui::SpinBox *yaw_init;
     108  flair::gui::CheckBox *enable_opti;
     109  flair::simulator::Model *self;
     110  flair::core::cvmatrix *output;
     111  flair::core::Mutex *states_mutex;
    111112
    112         struct timeval _timestamp;
    113         void Run(void);
    114         flair::core::Quaternion ComputeInitRotation(flair::core::Quaternion quat_in);
     113  struct timeval _timestamp;
     114  void Run(void);
     115  flair::core::Quaternion ComputeInitRotation(flair::core::Quaternion quat_in);
    115116#ifdef GL
    116         void CollisionHandler(void);
     117  void CollisionHandler(void);
    117118
    118         irr::scene::IMetaTriangleSelector* meta_selector;
    119         irr::scene::ISceneNodeAnimatorCollisionResponse* anim;
     119  irr::scene::IMetaTriangleSelector *meta_selector;
     120  irr::scene::ISceneNodeAnimatorCollisionResponse *anim;
    120121
    121         bool position_init;
     122  bool position_init;
    122123
    123         flair::core::ConditionVariable* cond;
    124         int sync_count;
     124  flair::core::ConditionVariable *cond;
     125  int sync_count;
    125126
    126         flair::core::Mutex *collision_mutex;
    127         bool collision_occured;
    128         flair::core::Vector3D collision_point;
     127  flair::core::Mutex *collision_mutex;
     128  bool collision_occured;
     129  flair::core::Vector3D collision_point;
    129130#endif
    130 
    131131};
    132132

  • trunk/lib/FlairSimulator/src/unexported/Simulator_impl.h

    r10 r15  
    2121#include <Thread.h>
    2222
    23 
    24 namespace flair
    25 {
    26     namespace simulator
    27     {
    28         class Simulator;
    29         class Model;
    30         class GenericObject;
    31     }
     23namespace flair {
     24namespace simulator {
     25class Simulator;
     26class Model;
     27class GenericObject;
     28}
    3229}
    3330
    34 class Simulator_impl: public vrpn_Connection_IP, private flair::core::Thread
    35 {
    36     friend class flair::simulator::Model;
    37     friend class flair::simulator::GenericObject;
     31class Simulator_impl : public vrpn_Connection_IP, private flair::core::Thread {
     32  friend class flair::simulator::Model;
     33  friend class flair::simulator::GenericObject;
    3834
    39     public:
    40         Simulator_impl(flair::simulator::Simulator* self,int optitrack_mstime=10,float yaw_deg=30);
    41         ~Simulator_impl();
     35public:
     36  Simulator_impl(flair::simulator::Simulator *self, int optitrack_mstime = 10,
     37                 float yaw_deg = 30);
     38  ~Simulator_impl();
    4239
    43         void RunSimu(void);
    44         float yaw_rad;
     40  void RunSimu(void);
     41  float yaw_rad;
    4542
    46     private:
    47         void Run(void);
    48         flair::simulator::Simulator* self;
    49         std::vector<flair::simulator::Model*> models;
    50         std::vector<flair::simulator::GenericObject*> objects;
    51         int optitrack_mstime;
     43private:
     44  void Run(void);
     45  flair::simulator::Simulator *self;
     46  std::vector<flair::simulator::Model *> models;
     47  std::vector<flair::simulator::GenericObject *> objects;
     48  int optitrack_mstime;
    5249};
    5350
Note: See TracChangeset for help on using the changeset viewer.