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

Last change on this file since 2 was 2, checked in by Sanahuja Guillaume, 5 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.