private void UpdateAngles(int[] byteValues)
{
m_angle.x = LMUtility.ConvertBitwiseToInt16((byteValues[1] << 8) | byteValues[0]);
m_angle.y = LMUtility.ConvertBitwiseToInt16((byteValues[3] << 8) | byteValues[2]);
m_angle.z = LMUtility.ConvertBitwiseToInt16((byteValues[5] << 8) | byteValues[4]);
m_angle *= 0.01f;
if (values == null)
{
return;
}
try
{
if (values.Length > 0)
{
values[0].SetValue(m_angle.x);
}
if (values.Length > 1)
{
values[1].SetValue(m_angle.y);
}
if (values.Length > 2)
{
values[2].SetValue(m_angle.z);
}
}
catch (Exception e)
{
Debug.LogWarning(e);
Debug.Log("Values Length: " + values.Length);
}
}
private void UpdateAcceleration(int[] byteValues)
{
var na = new Vector3();
na.x = LMUtility.ConvertBitwiseToInt16(byteValues[1] << 8 | byteValues[0]);
na.y = LMUtility.ConvertBitwiseToInt16(byteValues[3] << 8 | byteValues[2]);
na.z = LMUtility.ConvertBitwiseToInt16(byteValues[5] << 8 | byteValues[4]);
if (m_initAcceleration == Vector3.zero)
{
m_initAcceleration = m_rawAccel = m_lastRawAccel = na;
Debug.Log(m_initAcceleration);
m_timeStart = DateTime.Now;
return;
}
var span = DateTime.Now.Subtract(m_timeStart);
m_timeStart = DateTime.Now;
m_lastRawAccel = m_rawAccel;
m_rawAccel = na;
m_acceleration += (m_rawAccel - m_lastRawAccel) * (float)span.TotalSeconds;
}
private void SetupKeyValues(int[] _byteValues)
{
m_angle.x = LMUtility.ConvertBitwiseToInt16((_byteValues[1] << 8) | _byteValues[0]);
m_angle.y = LMUtility.ConvertBitwiseToInt16((_byteValues[3] << 8) | _byteValues[2]);
m_angle.z = LMUtility.ConvertBitwiseToInt16((_byteValues[5] << 8) | _byteValues[4]);
m_angle *= 0.01f;
if (values == null)
{
return;
}
if (values.Length >= 0)
{
values[0].SetValue(m_angle.x);
}
if (values.Length >= 1)
{
values[1].SetValue(m_angle.y);
}
if (values.Length >= 2)
{
values[2].SetValue(m_angle.z);
}
}
private void SetupAngularVel(string[] _split)
{
int[] array = HexToArray(_split);
if (array == null)
{
return;
}
// m_lastGyro = m_gyro;
m_gyro.x = LMUtility.ConvertBitwiseToInt16(((array[3] << 8) | array[2])) / 32768f * 2000;
m_gyro.y = LMUtility.ConvertBitwiseToInt16(((array[5] << 8) | array[4])) / 32768f * 2000;
m_gyro.z = LMUtility.ConvertBitwiseToInt16(((array[7] << 8) | array[6])) / 32768f * 2000;
}
private void SetupAcceleration(string[] _split)
{
int[] array = HexToArray(_split);
if (array == null)
{
return;
}
float g = 9.8f;
// m_lastAcc = m_acc;
m_acc.x = LMUtility.ConvertBitwiseToInt16((array[3] << 8) | array[2]) / 32768f * 16 * g;
m_acc.y = LMUtility.ConvertBitwiseToInt16((array[5] << 8) | array[4]) / 32768f * 16 * g;
m_acc.z = LMUtility.ConvertBitwiseToInt16((array[7] << 8) | array[6]) / 32768f * 16 * g;
}
private bool SetupAngle(string[] _split)
{
if (_split.Length < 11)
{
return(false);
}
int[] array = HexToArray(_split);
if (array == null)
{
return(false);
}
// m_lastAngle = m_angle;
m_angle.x = LMUtility.ConvertBitwiseToInt16((array[3] << 8) | array[2]) / 32768f * 180f;
m_angle.y = LMUtility.ConvertBitwiseToInt16((array[5] << 8) | array[4]) / 32768f * 180f;
m_angle.z = LMUtility.ConvertBitwiseToInt16((array[7] << 8) | array[6]) / 32768f * 180f;
return(true);
}
