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); }