Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

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?

UserRevisionLine numberNew 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 }