AHRS library, modified version of Peter Bartz work.
Math.cpp@1:da3b20b5d38a, 2012-11-08 (annotated)
- Committer:
- tylerjw
- Date:
- Thu Nov 08 20:13:35 2012 +0000
- Revision:
- 1:da3b20b5d38a
- Parent:
- 0:014ee3239c80
Modified AHRS library.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tylerjw | 0:014ee3239c80 | 1 | /* This file is part of the Razor AHRS Firmware */ |
tylerjw | 0:014ee3239c80 | 2 | #include <math.h> |
tylerjw | 0:014ee3239c80 | 3 | |
tylerjw | 0:014ee3239c80 | 4 | // Computes the dot product of two vectors |
tylerjw | 0:014ee3239c80 | 5 | float Vector_Dot_Product(float vector1[3], float vector2[3]) |
tylerjw | 0:014ee3239c80 | 6 | { |
tylerjw | 0:014ee3239c80 | 7 | float op=0; |
tylerjw | 0:014ee3239c80 | 8 | |
tylerjw | 0:014ee3239c80 | 9 | for(int c=0; c<3; c++) |
tylerjw | 0:014ee3239c80 | 10 | { |
tylerjw | 0:014ee3239c80 | 11 | op+=vector1[c]*vector2[c]; |
tylerjw | 0:014ee3239c80 | 12 | } |
tylerjw | 0:014ee3239c80 | 13 | |
tylerjw | 0:014ee3239c80 | 14 | return op; |
tylerjw | 0:014ee3239c80 | 15 | } |
tylerjw | 0:014ee3239c80 | 16 | |
tylerjw | 0:014ee3239c80 | 17 | // Computes the cross product of two vectors |
tylerjw | 0:014ee3239c80 | 18 | void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]) |
tylerjw | 0:014ee3239c80 | 19 | { |
tylerjw | 0:014ee3239c80 | 20 | vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); |
tylerjw | 0:014ee3239c80 | 21 | vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); |
tylerjw | 0:014ee3239c80 | 22 | vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); |
tylerjw | 0:014ee3239c80 | 23 | } |
tylerjw | 0:014ee3239c80 | 24 | |
tylerjw | 0:014ee3239c80 | 25 | // Multiply the vector by a scalar. |
tylerjw | 0:014ee3239c80 | 26 | void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2) |
tylerjw | 0:014ee3239c80 | 27 | { |
tylerjw | 0:014ee3239c80 | 28 | for(int c=0; c<3; c++) |
tylerjw | 0:014ee3239c80 | 29 | { |
tylerjw | 0:014ee3239c80 | 30 | vectorOut[c]=vectorIn[c]*scale2; |
tylerjw | 0:014ee3239c80 | 31 | } |
tylerjw | 0:014ee3239c80 | 32 | } |
tylerjw | 0:014ee3239c80 | 33 | |
tylerjw | 0:014ee3239c80 | 34 | // Adds two vectors |
tylerjw | 0:014ee3239c80 | 35 | void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]) |
tylerjw | 0:014ee3239c80 | 36 | { |
tylerjw | 0:014ee3239c80 | 37 | for(int c=0; c<3; c++) |
tylerjw | 0:014ee3239c80 | 38 | { |
tylerjw | 0:014ee3239c80 | 39 | vectorOut[c]=vectorIn1[c]+vectorIn2[c]; |
tylerjw | 0:014ee3239c80 | 40 | } |
tylerjw | 0:014ee3239c80 | 41 | } |
tylerjw | 0:014ee3239c80 | 42 | |
tylerjw | 0:014ee3239c80 | 43 | //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). |
tylerjw | 0:014ee3239c80 | 44 | void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) |
tylerjw | 0:014ee3239c80 | 45 | { |
tylerjw | 0:014ee3239c80 | 46 | float op[3]; |
tylerjw | 0:014ee3239c80 | 47 | for(int x=0; x<3; x++) |
tylerjw | 0:014ee3239c80 | 48 | { |
tylerjw | 0:014ee3239c80 | 49 | for(int y=0; y<3; y++) |
tylerjw | 0:014ee3239c80 | 50 | { |
tylerjw | 0:014ee3239c80 | 51 | for(int w=0; w<3; w++) |
tylerjw | 0:014ee3239c80 | 52 | { |
tylerjw | 0:014ee3239c80 | 53 | op[w]=a[x][w]*b[w][y]; |
tylerjw | 0:014ee3239c80 | 54 | } |
tylerjw | 0:014ee3239c80 | 55 | mat[x][y]=0; |
tylerjw | 0:014ee3239c80 | 56 | mat[x][y]=op[0]+op[1]+op[2]; |
tylerjw | 0:014ee3239c80 | 57 | |
tylerjw | 0:014ee3239c80 | 58 | //float test=mat[x][y]; |
tylerjw | 0:014ee3239c80 | 59 | } |
tylerjw | 0:014ee3239c80 | 60 | } |
tylerjw | 0:014ee3239c80 | 61 | } |
tylerjw | 0:014ee3239c80 | 62 | |
tylerjw | 0:014ee3239c80 | 63 | // Init rotation matrix using euler angles |
tylerjw | 0:014ee3239c80 | 64 | void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll) |
tylerjw | 0:014ee3239c80 | 65 | { |
tylerjw | 0:014ee3239c80 | 66 | float c1 = cos(roll); |
tylerjw | 0:014ee3239c80 | 67 | float s1 = sin(roll); |
tylerjw | 0:014ee3239c80 | 68 | float c2 = cos(pitch); |
tylerjw | 0:014ee3239c80 | 69 | float s2 = sin(pitch); |
tylerjw | 0:014ee3239c80 | 70 | float c3 = cos(yaw); |
tylerjw | 0:014ee3239c80 | 71 | float s3 = sin(yaw); |
tylerjw | 0:014ee3239c80 | 72 | |
tylerjw | 0:014ee3239c80 | 73 | // Euler angles, right-handed, intrinsic, XYZ convention |
tylerjw | 0:014ee3239c80 | 74 | // (which means: rotate around body axes Z, Y', X'') |
tylerjw | 0:014ee3239c80 | 75 | m[0][0] = c2 * c3; |
tylerjw | 0:014ee3239c80 | 76 | m[0][1] = c3 * s1 * s2 - c1 * s3; |
tylerjw | 0:014ee3239c80 | 77 | m[0][2] = s1 * s3 + c1 * c3 * s2; |
tylerjw | 0:014ee3239c80 | 78 | |
tylerjw | 0:014ee3239c80 | 79 | m[1][0] = c2 * s3; |
tylerjw | 0:014ee3239c80 | 80 | m[1][1] = c1 * c3 + s1 * s2 * s3; |
tylerjw | 0:014ee3239c80 | 81 | m[1][2] = c1 * s2 * s3 - c3 * s1; |
tylerjw | 0:014ee3239c80 | 82 | |
tylerjw | 0:014ee3239c80 | 83 | m[2][0] = -s2; |
tylerjw | 0:014ee3239c80 | 84 | m[2][1] = c2 * s1; |
tylerjw | 0:014ee3239c80 | 85 | m[2][2] = c1 * c2; |
tylerjw | 0:014ee3239c80 | 86 | } |
tylerjw | 0:014ee3239c80 | 87 | |
tylerjw | 0:014ee3239c80 | 88 | float constrain(float in, float min, float max) |
tylerjw | 0:014ee3239c80 | 89 | { |
tylerjw | 0:014ee3239c80 | 90 | in = in > max ? max : in; |
tylerjw | 0:014ee3239c80 | 91 | in = in < min ? min : in; |
tylerjw | 0:014ee3239c80 | 92 | return in; |
tylerjw | 0:014ee3239c80 | 93 | } |