DCM Code ported from Arduino for FRDM-KL25Z
Dependents: minimu_data_capture minimu_data_capture
Fork of DCM_AHRS by
Matrix.cpp@1:3272ece36ce1, 2012-04-23 (annotated)
- Committer:
- krmreynolds
- Date:
- Mon Apr 23 14:31:08 2012 +0000
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
Ported L3G4200D and LSM303 from the Polulu website, dropped the horrible code that someone else built
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krmreynolds | 1:3272ece36ce1 | 1 | |
krmreynolds | 1:3272ece36ce1 | 2 | //Computes the dot product of two vectors |
krmreynolds | 1:3272ece36ce1 | 3 | float vector_dot_product(float vector1[3],float vector2[3]) { |
krmreynolds | 1:3272ece36ce1 | 4 | float op=0; |
krmreynolds | 1:3272ece36ce1 | 5 | |
krmreynolds | 1:3272ece36ce1 | 6 | for (int c=0; c<3; c++) { |
krmreynolds | 1:3272ece36ce1 | 7 | op+=vector1[c]*vector2[c]; |
krmreynolds | 1:3272ece36ce1 | 8 | } |
krmreynolds | 1:3272ece36ce1 | 9 | |
krmreynolds | 1:3272ece36ce1 | 10 | return op; |
krmreynolds | 1:3272ece36ce1 | 11 | } |
krmreynolds | 1:3272ece36ce1 | 12 | |
krmreynolds | 1:3272ece36ce1 | 13 | //Computes the cross product of two vectors |
krmreynolds | 1:3272ece36ce1 | 14 | void vector_cross_product(float vectorOut[3], float v1[3],float v2[3]) { |
krmreynolds | 1:3272ece36ce1 | 15 | vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); |
krmreynolds | 1:3272ece36ce1 | 16 | vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); |
krmreynolds | 1:3272ece36ce1 | 17 | vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); |
krmreynolds | 1:3272ece36ce1 | 18 | } |
krmreynolds | 1:3272ece36ce1 | 19 | |
krmreynolds | 1:3272ece36ce1 | 20 | //Multiply the vector by a scalar. |
krmreynolds | 1:3272ece36ce1 | 21 | void vector_scale(float vectorOut[3],float vectorIn[3], float scale2) { |
krmreynolds | 1:3272ece36ce1 | 22 | for (int c=0; c<3; c++) { |
krmreynolds | 1:3272ece36ce1 | 23 | vectorOut[c]=vectorIn[c]*scale2; |
krmreynolds | 1:3272ece36ce1 | 24 | } |
krmreynolds | 1:3272ece36ce1 | 25 | } |
krmreynolds | 1:3272ece36ce1 | 26 | |
krmreynolds | 1:3272ece36ce1 | 27 | void vector_add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) { |
krmreynolds | 1:3272ece36ce1 | 28 | for (int c=0; c<3; c++) { |
krmreynolds | 1:3272ece36ce1 | 29 | vectorOut[c]=vectorIn1[c]+vectorIn2[c]; |
krmreynolds | 1:3272ece36ce1 | 30 | } |
krmreynolds | 1:3272ece36ce1 | 31 | } |
krmreynolds | 1:3272ece36ce1 | 32 | |
krmreynolds | 1:3272ece36ce1 | 33 | //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). |
krmreynolds | 1:3272ece36ce1 | 34 | void matrix_multiply(float a[3][3], float b[3][3],float mat[3][3]) { |
krmreynolds | 1:3272ece36ce1 | 35 | float op[3]; |
krmreynolds | 1:3272ece36ce1 | 36 | for (int x=0; x<3; x++) { |
krmreynolds | 1:3272ece36ce1 | 37 | for (int y=0; y<3; y++) { |
krmreynolds | 1:3272ece36ce1 | 38 | for (int w=0; w<3; w++) { |
krmreynolds | 1:3272ece36ce1 | 39 | op[w]=a[x][w]*b[w][y]; |
krmreynolds | 1:3272ece36ce1 | 40 | } |
krmreynolds | 1:3272ece36ce1 | 41 | mat[x][y]=0; |
krmreynolds | 1:3272ece36ce1 | 42 | mat[x][y]=op[0]+op[1]+op[2]; |
krmreynolds | 1:3272ece36ce1 | 43 | |
krmreynolds | 1:3272ece36ce1 | 44 | //float test=mat[x][y]; |
krmreynolds | 1:3272ece36ce1 | 45 | } |
krmreynolds | 1:3272ece36ce1 | 46 | } |
krmreynolds | 0:dc35364e2291 | 47 | } |