source: flair-src/branches/sanscv/lib/FlairCore/src/ImuData.cpp@ 324

Last change on this file since 324 was 324, checked in by Sanahuja Guillaume, 2 years ago

removing opencv dependency

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 Vector3Df rawAcc = imudata->GetRawAcc();
47 Vector3Df rawMag = imudata->GetRawMag();
48 Vector3Df 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
123Vector3Df ImuData::GetRawAcc(void) const {
124 Vector3Df out;
125 GetMutex();
126 out = rawAcc;
127 ReleaseMutex();
128 return out;
129}
130
131Vector3Df ImuData::GetRawMag(void) const {
132 Vector3Df out;
133 GetMutex();
134 out = rawMag;
135 ReleaseMutex();
136 return out;
137}
138
139Vector3Df ImuData::GetRawGyr(void) const {
140 Vector3Df out;
141 GetMutex();
142 out = rawGyr;
143 ReleaseMutex();
144 return out;
145}
146
147void ImuData::GetRawAccMagAndGyr(Vector3Df &inRawAcc, Vector3Df &inRawMag,
148 Vector3Df &inRawGyr) const {
149 GetMutex();
150 inRawAcc = rawAcc;
151 inRawMag = rawMag;
152 inRawGyr = rawGyr;
153 ReleaseMutex();
154}
155
156void ImuData::SetRawAcc(const Vector3Df &inRawAcc) {
157 GetMutex();
158 rawAcc = inRawAcc;
159 ReleaseMutex();
160}
161
162void ImuData::SetRawMag(const Vector3Df &inRawMag) {
163 GetMutex();
164 rawMag = inRawMag;
165 ReleaseMutex();
166}
167
168void ImuData::SetRawGyr(const Vector3Df &inRawGyr) {
169 GetMutex();
170 rawGyr = inRawGyr;
171 ReleaseMutex();
172}
173
174void ImuData::SetRawAccMagAndGyr(const Vector3Df &inRawAcc,
175 const Vector3Df &inRawMag,
176 const Vector3Df &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::RawRead(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.