AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5
Matrix.h@0:62284d27d75e, 2012-01-24 (annotated)
- Committer:
- shimniok
- Date:
- Tue Jan 24 17:40:40 2012 +0000
- Revision:
- 0:62284d27d75e
Initial version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:62284d27d75e | 1 | #ifndef __MATRIX_H |
shimniok | 0:62284d27d75e | 2 | #define __MATRIX_H |
shimniok | 0:62284d27d75e | 3 | |
shimniok | 0:62284d27d75e | 4 | /** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut |
shimniok | 0:62284d27d75e | 5 | * @param v1 is the first vector |
shimniok | 0:62284d27d75e | 6 | * @param v2 is the second vector |
shimniok | 0:62284d27d75e | 7 | * @returns vectorOut = v1 x v2 |
shimniok | 0:62284d27d75e | 8 | */ |
shimniok | 0:62284d27d75e | 9 | void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); |
shimniok | 0:62284d27d75e | 10 | |
shimniok | 0:62284d27d75e | 11 | /** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2 |
shimniok | 0:62284d27d75e | 12 | * @param vectorIn the vector |
shimniok | 0:62284d27d75e | 13 | * @param scale2 is the scalar |
shimniok | 0:62284d27d75e | 14 | * @returns vectorOut the result |
shimniok | 0:62284d27d75e | 15 | */ |
shimniok | 0:62284d27d75e | 16 | void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); |
shimniok | 0:62284d27d75e | 17 | |
shimniok | 0:62284d27d75e | 18 | /** TDot product of two 3x1 vectors vector1 . vector2 |
shimniok | 0:62284d27d75e | 19 | * @param vector1 is the first vector |
shimniok | 0:62284d27d75e | 20 | * @param vector2 is the second vector |
shimniok | 0:62284d27d75e | 21 | * @returns float result of dot product |
shimniok | 0:62284d27d75e | 22 | */ |
shimniok | 0:62284d27d75e | 23 | float Vector_Dot_Product(float vector1[3], float vector2[3]); |
shimniok | 0:62284d27d75e | 24 | |
shimniok | 0:62284d27d75e | 25 | /** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2 |
shimniok | 0:62284d27d75e | 26 | * @param vectorIn1 is the first vector |
shimniok | 0:62284d27d75e | 27 | * @param vectorIn2 is the second vector |
shimniok | 0:62284d27d75e | 28 | * @returns vectorOut is the result of the addition |
shimniok | 0:62284d27d75e | 29 | */ |
shimniok | 0:62284d27d75e | 30 | void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); |
shimniok | 0:62284d27d75e | 31 | |
shimniok | 0:62284d27d75e | 32 | /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab |
shimniok | 0:62284d27d75e | 33 | * @param a is the first vector |
shimniok | 0:62284d27d75e | 34 | * @param b is the second vector |
shimniok | 0:62284d27d75e | 35 | * @returns c as the result of the mutliplication |
shimniok | 0:62284d27d75e | 36 | */ |
shimniok | 0:62284d27d75e | 37 | void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]); |
shimniok | 0:62284d27d75e | 38 | |
shimniok | 0:62284d27d75e | 39 | #endif |