Prosper Van / DCM_AHRS_GY80

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

Dependents:   quadCommand2

Fork of DCM_AHRS by Michael Shimniok

Matrix.cpp@14:6cb3c2204b9b, 2013-10-16 (annotated)

Committer:
oprospero
Date:
Wed Oct 16 07:49:34 2013 +0000
Revision:
14:6cb3c2204b9b
Parent:
11:8d46121bd255 
Fixed vector copy;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oprospero 6:49cbf2acc4e6 1 #include "Matrix.h"
shimniok 0:62284d27d75e 2 void Vector_Cross_Product(float C[3], float A[3], float B[3])
shimniok 0:62284d27d75e 3 {
shimniok 0:62284d27d75e 4 C[0] = (A[1] * B[2]) - (A[2] * B[1]);
shimniok 0:62284d27d75e 5 C[1] = (A[2] * B[0]) - (A[0] * B[2]);
shimniok 0:62284d27d75e 6 C[2] = (A[0] * B[1]) - (A[1] * B[0]);
shimniok 0:62284d27d75e 7
shimniok 0:62284d27d75e 8 return;
shimniok 0:62284d27d75e 9 }
shimniok 0:62284d27d75e 10
shimniok 0:62284d27d75e 11 void Vector_Scale(float C[3], float A[3], float b)
shimniok 0:62284d27d75e 12 {
shimniok 0:62284d27d75e 13 for (int m = 0; m < 3; m++)
shimniok 0:62284d27d75e 14 C[m] = A[m] * b;
shimniok 0:62284d27d75e 15
shimniok 0:62284d27d75e 16 return;
shimniok 0:62284d27d75e 17 }
shimniok 0:62284d27d75e 18
shimniok 0:62284d27d75e 19 float Vector_Dot_Product(float A[3], float B[3])
shimniok 0:62284d27d75e 20 {
shimniok 0:62284d27d75e 21 float result = 0.0;
shimniok 0:62284d27d75e 22
shimniok 0:62284d27d75e 23 for (int i = 0; i < 3; i++) {
shimniok 0:62284d27d75e 24 result += A[i] * B[i];
shimniok 0:62284d27d75e 25 }
shimniok 0:62284d27d75e 26
shimniok 0:62284d27d75e 27 return result;
shimniok 0:62284d27d75e 28 }
shimniok 0:62284d27d75e 29
shimniok 0:62284d27d75e 30 void Vector_Add(float C[3], float A[3], float B[3])
shimniok 0:62284d27d75e 31 {
shimniok 0:62284d27d75e 32 for (int m = 0; m < 3; m++)
shimniok 0:62284d27d75e 33 C[m] = A[m] + B[m];
shimniok 0:62284d27d75e 34
shimniok 0:62284d27d75e 35 return;
shimniok 0:62284d27d75e 36 }
shimniok 0:62284d27d75e 37
shimniok 0:62284d27d75e 38 void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
shimniok 0:62284d27d75e 39 {
shimniok 0:62284d27d75e 40 for (int m = 0; m < 3; m++)
shimniok 0:62284d27d75e 41 for (int n = 0; n < 3; n++)
shimniok 0:62284d27d75e 42 C[m][n] = A[m][n] + B[m][n];
shimniok 0:62284d27d75e 43 }
shimniok 0:62284d27d75e 44
shimniok 0:62284d27d75e 45 void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
shimniok 0:62284d27d75e 46 {
shimniok 0:62284d27d75e 47 for (int i = 0; i < 3; i++) {
shimniok 0:62284d27d75e 48 for (int j = 0; j < 3; j++) {
shimniok 0:62284d27d75e 49 C[i][j] = 0;
shimniok 0:62284d27d75e 50 for (int k = 0; k < 3; k++) {
shimniok 0:62284d27d75e 51 C[i][j] += A[i][k] * B[k][j];
shimniok 0:62284d27d75e 52 }
shimniok 0:62284d27d75e 53 }
shimniok 0:62284d27d75e 54 }
oprospero 6:49cbf2acc4e6 55 }
oprospero 6:49cbf2acc4e6 56
oprospero 6:49cbf2acc4e6 57 void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
oprospero 6:49cbf2acc4e6 58 {
oprospero 11:8d46121bd255 59 float c1 = cos(roll);
oprospero 11:8d46121bd255 60 float s1 = sin(roll);
oprospero 11:8d46121bd255 61 float c2 = cos(pitch);
oprospero 11:8d46121bd255 62 float s2 = sin(pitch);
oprospero 11:8d46121bd255 63 float c3 = cos(yaw);
oprospero 11:8d46121bd255 64 float s3 = sin(yaw);
oprospero 11:8d46121bd255 65
oprospero 11:8d46121bd255 66 // Euler angles, right-handed, intrinsic, XYZ convention
oprospero 11:8d46121bd255 67 // (which means: rotate around body axes Z, Y', X'')
oprospero 11:8d46121bd255 68 m[0][0] = c2 * c3;
oprospero 11:8d46121bd255 69 m[0][1] = c3 * s1 * s2 - c1 * s3;
oprospero 11:8d46121bd255 70 m[0][2] = s1 * s3 + c1 * c3 * s2;
oprospero 11:8d46121bd255 71
oprospero 11:8d46121bd255 72 m[1][0] = c2 * s3;
oprospero 11:8d46121bd255 73 m[1][1] = c1 * c3 + s1 * s2 * s3;
oprospero 11:8d46121bd255 74 m[1][2] = c1 * s2 * s3 - c3 * s1;
oprospero 11:8d46121bd255 75
oprospero 11:8d46121bd255 76 m[2][0] = -s2;
oprospero 11:8d46121bd255 77 m[2][1] = c2 * s1;
oprospero 11:8d46121bd255 78 m[2][2] = c1 * c2;
shimniok 0:62284d27d75e 79 }