//void calculate(string serverSays)
void calculate(byte[] bytesFromServer)
{
// just return sensor fusion is not yet ready
if (sF == null)
{
return;
}
samplecount += 1;
var bytes = ConvertFuncs.ToByteArray(bytesFromServer);
// TODO: Add other stream data options/data lengths
if (bytes.Length == 9)
{
// get Gyroscope data
gtx = bytes[0];
gty = bytes[1];
gtz = bytes[2];
gx = gtx * gyroRawMultiplier;
gy = gty * gyroRawMultiplier;
gz = gtz * gyroRawMultiplier;
grx = gx; gry = gy; grz = gz;
gx -= gxo; gy -= gyo; gz -= gzo;
// get Accelerometer data
ax = bytes[3];
ay = bytes[4];
az = bytes[5];
// store previous values
if (prevAx != 0 || prevAy != 0 || prevAz != 0)
{
//print(String.Format("acc difference: {0}, {1}, {2}", Math.Abs(ax - prevAx), Math.Abs(ay - prevAy), Math.Abs(az - prevAz)));
if (Math.Abs(ax - prevAx) >= 100 || Math.Abs(ay - prevAy) >= 100 || Math.Abs(az - prevAz) >= 100)
{
movementDetected = true;
}
else
{
movementDetected = false;
}
}
prevAx = ax;
prevAy = ay;
prevAz = az;
// get Magnetometer data
mx = bytes[6];
my = bytes[7];
mz = bytes[8];
}
// run sensor fusion with raw sensor data
runFusion(bytesFromServer);
}