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

Last change on this file since 252 was 252, checked in by Sanahuja Guillaume, 3 years ago

change io_data CopyDate? to RawRead?

File size: 5.8 KB
RevLine 
[2]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[2]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
[15]26namespace flair {
27namespace core {
[2]28
29/*! \class AhrsDataElement
30    */
[15]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) {
[2]37
[15]38    size = sizeof(float);
39  }
[2]40
[15]41  ~AhrsDataElement() {}
[2]42
[15]43  void CopyData(char *destination) const {
44    float data;
[167]45    Vector3Df angularRates;
[15]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  }
[2]106
[15]107  const FloatType &GetDataType(void) const { return dataType; }
[2]108
[15]109private:
110  const AhrsData *ahrsData;
111  AhrsData::PlotableData_t plotableData;
112  FloatType dataType;
113};
[2]114
[15]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");
[2]119
[15]120  AppendLogDescription("q0", floatType);
121  AppendLogDescription("q1", floatType);
122  AppendLogDescription("q2", floatType);
123  AppendLogDescription("q3", floatType);
[2]124
[15]125  AppendLogDescription("wx", floatType);
126  AppendLogDescription("wy", floatType);
127  AppendLogDescription("wz", floatType);
[2]128}
129
130AhrsData::~AhrsData() {}
131
132Quaternion AhrsData::GetQuaternion(void) const {
[15]133  Quaternion out;
134  GetMutex();
135  out = quaternion;
136  ReleaseMutex();
137  return out;
[2]138}
139
[167]140Vector3Df AhrsData::GetAngularRates(void) const {
141  Vector3Df out;
[15]142  GetMutex();
143  out = angularRates;
144  ReleaseMutex();
145  return out;
[2]146}
147
[15]148void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,
[167]149                                            Vector3Df &outAngularRates) const {
[15]150  GetMutex();
151  outQuaternion = quaternion;
152  outAngularRates = angularRates;
153  ReleaseMutex();
[2]154}
155
[15]156void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,
[167]157                                            const Vector3Df &inAngularRates) {
[15]158  GetMutex();
159  quaternion = inQuaternion;
160  angularRates = inAngularRates;
161  ReleaseMutex();
[2]162}
163
164void AhrsData::SetQuaternion(const Quaternion &inQuaternion) {
[15]165  GetMutex();
166  quaternion = inQuaternion;
167  ReleaseMutex();
[2]168}
169
[167]170void AhrsData::SetAngularRates(const Vector3Df &inAngularRates) {
[15]171  GetMutex();
172  angularRates = inAngularRates;
173  ReleaseMutex();
[2]174}
175
[15]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  }
[2]230
[15]231  return new AhrsDataElement(this, name, plotableData);
[2]232}
233
[252]234void AhrsData::RawRead(char *dst) const {
[15]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));
[2]240
[15]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();
[2]245}
246
[15]247void AhrsData::Queue(char **dst, const void *src, size_t size) const {
248  memcpy(*dst, src, size);
249  *dst += size;
[2]250}
251
252} // end namespace core
253} // end namespace flair
Note: See TracBrowser for help on using the repository browser.