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 |
|
---|
24 | using std::string;
|
---|
25 |
|
---|
26 | namespace flair {
|
---|
27 | namespace core {
|
---|
28 |
|
---|
29 | /*! \class AhrsDataElement
|
---|
30 | */
|
---|
31 | class AhrsDataElement : public IODataElement {
|
---|
32 | public:
|
---|
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 |
|
---|
109 | private:
|
---|
110 | const AhrsData *ahrsData;
|
---|
111 | AhrsData::PlotableData_t plotableData;
|
---|
112 | FloatType dataType;
|
---|
113 | };
|
---|
114 |
|
---|
115 | AhrsData::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 |
|
---|
130 | AhrsData::~AhrsData() {}
|
---|
131 |
|
---|
132 | Quaternion AhrsData::GetQuaternion(void) const {
|
---|
133 | Quaternion out;
|
---|
134 | GetMutex();
|
---|
135 | out = quaternion;
|
---|
136 | ReleaseMutex();
|
---|
137 | return out;
|
---|
138 | }
|
---|
139 |
|
---|
140 | Vector3Df AhrsData::GetAngularRates(void) const {
|
---|
141 | Vector3Df out;
|
---|
142 | GetMutex();
|
---|
143 | out = angularRates;
|
---|
144 | ReleaseMutex();
|
---|
145 | return out;
|
---|
146 | }
|
---|
147 |
|
---|
148 | void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,
|
---|
149 | Vector3Df &outAngularRates) const {
|
---|
150 | GetMutex();
|
---|
151 | outQuaternion = quaternion;
|
---|
152 | outAngularRates = angularRates;
|
---|
153 | ReleaseMutex();
|
---|
154 | }
|
---|
155 |
|
---|
156 | void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,
|
---|
157 | const Vector3Df &inAngularRates) {
|
---|
158 | GetMutex();
|
---|
159 | quaternion = inQuaternion;
|
---|
160 | angularRates = inAngularRates;
|
---|
161 | ReleaseMutex();
|
---|
162 | }
|
---|
163 |
|
---|
164 | void AhrsData::SetQuaternion(const Quaternion &inQuaternion) {
|
---|
165 | GetMutex();
|
---|
166 | quaternion = inQuaternion;
|
---|
167 | ReleaseMutex();
|
---|
168 | }
|
---|
169 |
|
---|
170 | void AhrsData::SetAngularRates(const Vector3Df &inAngularRates) {
|
---|
171 | GetMutex();
|
---|
172 | angularRates = inAngularRates;
|
---|
173 | ReleaseMutex();
|
---|
174 | }
|
---|
175 |
|
---|
176 | IODataElement *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 |
|
---|
234 | void 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 |
|
---|
247 | void 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
|
---|