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

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