AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5
Matrix.h
00001 #ifndef __MATRIX_H 00002 #define __MATRIX_H 00003 00004 /** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut 00005 * @param v1 is the first vector 00006 * @param v2 is the second vector 00007 * @returns vectorOut = v1 x v2 00008 */ 00009 void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); 00010 00011 /** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2 00012 * @param vectorIn the vector 00013 * @param scale2 is the scalar 00014 * @returns vectorOut the result 00015 */ 00016 void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); 00017 00018 /** TDot product of two 3x1 vectors vector1 . vector2 00019 * @param vector1 is the first vector 00020 * @param vector2 is the second vector 00021 * @returns float result of dot product 00022 */ 00023 float Vector_Dot_Product(float vector1[3], float vector2[3]); 00024 00025 /** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2 00026 * @param vectorIn1 is the first vector 00027 * @param vectorIn2 is the second vector 00028 * @returns vectorOut is the result of the addition 00029 */ 00030 void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); 00031 00032 /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab 00033 * @param a is the first vector 00034 * @param b is the second vector 00035 * @returns c as the result of the mutliplication 00036 */ 00037 void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]); 00038 00039 #endif
Generated on Sat Jul 16 2022 20:25:06 by 1.7.2