AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5 built for sensor gy_80
Fork of DCM_AHRS by
Matrix.h
- Committer:
- oprospero
- Date:
- 2013-10-16
- Revision:
- 14:6cb3c2204b9b
- Parent:
- 6:49cbf2acc4e6
File content as of revision 14:6cb3c2204b9b:
#ifndef __MATRIX_H #define __MATRIX_H #include "math.h" /** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut * @param v1 is the first vector * @param v2 is the second vector * @returns vectorOut = v1 x v2 */ void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); /** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2 * @param vectorIn the vector * @param scale2 is the scalar * @returns vectorOut the result */ void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); /** TDot product of two 3x1 vectors vector1 . vector2 * @param vector1 is the first vector * @param vector2 is the second vector * @returns float result of dot product */ float Vector_Dot_Product(float vector1[3], float vector2[3]); /** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2 * @param vectorIn1 is the first vector * @param vectorIn2 is the second vector * @returns vectorOut is the result of the addition */ void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab * @param a is the first vector * @param b is the second vector * @returns c as the result of the mutliplication */ void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]); void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll); #endif