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