Changeset 15 in flair-src for trunk/lib/FlairCore/src/ImuData.cpp


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

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • 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
Note: See TracChangeset for help on using the changeset viewer.