/** * @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; }