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

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

flaircore

File size: 6.8 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:    2016/03/01
6//  filename:   AhrsData.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    class defining ahrs datas
14//
15//
16/*********************************************************************/
17
18#include "AhrsData.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 AhrsDataElement
29    */
30    class AhrsDataElement: public IODataElement {
31        public:
32
33            AhrsDataElement(const AhrsData *inAhrsData,string name,AhrsData::PlotableData_t inPlotableData):
34                            IODataElement(inAhrsData,name),
35                            ahrsData(inAhrsData),
36                            plotableData(inPlotableData) {
37
38                size=sizeof(float);
39            }
40
41            ~AhrsDataElement() {}
42
43            void CopyData(char* destination) const {
44                float data;
45                Vector3D angularRates;
46                Euler eulerAngles;
47                Quaternion quaternion;
48                ahrsData->GetQuaternionAndAngularRates(quaternion,angularRates);
49                quaternion.ToEuler(eulerAngles);
50                switch(plotableData) {
51                case AhrsData::Roll:
52                    data=eulerAngles.roll;
53                    break;
54                case AhrsData::Pitch:
55                    data=eulerAngles.pitch;
56                    break;
57                case AhrsData::Yaw:
58                    data=eulerAngles.yaw;
59                    break;
60                case AhrsData::RollDeg:
61                    data=Euler::ToDegree(eulerAngles.roll);
62                    break;
63                case AhrsData::PitchDeg:
64                    data=Euler::ToDegree(eulerAngles.pitch);
65                    break;
66                case AhrsData::YawDeg:
67                    data=Euler::ToDegree(eulerAngles.yaw);
68                    break;
69                case AhrsData::Q0:
70                    data=quaternion.q0;
71                    break;
72                case AhrsData::Q1:
73                    data=quaternion.q1;
74                    break;
75                case AhrsData::Q2:
76                    data=quaternion.q2;
77                    break;
78                case AhrsData::Q3:
79                    data=quaternion.q3;
80                    break;
81                case AhrsData::Wx:
82                    data=angularRates.x;
83                    break;
84                case AhrsData::Wy:
85                    data=angularRates.y;
86                    break;
87                case AhrsData::Wz:
88                    data=angularRates.z;
89                    break;
90                case AhrsData::WxDeg:
91                    data=Euler::ToDegree(angularRates.x);
92                    break;
93                case AhrsData::WyDeg:
94                    data=Euler::ToDegree(angularRates.y);
95                    break;
96                case AhrsData::WzDeg:
97                    data=Euler::ToDegree(angularRates.z);
98                    break;
99                default:
100                    ahrsData->Err("data type unavailable.\n");
101                    data=0;
102                    break;
103                }
104                memcpy(destination,&data,sizeof(float));
105            }
106
107            const FloatType &GetDataType(void) const {
108                return dataType;
109            }
110
111        private:
112            const AhrsData *ahrsData;
113            AhrsData::PlotableData_t plotableData;
114            FloatType dataType;
115
116    };
117
118AhrsData::AhrsData(const Object *parent,std::string name,int n): io_data(parent,name,n), dataType(floatType) {
119    if(n>1) Warn("n>1 not supported\n");
120
121    AppendLogDescription("q0",floatType);
122    AppendLogDescription("q1",floatType);
123    AppendLogDescription("q2",floatType);
124    AppendLogDescription("q3",floatType);
125
126    AppendLogDescription("wx",floatType);
127    AppendLogDescription("wy",floatType);
128    AppendLogDescription("wz",floatType);
129}
130
131AhrsData::~AhrsData() {}
132
133Quaternion AhrsData::GetQuaternion(void) const {
134    Quaternion out;
135    GetMutex();
136    out=quaternion;
137    ReleaseMutex();
138    return out;
139}
140
141Vector3D AhrsData::GetAngularRates(void) const {
142    Vector3D out;
143    GetMutex();
144    out=angularRates;
145    ReleaseMutex();
146    return out;
147}
148
149void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,Vector3D &outAngularRates) const {
150    GetMutex();
151    outQuaternion=quaternion;
152    outAngularRates=angularRates;
153    ReleaseMutex();
154}
155
156void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,const Vector3D &inAngularRates) {
157    GetMutex();
158    quaternion=inQuaternion;
159    angularRates=inAngularRates;
160    ReleaseMutex();
161}
162
163void AhrsData::SetQuaternion(const Quaternion &inQuaternion) {
164    GetMutex();
165    quaternion=inQuaternion;
166    ReleaseMutex();
167}
168
169void AhrsData::SetAngularRates(const Vector3D &inAngularRates) {
170    GetMutex();
171    angularRates=inAngularRates;
172    ReleaseMutex();
173}
174
175IODataElement* AhrsData::Element(PlotableData_t plotableData) const {
176    string name;
177    switch(plotableData) {
178    case AhrsData::Roll:
179        name="Roll";
180        break;
181    case AhrsData::Pitch:
182        name="Pitch";
183        break;
184    case AhrsData::Yaw:
185        name="Yaw";
186        break;
187     case AhrsData::RollDeg:
188        name="Roll degree";
189        break;
190    case AhrsData::PitchDeg:
191        name="Pitch degree";
192        break;
193    case AhrsData::YawDeg:
194        name="Yaw degree";
195        break;
196    case AhrsData::Q0:
197        name="Q0";
198        break;
199    case AhrsData::Q1:
200        name="Q1";
201        break;
202    case AhrsData::Q2:
203        name="Q2";
204        break;
205    case AhrsData::Q3:
206        name="Q3";
207        break;
208    case AhrsData::Wx:
209        name="Wx";
210        break;
211    case AhrsData::Wy:
212        name="Wy";
213        break;
214    case AhrsData::Wz:
215        name="Wz";
216        break;
217    case AhrsData::WxDeg:
218        name="Wx degree";
219        break;
220    case AhrsData::WyDeg:
221        name="Wy degree";
222        break;
223    case AhrsData::WzDeg:
224        name="Wz degree";
225        break;
226    default:
227        Err("data type unavailable.\n");
228    }
229
230    return new AhrsDataElement(this,name,plotableData);
231}
232
233void AhrsData::CopyDatas(char *dst) const {
234    GetMutex();
235    Queue(&dst,&quaternion.q0,sizeof(quaternion.q0));
236    Queue(&dst,&quaternion.q1,sizeof(quaternion.q1));
237    Queue(&dst,&quaternion.q2,sizeof(quaternion.q2));
238    Queue(&dst,&quaternion.q3,sizeof(quaternion.q3));
239
240    Queue(&dst,&angularRates.x,sizeof(angularRates.x));
241    Queue(&dst,&angularRates.y,sizeof(angularRates.y));
242    Queue(&dst,&angularRates.z,sizeof(angularRates.z));
243    ReleaseMutex();
244}
245
246void AhrsData::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.