/**
* @brief Read Acc values, Calibrate them, filter them, and update the pitch value to be displayed
* @param None
* @retval None
*/
void ReadAcc(void){
/* Get values */
LIS3DSH_ReadACC(accValue);
Calibrate(accValue);
/* Filter values */
kalmanUpdate(xState, accValue[0]);
kalmanUpdate(yState, accValue[1]);
kalmanUpdate(zState, accValue[2]);
pitchAngle = calcPitch(xState->x, yState->x, zState->x);
rollAngle = calcRoll(xState->x, yState->x, zState->x);
}
bool GenericTiltSensor::filter(QAccelerometerReading *reading)
{
/*
z y
| /
|/___ x
*/
qreal ax = reading->x();
qreal ay = reading->y();
qreal az = reading->z();
#ifdef LOGCALIBRATION
qDebug() << "------------ new value -----------";
qDebug() << "old _pitch: " << pitch;
qDebug() << "old _roll: " << roll;
qDebug() << "_calibratedPitch: " << calibratedPitch;
qDebug() << "_calibratedRoll: " << calibratedRoll;
#endif
pitch = calcPitch(ax, ay, az);
roll = calcRoll (ax, ay, az);
#ifdef LOGCALIBRATION
qDebug() << "_pitch: " << pitch;
qDebug() << "_roll: " << roll;
#endif
qreal xrot = roll - calibratedRoll;
qreal yrot = pitch - calibratedPitch;
//get angle beteen 0 and 180 or 0 -180
qreal aG = 1 * sin(xrot);
qreal aK = 1 * cos(xrot);
xrot = qAtan2(aG, aK);
if (xrot > M_PI_2)
xrot = M_PI - xrot;
else if (xrot < -M_PI_2)
xrot = -(M_PI + xrot);
aG = 1 * sin(yrot);
aK = 1 * cos(yrot);
yrot = qAtan2(aG, aK);
if (yrot > M_PI_2)
yrot = M_PI - yrot;
else if (yrot < -M_PI_2)
yrot = -(M_PI + yrot);
#ifdef LOGCALIBRATION
qDebug() << "new xrot: " << xrot;
qDebug() << "new yrot: " << yrot;
qDebug() << "----------------------------------";
#endif
qreal dxrot = rad2deg(xrot) - xRotation;
qreal dyrot = rad2deg(yrot) - yRotation;
if (dxrot < 0) dxrot = -dxrot;
if (dyrot < 0) dyrot = -dyrot;
bool setNewReading = false;
if (dxrot >= rad2deg(radAccuracy) || !sensor()->skipDuplicates()) {
xRotation = rad2deg(xrot);
setNewReading = true;
}
if (dyrot >= rad2deg(radAccuracy) || !sensor()->skipDuplicates()) {
yRotation = rad2deg(yrot);
setNewReading = true;
}
if (setNewReading || m_reading.timestamp() == 0) {
m_reading.setTimestamp(reading->timestamp());
m_reading.setXRotation(xRotation);
m_reading.setYRotation(yRotation);
newReadingAvailable();
}
return false;
}
