[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: 2014/01/15
|
---|
| 6 | // filename: ImuData.cpp
|
---|
| 7 | //
|
---|
| 8 | // author: Guillaume Sanahuja
|
---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 10 | //
|
---|
| 11 | // version: $Id: $
|
---|
| 12 | //
|
---|
| 13 | // purpose: class defining imu datas
|
---|
| 14 | //
|
---|
| 15 | //
|
---|
| 16 | /*********************************************************************/
|
---|
| 17 |
|
---|
| 18 | #include "ImuData.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 ImuDataElement
|
---|
| 30 | */
|
---|
[15] | 31 | class ImuDataElement : public IODataElement {
|
---|
| 32 | public:
|
---|
| 33 | ImuDataElement(const ImuData *inImudata, string name,
|
---|
| 34 | ImuData::PlotableData_t inPlotableData)
|
---|
| 35 | : IODataElement(inImudata, name) {
|
---|
| 36 | imudata = inImudata;
|
---|
| 37 | plotableData = inPlotableData;
|
---|
[2] | 38 |
|
---|
[15] | 39 | size = 4;
|
---|
| 40 | }
|
---|
[2] | 41 |
|
---|
[15] | 42 | ~ImuDataElement() {}
|
---|
[2] | 43 |
|
---|
[15] | 44 | void CopyData(char *dst) const {
|
---|
| 45 | float data;
|
---|
[167] | 46 | Vector3Df rawAcc = imudata->GetRawAcc();
|
---|
| 47 | Vector3Df rawMag = imudata->GetRawMag();
|
---|
| 48 | Vector3Df rawGyr = imudata->GetRawGyr();
|
---|
[15] | 49 | switch (plotableData) {
|
---|
| 50 | case ImuData::RawAx:
|
---|
| 51 | data = rawAcc.x;
|
---|
| 52 | break;
|
---|
| 53 | case ImuData::RawAy:
|
---|
| 54 | data = rawAcc.y;
|
---|
| 55 | break;
|
---|
| 56 | case ImuData::RawAz:
|
---|
| 57 | data = rawAcc.z;
|
---|
| 58 | break;
|
---|
| 59 | case ImuData::RawGx:
|
---|
| 60 | data = rawGyr.x;
|
---|
| 61 | break;
|
---|
| 62 | case ImuData::RawGy:
|
---|
| 63 | data = rawGyr.y;
|
---|
| 64 | break;
|
---|
| 65 | case ImuData::RawGz:
|
---|
| 66 | data = rawGyr.z;
|
---|
| 67 | break;
|
---|
| 68 | case ImuData::RawGxDeg:
|
---|
| 69 | data = Euler::ToDegree(rawGyr.x);
|
---|
| 70 | break;
|
---|
| 71 | case ImuData::RawGyDeg:
|
---|
| 72 | data = Euler::ToDegree(rawGyr.y);
|
---|
| 73 | break;
|
---|
| 74 | case ImuData::RawGzDeg:
|
---|
| 75 | data = Euler::ToDegree(rawGyr.z);
|
---|
| 76 | break;
|
---|
| 77 | case ImuData::RawMx:
|
---|
| 78 | data = rawMag.x;
|
---|
| 79 | break;
|
---|
| 80 | case ImuData::RawMy:
|
---|
| 81 | data = rawMag.y;
|
---|
| 82 | break;
|
---|
| 83 | case ImuData::RawMz:
|
---|
| 84 | data = rawMag.z;
|
---|
| 85 | break;
|
---|
| 86 | default:
|
---|
| 87 | imudata->Err("data type unavailable.\n");
|
---|
| 88 | data = 0;
|
---|
| 89 | break;
|
---|
| 90 | }
|
---|
[2] | 91 |
|
---|
[15] | 92 | memcpy(dst, &data, sizeof(float));
|
---|
| 93 | }
|
---|
[2] | 94 |
|
---|
[15] | 95 | FloatType const &GetDataType(void) const { return dataType; }
|
---|
[2] | 96 |
|
---|
[15] | 97 | private:
|
---|
| 98 | const ImuData *imudata;
|
---|
| 99 | ImuData::PlotableData_t plotableData;
|
---|
| 100 | FloatType dataType;
|
---|
| 101 | };
|
---|
[2] | 102 |
|
---|
[15] | 103 | ImuData::ImuData(const Object *parent, std::string name, int n)
|
---|
| 104 | : io_data(parent, name, n), dataType(floatType) {
|
---|
| 105 | if (n > 1)
|
---|
| 106 | Warn("n>1 not supported\n");
|
---|
[2] | 107 |
|
---|
[15] | 108 | AppendLogDescription("raw_ax", floatType);
|
---|
| 109 | AppendLogDescription("raw_ay", floatType);
|
---|
| 110 | AppendLogDescription("raw_az", floatType);
|
---|
[2] | 111 |
|
---|
[15] | 112 | AppendLogDescription("raw_gx", floatType);
|
---|
| 113 | AppendLogDescription("raw_gy", floatType);
|
---|
| 114 | AppendLogDescription("raw_gz", floatType);
|
---|
[2] | 115 |
|
---|
[15] | 116 | AppendLogDescription("raw_mx", floatType);
|
---|
| 117 | AppendLogDescription("raw_my", floatType);
|
---|
| 118 | AppendLogDescription("raw_mz", floatType);
|
---|
[2] | 119 | }
|
---|
| 120 |
|
---|
| 121 | ImuData::~ImuData() {}
|
---|
| 122 |
|
---|
[167] | 123 | Vector3Df ImuData::GetRawAcc(void) const {
|
---|
| 124 | Vector3Df out;
|
---|
[15] | 125 | GetMutex();
|
---|
| 126 | out = rawAcc;
|
---|
| 127 | ReleaseMutex();
|
---|
| 128 | return out;
|
---|
[2] | 129 | }
|
---|
| 130 |
|
---|
[167] | 131 | Vector3Df ImuData::GetRawMag(void) const {
|
---|
| 132 | Vector3Df out;
|
---|
[15] | 133 | GetMutex();
|
---|
| 134 | out = rawMag;
|
---|
| 135 | ReleaseMutex();
|
---|
| 136 | return out;
|
---|
[2] | 137 | }
|
---|
| 138 |
|
---|
[167] | 139 | Vector3Df ImuData::GetRawGyr(void) const {
|
---|
| 140 | Vector3Df out;
|
---|
[15] | 141 | GetMutex();
|
---|
| 142 | out = rawGyr;
|
---|
| 143 | ReleaseMutex();
|
---|
| 144 | return out;
|
---|
[2] | 145 | }
|
---|
| 146 |
|
---|
[167] | 147 | void ImuData::GetRawAccMagAndGyr(Vector3Df &inRawAcc, Vector3Df &inRawMag,
|
---|
| 148 | Vector3Df &inRawGyr) const {
|
---|
[15] | 149 | GetMutex();
|
---|
| 150 | inRawAcc = rawAcc;
|
---|
| 151 | inRawMag = rawMag;
|
---|
| 152 | inRawGyr = rawGyr;
|
---|
| 153 | ReleaseMutex();
|
---|
[2] | 154 | }
|
---|
| 155 |
|
---|
[167] | 156 | void ImuData::SetRawAcc(const Vector3Df &inRawAcc) {
|
---|
[15] | 157 | GetMutex();
|
---|
| 158 | rawAcc = inRawAcc;
|
---|
| 159 | ReleaseMutex();
|
---|
[2] | 160 | }
|
---|
| 161 |
|
---|
[167] | 162 | void ImuData::SetRawMag(const Vector3Df &inRawMag) {
|
---|
[15] | 163 | GetMutex();
|
---|
| 164 | rawMag = inRawMag;
|
---|
| 165 | ReleaseMutex();
|
---|
[2] | 166 | }
|
---|
| 167 |
|
---|
[167] | 168 | void ImuData::SetRawGyr(const Vector3Df &inRawGyr) {
|
---|
[15] | 169 | GetMutex();
|
---|
| 170 | rawGyr = inRawGyr;
|
---|
| 171 | ReleaseMutex();
|
---|
[2] | 172 | }
|
---|
| 173 |
|
---|
[167] | 174 | void ImuData::SetRawAccMagAndGyr(const Vector3Df &inRawAcc,
|
---|
| 175 | const Vector3Df &inRawMag,
|
---|
| 176 | const Vector3Df &inRawGyr) {
|
---|
[15] | 177 | GetMutex();
|
---|
| 178 | rawAcc = inRawAcc;
|
---|
| 179 | rawMag = inRawMag;
|
---|
| 180 | rawGyr = inRawGyr;
|
---|
| 181 | ReleaseMutex();
|
---|
[2] | 182 | }
|
---|
| 183 |
|
---|
[15] | 184 | IODataElement *ImuData::Element(PlotableData_t data_type) const {
|
---|
| 185 | string name;
|
---|
| 186 | switch (data_type) {
|
---|
| 187 | case ImuData::RawAx:
|
---|
| 188 | name = "RawAx";
|
---|
| 189 | break;
|
---|
| 190 | case ImuData::RawAy:
|
---|
| 191 | name = "RawAy";
|
---|
| 192 | break;
|
---|
| 193 | case ImuData::RawAz:
|
---|
| 194 | name = "RawAz";
|
---|
| 195 | break;
|
---|
| 196 | case ImuData::RawGx:
|
---|
| 197 | name = "RawGx";
|
---|
| 198 | break;
|
---|
| 199 | case ImuData::RawGy:
|
---|
| 200 | name = "RawGy";
|
---|
| 201 | break;
|
---|
| 202 | case ImuData::RawGz:
|
---|
| 203 | name = "RawGz";
|
---|
| 204 | break;
|
---|
| 205 | case ImuData::RawGxDeg:
|
---|
| 206 | name = "RawGx degree";
|
---|
| 207 | break;
|
---|
| 208 | case ImuData::RawGyDeg:
|
---|
| 209 | name = "RawGy degree";
|
---|
| 210 | break;
|
---|
| 211 | case ImuData::RawGzDeg:
|
---|
| 212 | name = "RawGz degree";
|
---|
| 213 | break;
|
---|
| 214 | case ImuData::RawMx:
|
---|
| 215 | name = "RawMx";
|
---|
| 216 | break;
|
---|
| 217 | case ImuData::RawMy:
|
---|
| 218 | name = "RawMy";
|
---|
| 219 | break;
|
---|
| 220 | case ImuData::RawMz:
|
---|
| 221 | name = "RawMz";
|
---|
| 222 | break;
|
---|
| 223 | }
|
---|
[2] | 224 |
|
---|
[15] | 225 | return new ImuDataElement(this, name, data_type);
|
---|
[2] | 226 | }
|
---|
| 227 |
|
---|
[252] | 228 | void ImuData::RawRead(char *dst) const {
|
---|
[15] | 229 | GetMutex();
|
---|
[2] | 230 |
|
---|
[15] | 231 | Queue(&dst, &rawAcc.x, sizeof(rawAcc.x));
|
---|
| 232 | Queue(&dst, &rawAcc.y, sizeof(rawAcc.y));
|
---|
| 233 | Queue(&dst, &rawAcc.z, sizeof(rawAcc.z));
|
---|
[2] | 234 |
|
---|
[15] | 235 | Queue(&dst, &rawGyr.x, sizeof(rawGyr.x));
|
---|
| 236 | Queue(&dst, &rawGyr.y, sizeof(rawGyr.y));
|
---|
| 237 | Queue(&dst, &rawGyr.z, sizeof(rawGyr.z));
|
---|
[2] | 238 |
|
---|
[15] | 239 | Queue(&dst, &rawMag.x, sizeof(rawMag.x));
|
---|
| 240 | Queue(&dst, &rawMag.y, sizeof(rawMag.y));
|
---|
| 241 | Queue(&dst, &rawMag.z, sizeof(rawMag.z));
|
---|
[2] | 242 |
|
---|
[15] | 243 | ReleaseMutex();
|
---|
[2] | 244 | }
|
---|
| 245 |
|
---|
[15] | 246 | void ImuData::Queue(char **dst, const void *src, size_t size) const {
|
---|
| 247 | memcpy(*dst, src, size);
|
---|
| 248 | *dst += size;
|
---|
[2] | 249 | }
|
---|
| 250 |
|
---|
| 251 | } // end namespace core
|
---|
| 252 | } // end namespace flair
|
---|