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];
}
/**
* Plots current guess on the graph, if possible
* Not possible if function or data are null
* @returns :: success or failure
*/
bool ALCPeakFittingPresenter::plotGuessOnGraph() {
bool plotted = false;
auto func = m_view->function("");
auto dataWS = m_model->data();
if (func && dataWS) {
auto xdata = dataWS->readX(0);
m_view->setFittedCurve(*(ALCHelper::curveDataFromFunction(func, xdata)));
plotted = true;
}
return plotted;
}
void ALCPeakFittingPresenter::onFittedPeaksChanged() {
IFunction_const_sptr fittedPeaks = m_model->fittedPeaks();
auto dataWS = m_model->data();
if (fittedPeaks && dataWS) {
auto x = dataWS->readX(0);
m_view->setFittedCurve(*(ALCHelper::curveDataFromFunction(fittedPeaks, x)));
m_view->setFunction(fittedPeaks);
} else {
m_view->setFittedCurve(*(ALCHelper::emptyCurveData()));
m_view->setFunction(IFunction_const_sptr());
}
}
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::readGaussX() {
return readX() / _mult;
}
float LIS331DLH_TWI::readX_G()
{
return readX()*_mult*G;
}
float L3G4200D_TWI::readX_DegPerSec()
{
return readX()*_mult;
}
