private AccelerationGyroleration ReadInternal()
{
var data = new byte[14]; //6 bytes equals 2 bytes * 3 axes
var readAddress = new byte[] { REGISTER_ACCELEROMETER_X }; //0x80 for autoincrement, read from register x all three axis
QueuedLock.Enter();
_accelerometer.WriteRead(readAddress, data);
QueuedLock.Exit();
var xa = (short)(data[0] << 8 | data[1]);
var ya = (short)(data[2] << 8 | data[3]);
var za = (short)(data[4] << 8 | data[5]);
var temperature = (short)(data[6] << 8 | data[7]);
var xg = (short)(data[8] << 8 | data[9]);
var yg = (short)(data[10] << 8 | data[11]);
var zg = (short)(data[12] << 8 | data[13]);
var acceleration = new AccelerationGyroleration
{
AccelerationX = xa / (float)16384,
AccelerationY = ya / (float)16384,
AccelerationZ = za / (float)16384,
TemperatureInC = temperature / 340.00 + 36.53,
GyroX = xg / (float)131,
GyroY = yg / (float)131,
GyroZ = zg / (float)131
};
return(acceleration);
}
private void StartInternal()
{
var warmUp = 0;
_isStopped = false;
while (!_isStopping)
{
var acceleration = ReadInternal();
//Remove gravity
var alpha = 0.8d;
_gravityX = alpha * _gravityX + (1 - alpha) * acceleration.AccelerationX;
_gravityY = alpha * _gravityY + (1 - alpha) * acceleration.AccelerationY;
_gravityZ = alpha * _gravityZ + (1 - alpha) * acceleration.AccelerationZ;
var accelerationX = acceleration.AccelerationX - _gravityX;
var accelerationY = acceleration.AccelerationY - _gravityY;
var accelerationZ = acceleration.AccelerationZ - _gravityZ;
if (warmUp <= 60)
{
warmUp++;
}
else
{
_currentAcceleration = acceleration;
_currentLinearAcceleration = new AccelerationGyroleration
{
AccelerationX = accelerationX,
AccelerationY = accelerationY,
AccelerationZ = accelerationZ,
GyroX = acceleration.GyroX,
GyroY = acceleration.GyroY,
GyroZ = acceleration.GyroZ
};
}
Task.Delay(10).Wait();
}
_isStopped = true;
}