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

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

flaircore
File size: 6.2 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 { namespace core {
27
28/*! \class ImuDataElement
29 */
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
104ImuData::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
119}
120
121ImuData::~ImuData() {}
122
123
124Vector3D ImuData::GetRawAcc(void) const {
125 Vector3D out;
126 GetMutex();
127 out=rawAcc;
128 ReleaseMutex();
129 return out;
130}
131
132Vector3D ImuData::GetRawMag(void) const {
133 Vector3D out;
134 GetMutex();
135 out=rawMag;
136 ReleaseMutex();
137 return out;
138}
139
140Vector3D ImuData::GetRawGyr(void) const {
141 Vector3D out;
142 GetMutex();
143 out=rawGyr;
144 ReleaseMutex();
145 return out;
146}
147
148void ImuData::GetRawAccMagAndGyr(Vector3D &inRawAcc,Vector3D &inRawMag,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,const Vector3D &inRawMag,const Vector3D &inRawGyr) {
175 GetMutex();
176 rawAcc=inRawAcc;
177 rawMag=inRawMag;
178 rawGyr=inRawGyr;
179 ReleaseMutex();
180}
181
182IODataElement* 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
226void 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
244void ImuData::Queue(char** dst,const void *src,size_t size) const {
245 memcpy(*dst,src,size);
246 *dst+=size;
247}
248
249} // end namespace core
250} // end namespace flair
Note: See TracBrowser for help on using the repository browser.