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
- Committer:
- lpetre
- Date:
- 2011-12-27
- Revision:
- 0:9a72d42c0da3
File content as of revision 0:9a72d42c0da3:
/* This file is part of the Razor AHRS Firmware */ #include <math.h> // Computes the dot product of two vectors float Vector_Dot_Product(float vector1[3], float vector2[3]) { float op=0; for(int c=0; c<3; c++) { op+=vector1[c]*vector2[c]; } return op; } // Computes the cross product of two vectors void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]) { vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); } // Multiply the vector by a scalar. void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2) { for(int c=0; c<3; c++) { vectorOut[c]=vectorIn[c]*scale2; } } // Adds two vectors void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]) { for(int c=0; c<3; c++) { vectorOut[c]=vectorIn1[c]+vectorIn2[c]; } } //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) { float op[3]; for(int x=0; x<3; x++) { for(int y=0; y<3; y++) { for(int w=0; w<3; w++) { op[w]=a[x][w]*b[w][y]; } mat[x][y]=0; mat[x][y]=op[0]+op[1]+op[2]; float test=mat[x][y]; } } } // Init rotation matrix using euler angles void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll) { float c1 = cos(roll); float s1 = sin(roll); float c2 = cos(pitch); float s2 = sin(pitch); float c3 = cos(yaw); float s3 = sin(yaw); // Euler angles, right-handed, intrinsic, XYZ convention // (which means: rotate around body axes Z, Y', X'') m[0][0] = c2 * c3; m[0][1] = c3 * s1 * s2 - c1 * s3; m[0][2] = s1 * s3 + c1 * c3 * s2; m[1][0] = c2 * s3; m[1][1] = c1 * c3 + s1 * s2 * s3; m[1][2] = c1 * s2 * s3 - c3 * s1; m[2][0] = -s2; m[2][1] = c2 * s1; m[2][2] = c1 * c2; } float constrain(float in, float min, float max) { in = in > max ? max : in; in = in < min ? min : in; return in; }