Changeset 15 in flair-src for trunk/lib/FlairCore/src/ImuData.h
- Timestamp:
- 04/08/16 15:40:57 (8 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairCore/src/ImuData.h
r2 r15 17 17 #include <Vector3D.h> 18 18 19 namespace flair { namespace core { 20 21 /*! \class ImuData 22 * 23 * \brief Class defining IMU datas 24 * 25 * IMU (inertial measurement unit) datas consist of raw accelerometer values, raw gyrometer values 26 * and raw magnetometer values. 27 * 28 */ 29 class ImuData: public io_data { 30 public: 31 class Type: public DataType { 32 public: 33 Type(ScalarType const &_elementDataType): 34 elementDataType(_elementDataType){} 35 ScalarType const &GetElementDataType() const {return elementDataType;} 36 std::string GetDescription() const {return "imu data";} 37 size_t GetSize() const { 38 size_t size=0; 39 size+=3*elementDataType.GetSize();//RawAcc 40 size+=3*elementDataType.GetSize();//RawGyr 41 size+=3*elementDataType.GetSize();//RawMag 42 return size; 43 } 44 private: 45 ScalarType const &elementDataType; 46 }; 47 48 /*! 49 \enum PlotableData_t 50 \brief Datas wich can be plotted in a DataPlot1D 51 */ 52 typedef enum { 53 RawAx/*! x raw accelerometer */, RawAy/*! y raw accelerometer */ ,RawAz/*! z raw accelerometer */, 54 RawGx/*! x raw gyrometer */,RawGy/*! y raw gyrometer */,RawGz/*! z raw gyrometer */, 55 RawGxDeg/*! x raw gyrometer degree */,RawGyDeg/*! y raw gyrometer degree */,RawGzDeg/*! z raw gyrometer degree */, 56 RawMx/*! x raw magnetometer */,RawMy/*! y raw magnetometer */,RawMz/*! z raw magnetometer */ 57 } PlotableData_t; 58 59 /*! 60 * \brief Constructor 61 * 62 * Construct an io_data representing IMU datas. \n 63 * 64 * \param parent parent 65 * \param name name 66 * \param n number of samples 67 */ 68 ImuData(const Object* parent,std::string name="",int n=1); 69 70 /*! 71 * \brief Destructor 72 * 73 */ 74 ~ImuData(); 75 76 /*! 77 * \brief Element 78 * 79 * Get a pointer to a specific element. This pointer can be used for plotting. 80 * 81 * \param data_type data type 82 * 83 * \return pointer to the element 84 */ 85 IODataElement* Element(PlotableData_t data_type) const; 86 87 /*! 88 * \brief Get raw accelerations 89 * 90 * This method is mutex protected. 91 * 92 * \return raw accelerations 93 * 94 */ 95 Vector3D GetRawAcc(void) const; 96 97 /*! 98 * \brief Get raw magnetometers 99 * 100 * This method is mutex protected. 101 * 102 * \return raw magnetometers 103 * 104 */ 105 Vector3D GetRawMag(void) const; 106 107 /*! 108 * \brief Get raw angular speed 109 * 110 * This method is mutex protected. 111 * 112 * \return raw angular speed 113 * 114 */ 115 Vector3D GetRawGyr(void) const; 116 117 /*! 118 * \brief Get raw accelerations, magnetometers and angular speeds 119 * 120 * This method is mutex protected. 121 * 122 * \param rawAcc raw accelerations 123 * \param rawMag raw magnetometers 124 * \param rawGyr raw angular speeds 125 * 126 */ 127 void GetRawAccMagAndGyr(Vector3D &rawAcc,Vector3D &rawMag,Vector3D &rawGyr) const; 128 129 /*! 130 * \brief Set raw accelerations 131 * 132 * This method is mutex protected. 133 * 134 * \param raw accelerations 135 * 136 */ 137 void SetRawAcc(const Vector3D &rawAcc); 138 139 /*! 140 * \brief Set raw magnetometers 141 * 142 * This method is mutex protected. 143 * 144 * \param raw magnetometers 145 * 146 */ 147 void SetRawMag(const Vector3D &rawMag); 148 149 /*! 150 * \brief Set raw angular speed 151 * 152 * This method is mutex protected. 153 * 154 * \param raw angular speed 155 * 156 */ 157 void SetRawGyr(const Vector3D &rawGyr); 158 159 /*! 160 * \brief Set raw accelerations, magnetometers and angular speeds 161 * 162 * This method is mutex protected. 163 * 164 * \param rawAcc raw accelerations 165 * \param rawMag raw magnetometers 166 * \param rawGyr raw angular speeds 167 * 168 */ 169 void SetRawAccMagAndGyr(const Vector3D &rawAcc,const Vector3D &rawMag,const Vector3D &rawGyr); 170 171 Type const&GetDataType() const {return dataType;} 172 private: 173 /*! 174 * \brief Copy datas 175 * 176 * Reimplemented from io_data. \n 177 * See io_data::CopyDatas. 178 * 179 * \param dst destination buffer 180 */ 181 void CopyDatas(char* dst) const; 182 183 /*! 184 * \brief Raw accelerometer 185 * 186 */ 187 Vector3D rawAcc; 188 189 /*! 190 * \brief Raw gyrometer 191 * 192 */ 193 Vector3D rawGyr; 194 195 /*! 196 * \brief Raw magnetometer 197 * 198 */ 199 Vector3D rawMag; 200 201 void Queue(char** dst,const void *src,size_t size) const; 202 Type dataType; 203 204 }; 19 namespace flair { 20 namespace core { 21 22 /*! \class ImuData 23 * 24 * \brief Class defining IMU datas 25 * 26 * IMU (inertial measurement unit) datas consist of raw accelerometer values, raw 27 *gyrometer values 28 * and raw magnetometer values. 29 * 30 */ 31 class ImuData : public io_data { 32 public: 33 class Type : public DataType { 34 public: 35 Type(ScalarType const &_elementDataType) 36 : elementDataType(_elementDataType) {} 37 ScalarType const &GetElementDataType() const { return elementDataType; } 38 std::string GetDescription() const { return "imu data"; } 39 size_t GetSize() const { 40 size_t size = 0; 41 size += 3 * elementDataType.GetSize(); // RawAcc 42 size += 3 * elementDataType.GetSize(); // RawGyr 43 size += 3 * elementDataType.GetSize(); // RawMag 44 return size; 45 } 46 47 private: 48 ScalarType const &elementDataType; 49 }; 50 51 /*! 52 \enum PlotableData_t 53 \brief Datas wich can be plotted in a DataPlot1D 54 */ 55 typedef enum { 56 RawAx /*! x raw accelerometer */, 57 RawAy /*! y raw accelerometer */, 58 RawAz /*! z raw accelerometer */, 59 RawGx /*! x raw gyrometer */, 60 RawGy /*! y raw gyrometer */, 61 RawGz /*! z raw gyrometer */, 62 RawGxDeg /*! x raw gyrometer degree */, 63 RawGyDeg /*! y raw gyrometer degree */, 64 RawGzDeg /*! z raw gyrometer degree */, 65 RawMx /*! x raw magnetometer */, 66 RawMy /*! y raw magnetometer */, 67 RawMz /*! z raw magnetometer */ 68 } PlotableData_t; 69 70 /*! 71 * \brief Constructor 72 * 73 * Construct an io_data representing IMU datas. \n 74 * 75 * \param parent parent 76 * \param name name 77 * \param n number of samples 78 */ 79 ImuData(const Object *parent, std::string name = "", int n = 1); 80 81 /*! 82 * \brief Destructor 83 * 84 */ 85 ~ImuData(); 86 87 /*! 88 * \brief Element 89 * 90 * Get a pointer to a specific element. This pointer can be used for plotting. 91 * 92 * \param data_type data type 93 * 94 * \return pointer to the element 95 */ 96 IODataElement *Element(PlotableData_t data_type) const; 97 98 /*! 99 * \brief Get raw accelerations 100 * 101 * This method is mutex protected. 102 * 103 * \return raw accelerations 104 * 105 */ 106 Vector3D GetRawAcc(void) const; 107 108 /*! 109 * \brief Get raw magnetometers 110 * 111 * This method is mutex protected. 112 * 113 * \return raw magnetometers 114 * 115 */ 116 Vector3D GetRawMag(void) const; 117 118 /*! 119 * \brief Get raw angular speed 120 * 121 * This method is mutex protected. 122 * 123 * \return raw angular speed 124 * 125 */ 126 Vector3D GetRawGyr(void) const; 127 128 /*! 129 * \brief Get raw accelerations, magnetometers and angular speeds 130 * 131 * This method is mutex protected. 132 * 133 * \param rawAcc raw accelerations 134 * \param rawMag raw magnetometers 135 * \param rawGyr raw angular speeds 136 * 137 */ 138 void GetRawAccMagAndGyr(Vector3D &rawAcc, Vector3D &rawMag, 139 Vector3D &rawGyr) const; 140 141 /*! 142 * \brief Set raw accelerations 143 * 144 * This method is mutex protected. 145 * 146 * \param raw accelerations 147 * 148 */ 149 void SetRawAcc(const Vector3D &rawAcc); 150 151 /*! 152 * \brief Set raw magnetometers 153 * 154 * This method is mutex protected. 155 * 156 * \param raw magnetometers 157 * 158 */ 159 void SetRawMag(const Vector3D &rawMag); 160 161 /*! 162 * \brief Set raw angular speed 163 * 164 * This method is mutex protected. 165 * 166 * \param raw angular speed 167 * 168 */ 169 void SetRawGyr(const Vector3D &rawGyr); 170 171 /*! 172 * \brief Set raw accelerations, magnetometers and angular speeds 173 * 174 * This method is mutex protected. 175 * 176 * \param rawAcc raw accelerations 177 * \param rawMag raw magnetometers 178 * \param rawGyr raw angular speeds 179 * 180 */ 181 void SetRawAccMagAndGyr(const Vector3D &rawAcc, const Vector3D &rawMag, 182 const Vector3D &rawGyr); 183 184 Type const &GetDataType() const { return dataType; } 185 186 private: 187 /*! 188 * \brief Copy datas 189 * 190 * Reimplemented from io_data. \n 191 * See io_data::CopyDatas. 192 * 193 * \param dst destination buffer 194 */ 195 void CopyDatas(char *dst) const; 196 197 /*! 198 * \brief Raw accelerometer 199 * 200 */ 201 Vector3D rawAcc; 202 203 /*! 204 * \brief Raw gyrometer 205 * 206 */ 207 Vector3D rawGyr; 208 209 /*! 210 * \brief Raw magnetometer 211 * 212 */ 213 Vector3D rawMag; 214 215 void Queue(char **dst, const void *src, size_t size) const; 216 Type dataType; 217 }; 205 218 206 219 } // end namespace core
Note:
See TracChangeset
for help on using the changeset viewer.