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

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

sources reformatted with flair-format-dir script

File size: 5.4 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
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
26namespace flair {
27namespace core {
28
29/*! \class ImuDataElement
30    */
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);
119}
120
121ImuData::~ImuData() {}
122
123Vector3D ImuData::GetRawAcc(void) const {
124  Vector3D out;
125  GetMutex();
126  out = rawAcc;
127  ReleaseMutex();
128  return out;
129}
130
131Vector3D ImuData::GetRawMag(void) const {
132  Vector3D out;
133  GetMutex();
134  out = rawMag;
135  ReleaseMutex();
136  return out;
137}
138
139Vector3D ImuData::GetRawGyr(void) const {
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();
154}
155
156void ImuData::SetRawAcc(const Vector3D &inRawAcc) {
157  GetMutex();
158  rawAcc = inRawAcc;
159  ReleaseMutex();
160}
161
162void ImuData::SetRawMag(const Vector3D &inRawMag) {
163  GetMutex();
164  rawMag = inRawMag;
165  ReleaseMutex();
166}
167
168void ImuData::SetRawGyr(const Vector3D &inRawGyr) {
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;
249}
250
251} // end namespace core
252} // end namespace flair
Note: See TracBrowser for help on using the repository browser.