source: flair-src/trunk/lib/FlairCore/src/ImuData.cpp @ 15

Last change on this file since 15 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 5.4 KB
RevLine 
[2]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[2]4// %flair:license}
5//  created:    2014/01/15
6//  filename:   ImuData.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    class defining imu datas
14//
15//
16/*********************************************************************/
17
18#include "ImuData.h"
19#include "Euler.h"
20#include <math.h>
21#include <string.h>
22#include <stdlib.h>
23
24using std::string;
25
[15]26namespace flair {
27namespace core {
[2]28
29/*! \class ImuDataElement
30    */
[15]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;
[2]38
[15]39    size = 4;
40  }
[2]41
[15]42  ~ImuDataElement() {}
[2]43
[15]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    }
[2]91
[15]92    memcpy(dst, &data, sizeof(float));
93  }
[2]94
[15]95  FloatType const &GetDataType(void) const { return dataType; }
[2]96
[15]97private:
98  const ImuData *imudata;
99  ImuData::PlotableData_t plotableData;
100  FloatType dataType;
101};
[2]102
[15]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");
[2]107
[15]108  AppendLogDescription("raw_ax", floatType);
109  AppendLogDescription("raw_ay", floatType);
110  AppendLogDescription("raw_az", floatType);
[2]111
[15]112  AppendLogDescription("raw_gx", floatType);
113  AppendLogDescription("raw_gy", floatType);
114  AppendLogDescription("raw_gz", floatType);
[2]115
[15]116  AppendLogDescription("raw_mx", floatType);
117  AppendLogDescription("raw_my", floatType);
118  AppendLogDescription("raw_mz", floatType);
[2]119}
120
121ImuData::~ImuData() {}
122
123Vector3D ImuData::GetRawAcc(void) const {
[15]124  Vector3D out;
125  GetMutex();
126  out = rawAcc;
127  ReleaseMutex();
128  return out;
[2]129}
130
131Vector3D ImuData::GetRawMag(void) const {
[15]132  Vector3D out;
133  GetMutex();
134  out = rawMag;
135  ReleaseMutex();
136  return out;
[2]137}
138
139Vector3D ImuData::GetRawGyr(void) const {
[15]140  Vector3D out;
141  GetMutex();
142  out = rawGyr;
143  ReleaseMutex();
144  return out;
[2]145}
146
[15]147void ImuData::GetRawAccMagAndGyr(Vector3D &inRawAcc, Vector3D &inRawMag,
148                                 Vector3D &inRawGyr) const {
149  GetMutex();
150  inRawAcc = rawAcc;
151  inRawMag = rawMag;
152  inRawGyr = rawGyr;
153  ReleaseMutex();
[2]154}
155
156void ImuData::SetRawAcc(const Vector3D &inRawAcc) {
[15]157  GetMutex();
158  rawAcc = inRawAcc;
159  ReleaseMutex();
[2]160}
161
162void ImuData::SetRawMag(const Vector3D &inRawMag) {
[15]163  GetMutex();
164  rawMag = inRawMag;
165  ReleaseMutex();
[2]166}
167
168void ImuData::SetRawGyr(const Vector3D &inRawGyr) {
[15]169  GetMutex();
170  rawGyr = inRawGyr;
171  ReleaseMutex();
[2]172}
173
[15]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();
[2]182}
183
[15]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  }
[2]224
[15]225  return new ImuDataElement(this, name, data_type);
[2]226}
227
[15]228void ImuData::CopyDatas(char *dst) const {
229  GetMutex();
[2]230
[15]231  Queue(&dst, &rawAcc.x, sizeof(rawAcc.x));
232  Queue(&dst, &rawAcc.y, sizeof(rawAcc.y));
233  Queue(&dst, &rawAcc.z, sizeof(rawAcc.z));
[2]234
[15]235  Queue(&dst, &rawGyr.x, sizeof(rawGyr.x));
236  Queue(&dst, &rawGyr.y, sizeof(rawGyr.y));
237  Queue(&dst, &rawGyr.z, sizeof(rawGyr.z));
[2]238
[15]239  Queue(&dst, &rawMag.x, sizeof(rawMag.x));
240  Queue(&dst, &rawMag.y, sizeof(rawMag.y));
241  Queue(&dst, &rawMag.z, sizeof(rawMag.z));
[2]242
[15]243  ReleaseMutex();
[2]244}
245
[15]246void ImuData::Queue(char **dst, const void *src, size_t size) const {
247  memcpy(*dst, src, size);
248  *dst += size;
[2]249}
250
251} // end namespace core
252} // end namespace flair
Note: See TracBrowser for help on using the repository browser.