Exemplo n.º 1
0
void LIS3MDL_TWI::readXYZ_Calib()
{
    double calibration_matrix[3][3] = 
    {
        {2.446, 0.074, 0.006},
        {0.07, 2.317, -0.006},
        {-0.027, -0.12, 2.458}  
    };

    double bias[3] = 
    {
        -1221.593,
        -2042.451,
        6497.221
    };

    float result[3] = {0, 0, 0};
    float uncalibrated_values[3];
    uncalibrated_values[0] = readX() - bias[0];
    uncalibrated_values[1] = readY() - bias[1];
    uncalibrated_values[2] = readZ() - bias[2];

    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
        result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
        }
    }

    x_cal = result[0];
    y_cal = result[1];
    z_cal = result[2];
}
Exemplo n.º 2
0
void LIS3MDL_TWI::calibrate() {
    float result[3] = {0, 0, 0};
    float uncalibratedValues[3];
    uncalibratedValues[0] = readX() - _bias[0];
    uncalibratedValues[1] = readY() - _bias[1];
    uncalibratedValues[2] = readZ() - _bias[2];

    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
        result[i] += _calibrationMatrix[i][j] * uncalibratedValues[j];
        }
    }

    _xCalibrate = result[0];
    _yCalibrate = result[1];
    _zCalibrate = result[2];
}
Exemplo n.º 3
0
float LIS3MDL_TWI::readGaussY() {
    return readY() / _mult;
}
Exemplo n.º 4
0
float LIS331DLH_TWI::readY_G()
{
    return readY()*_mult*G;
}
Exemplo n.º 5
0
float L3G4200D_TWI::readY_DegPerSec()
{
    return readY()*_mult;
}