public void ApplyGyroAccel(short[] Accel, short[] Gyro, ushort timestamp)
{
if (previous_timestamp != -1 && timestamp > previous_timestamp)
{
float diff = (float)(timestamp - previous_timestamp) * 0.00125f / 188f; // s since last report
if (diff == 0) diff = 0.00125f;
_Gyro_Raw = new Vector3(-Gyro[0], -Gyro[1], Gyro[2]) * diff / 20f;
previous_timestamp = timestamp;
}
else
{
previous_timestamp = timestamp;
return;
}
_Accel_Raw = new Vector3(-Accel[0], Accel[1], Accel[2]) * 9.8f / 8100f;
if (accel_array.Count == max_accel_array_size)
accel_array.RemoveAt(0);
accel_array.Add(new Vector3(Accel[0], Accel[1], Accel[2]));
float sd = StandardDeviation().magnitude;
_Accel_Deviation = sd;
if (sd <= max_still_sd)
_Orientation = Quaternion.FromToRotation(Vector3.up, Accel_Raw);
else
_Orientation = Quaternion.Euler(Gyro_Raw) * _Orientation;
}