source: flair-src/branches/sanscv/lib/FlairCore/src/AhrsData.cpp

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

removing opencv dependency

File size: 5.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 {
27namespace core {
28
29/*! \class AhrsDataElement
30 */
31class AhrsDataElement : public IODataElement {
32public:
33 AhrsDataElement(const AhrsData *inAhrsData, string name,
34 AhrsData::PlotableData_t inPlotableData)
35 : IODataElement(inAhrsData, name), 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 Vector3Df 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 { return dataType; }
108
109private:
110 const AhrsData *ahrsData;
111 AhrsData::PlotableData_t plotableData;
112 FloatType dataType;
113};
114
115AhrsData::AhrsData(const Object *parent, std::string name, int n)
116 : io_data(parent, name, n), dataType(floatType) {
117 if (n > 1)
118 Warn("n>1 not supported\n");
119
120 AppendLogDescription("q0", floatType);
121 AppendLogDescription("q1", floatType);
122 AppendLogDescription("q2", floatType);
123 AppendLogDescription("q3", floatType);
124
125 AppendLogDescription("wx", floatType);
126 AppendLogDescription("wy", floatType);
127 AppendLogDescription("wz", floatType);
128}
129
130AhrsData::~AhrsData() {}
131
132Quaternion AhrsData::GetQuaternion(void) const {
133 Quaternion out;
134 GetMutex();
135 out = quaternion;
136 ReleaseMutex();
137 return out;
138}
139
140Vector3Df AhrsData::GetAngularRates(void) const {
141 Vector3Df out;
142 GetMutex();
143 out = angularRates;
144 ReleaseMutex();
145 return out;
146}
147
148void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,
149 Vector3Df &outAngularRates) const {
150 GetMutex();
151 outQuaternion = quaternion;
152 outAngularRates = angularRates;
153 ReleaseMutex();
154}
155
156void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,
157 const Vector3Df &inAngularRates) {
158 GetMutex();
159 quaternion = inQuaternion;
160 angularRates = inAngularRates;
161 ReleaseMutex();
162}
163
164void AhrsData::SetQuaternion(const Quaternion &inQuaternion) {
165 GetMutex();
166 quaternion = inQuaternion;
167 ReleaseMutex();
168}
169
170void AhrsData::SetAngularRates(const Vector3Df &inAngularRates) {
171 GetMutex();
172 angularRates = inAngularRates;
173 ReleaseMutex();
174}
175
176IODataElement *AhrsData::Element(PlotableData_t plotableData) const {
177 string name;
178 switch (plotableData) {
179 case AhrsData::Roll:
180 name = "Roll";
181 break;
182 case AhrsData::Pitch:
183 name = "Pitch";
184 break;
185 case AhrsData::Yaw:
186 name = "Yaw";
187 break;
188 case AhrsData::RollDeg:
189 name = "Roll degree";
190 break;
191 case AhrsData::PitchDeg:
192 name = "Pitch degree";
193 break;
194 case AhrsData::YawDeg:
195 name = "Yaw degree";
196 break;
197 case AhrsData::Q0:
198 name = "Q0";
199 break;
200 case AhrsData::Q1:
201 name = "Q1";
202 break;
203 case AhrsData::Q2:
204 name = "Q2";
205 break;
206 case AhrsData::Q3:
207 name = "Q3";
208 break;
209 case AhrsData::Wx:
210 name = "Wx";
211 break;
212 case AhrsData::Wy:
213 name = "Wy";
214 break;
215 case AhrsData::Wz:
216 name = "Wz";
217 break;
218 case AhrsData::WxDeg:
219 name = "Wx degree";
220 break;
221 case AhrsData::WyDeg:
222 name = "Wy degree";
223 break;
224 case AhrsData::WzDeg:
225 name = "Wz degree";
226 break;
227 default:
228 Err("data type unavailable.\n");
229 }
230
231 return new AhrsDataElement(this, name, plotableData);
232}
233
234void AhrsData::RawRead(char *dst) const {
235 GetMutex();
236 Queue(&dst, &quaternion.q0, sizeof(quaternion.q0));
237 Queue(&dst, &quaternion.q1, sizeof(quaternion.q1));
238 Queue(&dst, &quaternion.q2, sizeof(quaternion.q2));
239 Queue(&dst, &quaternion.q3, sizeof(quaternion.q3));
240
241 Queue(&dst, &angularRates.x, sizeof(angularRates.x));
242 Queue(&dst, &angularRates.y, sizeof(angularRates.y));
243 Queue(&dst, &angularRates.z, sizeof(angularRates.z));
244 ReleaseMutex();
245}
246
247void AhrsData::Queue(char **dst, const void *src, size_t size) const {
248 memcpy(*dst, src, size);
249 *dst += size;
250}
251
252} // end namespace core
253} // end namespace flair
Note: See TracBrowser for help on using the repository browser.