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);
}
}
public override void ResolveBytes(byte[] _bytes)
{
if (_bytes == null || _bytes.Length == 0)
{
return;
}
_bytes = LMUtility.RemoveSpacing(_bytes);
FilterIds(_bytes);
}
public override void ResolveBytes(byte[] _bytes)
{
base.ResolveBytes(_bytes);
if (m_bytes.Length == 0)
{
return;
}
m_bytes = LMUtility.RemoveSpacing(m_bytes);
FilterIds();
}
protected override void ResolveBytes(byte[] bytes)
{
// 消除字节中的空值
bytes = LMUtility.RemoveSpacing(bytes);
// 把字节转化为字符串并叠加起来
m_getString += Encoding.UTF8.GetString(bytes);
// 砂磨板的值会以分号为区分，如CB:0001;CC:00JK;等
// 因此当分号出现时候，则判断可能获取到数值了
if (m_getString.IndexOf(';') >= 0)
{
// 按分号分割成各别的数组，如[CB:0001,CC:00JK]
string[] split = m_getString.Split(';');
// 倒序迭代每个字符串
for (int i = split.Length - 1; i >= 0; i--)
{
if (split[i].IndexOf(':') < 0)
{
continue;
}
string[] splitKey = split[i].Split(':');
string key = splitKey[0] ?? string.Empty;
// 如果键值为空或者是个空格，则直接跳过
if (string.IsNullOrWhiteSpace(key))
{
continue;
}
// 如果获取到位置代号但是不完整，则跳过
if (key == "CC" && split[i].Length != 7)
{
continue;
}
// 如果有值，则把开始的0全部去掉
string value = splitKey[1].TrimStart('0') ?? string.Empty;
// 存到序列里
SetValue(key, value);
// 只要获取任何有效键值，就清空叠加的字符串
m_getString = string.Empty;
break;
}
}
}
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);
}
public override void ResolveBytes(byte[] _bytes)
{
if (_bytes == null || _bytes.Length == 0)
{
return;
}
// 把获取的bytes转化为字符串
m_getString += LMUtility.ByteToStr(_bytes);
// 获取角度
var angleValues = GetByteValues(m_getString, CODE_DETECTION);
// 获取加速度
var accelValues = GetByteValues(m_getString, ACCELERATE_DETECTION);
// 如果没有获取角度和加速度，则不清空存储的字符串
if (angleValues == null && accelValues == null)
{
return;
}
// 更新角度值
if (angleValues != null)
{
UpdateAngles(angleValues);
}
// 更新加速度值
if (accelValues != null)
{
UpdateAcceleration(accelValues);
}
// 清空字符串
m_getString = string.Empty;
}
private int[] GetByteValues(string value, string detection)
{
int keyIndex = value.IndexOf(detection);
if (keyIndex == -1)
{
return(null);
}
var sub = m_getString.Substring(keyIndex + detection.Length);
var split = sub.Split(' ');
if (split.Length == 0 || string.IsNullOrEmpty(split[0]))
{
return(null);
}
m_length = Convert.ToInt16(split[0], 16);
if (split.Length < m_length + 1)
{
return(null);
}
var m_checksum = Convert.ToInt16(split[1], 16);
var byteValues = ConvertHexStringToInt16(split);
if (byteValues == null || !LMUtility.CheckSum(m_checksum, byteValues))
{
return(null);
}
return(byteValues);
}
