Michael Shimniok / DCM_AHRS

AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5

Matrix.cpp@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?

UserRevisionLine numberNew contents of line
shimniok 0:62284d27d75e 1 void Vector_Cross_Product(float C[3], float A[3], float B[3])
shimniok 0:62284d27d75e 2 {
shimniok 0:62284d27d75e 3 C[0] = (A[1] * B[2]) - (A[2] * B[1]);
shimniok 0:62284d27d75e 4 C[1] = (A[2] * B[0]) - (A[0] * B[2]);
shimniok 0:62284d27d75e 5 C[2] = (A[0] * B[1]) - (A[1] * B[0]);
shimniok 0:62284d27d75e 6
shimniok 0:62284d27d75e 7 return;
shimniok 0:62284d27d75e 8 }
shimniok 0:62284d27d75e 9
shimniok 0:62284d27d75e 10 void Vector_Scale(float C[3], float A[3], float b)
shimniok 0:62284d27d75e 11 {
shimniok 0:62284d27d75e 12 for (int m = 0; m < 3; m++)
shimniok 0:62284d27d75e 13 C[m] = A[m] * b;
shimniok 0:62284d27d75e 14
shimniok 0:62284d27d75e 15 return;
shimniok 0:62284d27d75e 16 }
shimniok 0:62284d27d75e 17
shimniok 0:62284d27d75e 18 float Vector_Dot_Product(float A[3], float B[3])
shimniok 0:62284d27d75e 19 {
shimniok 0:62284d27d75e 20 float result = 0.0;
shimniok 0:62284d27d75e 21
shimniok 0:62284d27d75e 22 for (int i = 0; i < 3; i++) {
shimniok 0:62284d27d75e 23 result += A[i] * B[i];
shimniok 0:62284d27d75e 24 }
shimniok 0:62284d27d75e 25
shimniok 0:62284d27d75e 26 return result;
shimniok 0:62284d27d75e 27 }
shimniok 0:62284d27d75e 28
shimniok 0:62284d27d75e 29 void Vector_Add(float C[3], float A[3], float B[3])
shimniok 0:62284d27d75e 30 {
shimniok 0:62284d27d75e 31 for (int m = 0; m < 3; m++)
shimniok 0:62284d27d75e 32 C[m] = A[m] + B[m];
shimniok 0:62284d27d75e 33
shimniok 0:62284d27d75e 34 return;
shimniok 0:62284d27d75e 35 }
shimniok 0:62284d27d75e 36
shimniok 0:62284d27d75e 37 void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
shimniok 0:62284d27d75e 38 {
shimniok 0:62284d27d75e 39 for (int m = 0; m < 3; m++)
shimniok 0:62284d27d75e 40 for (int n = 0; n < 3; n++)
shimniok 0:62284d27d75e 41 C[m][n] = A[m][n] + B[m][n];
shimniok 0:62284d27d75e 42 }
shimniok 0:62284d27d75e 43
shimniok 0:62284d27d75e 44 void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
shimniok 0:62284d27d75e 45 {
shimniok 0:62284d27d75e 46 for (int i = 0; i < 3; i++) {
shimniok 0:62284d27d75e 47 for (int j = 0; j < 3; j++) {
shimniok 0:62284d27d75e 48 C[i][j] = 0;
shimniok 0:62284d27d75e 49 for (int k = 0; k < 3; k++) {
shimniok 0:62284d27d75e 50 C[i][j] += A[i][k] * B[k][j];
shimniok 0:62284d27d75e 51 }
shimniok 0:62284d27d75e 52 }
shimniok 0:62284d27d75e 53 }
shimniok 0:62284d27d75e 54 }