Changeset 15 in flairsrc for trunk/lib/FlairCore/src/AhrsData.cpp
 Timestamp:
 04/08/16 15:40:57 (6 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/lib/FlairCore/src/AhrsData.cpp
r2 r15 24 24 using std::string; 25 25 26 namespace flair { namespace core { 26 namespace flair { 27 namespace core { 27 28 28 29 /*! \class AhrsDataElement 29 30 */ 30 class AhrsDataElement: public IODataElement { 31 public: 32 33 AhrsDataElement(const AhrsData *inAhrsData,string name,AhrsData::PlotableData_t inPlotableData): 34 IODataElement(inAhrsData,name), 35 ahrsData(inAhrsData), 36 plotableData(inPlotableData) { 37 38 size=sizeof(float); 39 } 40 41 ~AhrsDataElement() {} 42 43 void CopyData(char* destination) const { 44 float data; 45 Vector3D angularRates; 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 } 106 107 const FloatType &GetDataType(void) const { 108 return dataType; 109 } 110 111 private: 112 const AhrsData *ahrsData; 113 AhrsData::PlotableData_t plotableData; 114 FloatType dataType; 115 116 }; 117 118 AhrsData::AhrsData(const Object *parent,std::string name,int n): io_data(parent,name,n), dataType(floatType) { 119 if(n>1) Warn("n>1 not supported\n"); 120 121 AppendLogDescription("q0",floatType); 122 AppendLogDescription("q1",floatType); 123 AppendLogDescription("q2",floatType); 124 AppendLogDescription("q3",floatType); 125 126 AppendLogDescription("wx",floatType); 127 AppendLogDescription("wy",floatType); 128 AppendLogDescription("wz",floatType); 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) { 37 38 size = sizeof(float); 39 } 40 41 ~AhrsDataElement() {} 42 43 void CopyData(char *destination) const { 44 float data; 45 Vector3D angularRates; 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 } 106 107 const FloatType &GetDataType(void) const { return dataType; } 108 109 private: 110 const AhrsData *ahrsData; 111 AhrsData::PlotableData_t plotableData; 112 FloatType dataType; 113 }; 114 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"); 119 120 AppendLogDescription("q0", floatType); 121 AppendLogDescription("q1", floatType); 122 AppendLogDescription("q2", floatType); 123 AppendLogDescription("q3", floatType); 124 125 AppendLogDescription("wx", floatType); 126 AppendLogDescription("wy", floatType); 127 AppendLogDescription("wz", floatType); 129 128 } 130 129 … … 132 131 133 132 Quaternion AhrsData::GetQuaternion(void) const { 134 135 136 out=quaternion;137 138 133 Quaternion out; 134 GetMutex(); 135 out = quaternion; 136 ReleaseMutex(); 137 return out; 139 138 } 140 139 141 140 Vector3D AhrsData::GetAngularRates(void) const { 142 Vector3D out; 143 GetMutex(); 144 out=angularRates; 145 ReleaseMutex(); 146 return out; 147 } 148 149 void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,Vector3D &outAngularRates) const { 150 GetMutex(); 151 outQuaternion=quaternion; 152 outAngularRates=angularRates; 153 ReleaseMutex(); 154 } 155 156 void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,const Vector3D &inAngularRates) { 157 GetMutex(); 158 quaternion=inQuaternion; 159 angularRates=inAngularRates; 160 ReleaseMutex(); 141 Vector3D out; 142 GetMutex(); 143 out = angularRates; 144 ReleaseMutex(); 145 return out; 146 } 147 148 void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion, 149 Vector3D &outAngularRates) const { 150 GetMutex(); 151 outQuaternion = quaternion; 152 outAngularRates = angularRates; 153 ReleaseMutex(); 154 } 155 156 void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion, 157 const Vector3D &inAngularRates) { 158 GetMutex(); 159 quaternion = inQuaternion; 160 angularRates = inAngularRates; 161 ReleaseMutex(); 161 162 } 162 163 163 164 void AhrsData::SetQuaternion(const Quaternion &inQuaternion) { 164 165 quaternion=inQuaternion;166 165 GetMutex(); 166 quaternion = inQuaternion; 167 ReleaseMutex(); 167 168 } 168 169 169 170 void AhrsData::SetAngularRates(const Vector3D &inAngularRates) { 170 171 angularRates=inAngularRates;172 173 } 174 175 IODataElement *AhrsData::Element(PlotableData_t plotableData) const {176 177 switch(plotableData) {178 179 name="Roll";180 181 182 name="Pitch";183 184 185 name="Yaw";186 187 188 name="Roll degree";189 190 191 name="Pitch degree";192 193 194 name="Yaw degree";195 196 197 name="Q0";198 199 200 name="Q1";201 202 203 name="Q2";204 205 206 name="Q3";207 208 209 name="Wx";210 211 212 name="Wy";213 214 215 name="Wz";216 217 218 name="Wx degree";219 220 221 name="Wy degree";222 223 224 name="Wz degree";225 226 227 228 229 230 return new AhrsDataElement(this,name,plotableData);171 GetMutex(); 172 angularRates = inAngularRates; 173 ReleaseMutex(); 174 } 175 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 } 230 231 return new AhrsDataElement(this, name, plotableData); 231 232 } 232 233 233 234 void AhrsData::CopyDatas(char *dst) const { 234 235 Queue(&dst,&quaternion.q0,sizeof(quaternion.q0));236 Queue(&dst,&quaternion.q1,sizeof(quaternion.q1));237 Queue(&dst,&quaternion.q2,sizeof(quaternion.q2));238 Queue(&dst,&quaternion.q3,sizeof(quaternion.q3));239 240 Queue(&dst,&angularRates.x,sizeof(angularRates.x));241 Queue(&dst,&angularRates.y,sizeof(angularRates.y));242 Queue(&dst,&angularRates.z,sizeof(angularRates.z));243 244 } 245 246 void AhrsData::Queue(char **dst, const void *src,size_t size) const {247 memcpy(*dst,src,size);248 *dst+=size;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)); 240 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(); 245 } 246 247 void AhrsData::Queue(char **dst, const void *src, size_t size) const { 248 memcpy(*dst, src, size); 249 *dst += size; 249 250 } 250 251
Note:
See TracChangeset
for help on using the changeset viewer.