Changeset 15 in flair-src for trunk/lib/FlairCore/src/AhrsData.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/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
Note: See TracChangeset for help on using the changeset viewer.