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