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];
}
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];
}
float LIS3MDL_TWI::readGaussZ() {
return readZ() / _mult;
}
float LIS331DLH_TWI::readZ_G()
{
return readZ()*_mult*G;
}
float L3G4200D_TWI::readZ_DegPerSec()
{
return readZ()*_mult;
}