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

Matrix.cpp

Committer:
shimniok
Date:
2012-01-24
Revision:
0:62284d27d75e

File content as of revision 0:62284d27d75e:

void Vector_Cross_Product(float C[3], float A[3], float B[3])
{
    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
  
    return;
}

void Vector_Scale(float C[3], float A[3], float b)
{
    for (int m = 0; m < 3; m++)
        C[m] = A[m] * b;
        
    return;
}

float Vector_Dot_Product(float A[3], float B[3])
{
    float result = 0.0;

    for (int i = 0; i < 3; i++) {
        result += A[i] * B[i];
    }
    
    return result;
}

void Vector_Add(float C[3], float A[3], float B[3])
{
    for (int m = 0; m < 3; m++)
        C[m] = A[m] + B[m];
        
    return;
}

void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
{
    for (int m = 0; m < 3; m++)
        for (int n = 0; n < 3; n++)
            C[m][n] = A[m][n] + B[m][n];
}

void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
{
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            C[i][j] = 0;
            for (int k = 0; k < 3; k++) {
               C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}