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

Last change on this file since 124 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

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    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 { 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
140Vector3D AhrsData::GetAngularRates(void) const {
141  Vector3D out;
142  GetMutex();
143  out = angularRates;
144  ReleaseMutex();
145  return out;
146}
147
148void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,
149                                            Vector3D &outAngularRates) const {
150  GetMutex();
151  outQuaternion = quaternion;
152  outAngularRates = angularRates;
153  ReleaseMutex();
154}
155
156void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,
157                                            const Vector3D &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 Vector3D &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::CopyDatas(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.