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