private void EthernetAnalysisEXPRESS_SCAN(byte[] responseData)
{
//string reciveStr = SerialCommunicationUtility.ByteToHexString(responseData);
//Debug.Log("____________收到消息_____________ Message => " + reciveStr.ToString());//打印一条数据信息
//A5 5A 54 00 00 40 82
if (responseData[0] == 0xA5 && responseData[1] == 0x5A && responseData[2] == 0x54 && responseData[3] == 0x00 && responseData[4] == 0x00 && responseData[5] == 0x40 && responseData[6] == 0x82)
{
IsStart = true;
CacheBuffer.AddRange(responseData);
return;
}
if (IsStart)
{
CacheBuffer.AddRange(responseData);
//综合
float start_angle_q6 = 0;
int breakIndex = 0;
SerialData item = new SerialData();
for (int i = 0; i < CacheBuffer.Count; i++)
{
if (i + 84 > CacheBuffer.Count)
{
breakIndex = i; break;
}
byte sync1 = Convert.ToByte(CacheBuffer[i + 0] >> 3);
// 0x07 = 00000111 获取右边3位
byte ChkSum1 = Convert.ToByte(CacheBuffer[i + 0] & 0x07);
byte sync2 = Convert.ToByte(CacheBuffer[i + 1] >> 3);
// 0x07 = 00000111 获取右边3位
byte ChkSum2 = Convert.ToByte(CacheBuffer[i + 1] & 0x07);
if (sync1 != 0xA || sync2 != 0x5)
{
continue;
}
byte start_angle_q6_7_0 = CacheBuffer[i + 2];
// 起始应答报文标志 当设置为1时，表示当前应答报文时本轮测距采样中的第一个。
int s = CacheBuffer[i + 3] >> 7;
if (s == 1)
{
// TODO: 清理缓存,重新计算
SerialDatasCache.Clear();
DataQueueCache.Clear();
}
// 0x7f = 01111111 获取右边7位
byte start_angle_q6_14_8 = Convert.ToByte(CacheBuffer[i + 3] & 0x7f);
// 夹角
int _res = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(start_angle_q6_14_8, start_angle_q6_7_0), 16);
start_angle_q6 = (_res >> 1) / 64f;
// Cabin
CabinStruct[] Cabin = new CabinStruct[CabinLength * 2];
int k = 1;
// Cabin 5位1组,共16组
for (int ck = i; ck < CabinLength * CabinElementLength; ck += CabinElementLength)
{
//dis1
byte distance1_5_0 = Convert.ToByte(CacheBuffer[ck + 0] >> 2);
//0x03 = 00000011 获取右边2位
byte d01_5_4 = Convert.ToByte(CacheBuffer[ck + 0] & 0x03);
byte distance1_16_0 = CacheBuffer[ck + 1];
// dis2
byte distance2_5_0 = Convert.ToByte(CacheBuffer[ck + 2] >> 2);
//0x03 = 00000011 获取右边2位
byte d02_5_4 = Convert.ToByte(CacheBuffer[ck + 2] & 0x03);
byte distance2_16_0 = CacheBuffer[ck + 3];
// 补偿角2
//取出后四位：10110111 & 00001111(即0x0f)，得到00000111，表达式： data & 0x0f；
byte d01_3_0 = Convert.ToByte((CacheBuffer[ck + 4] & 0x0f));
//取出前四位：10110111 & 11110000(即0xf0)，得到10110000，再右移四位(>>4)，表达式：(data & 0xf0) >> 4；
byte d02_3_0 = Convert.ToByte((CacheBuffer[ck + 4] & 0xf0) >> 4);
//
int distance1 = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(distance1_16_0, distance1_5_0), 16);
int d1 = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(d01_5_4, d01_3_0), 16);
int distance2 = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(distance2_16_0, distance2_5_0), 16);
int d2 = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(d02_5_4, d02_3_0), 16);
// add c1
Cabin[k - 1] = new CabinStruct()
{
K = k++, distance = distance1, d = d1
};
// add c2
Cabin[k - 1] = new CabinStruct()
{
K = k++, distance = distance2, d = d2
};
}
item.angle = start_angle_q6;
item.distance = 0;
item.quality = 0;
item.cabinStructs = (CabinStruct[])Cabin.Clone();
SerialDatasCache.Add(item);
}
for (int i = 0; i < SerialDatasCache.Count - 1; i++)
{
float angle1 = SerialDatasCache[i].angle;
float angle2 = SerialDatasCache[i + 1].angle;
CabinStruct[] cabinStructs = SerialDatasCache[i].cabinStructs;
for (int j = 0; j < cabinStructs.Length; j++)
{
item.angle = angle1 + (AngleDiff(angle1, angle2)) * cabinStructs[j].K - cabinStructs[j].d;
item.distance = cabinStructs[j].distance;
item.quality = 0;
item.cabinStructs = null;
if (item.distance > 10)
{
SerialDatasReal.Add(item);
}
}
}
DataQueueCache.Enqueue(SerialDatasReal);
for (int i = SerialDatasCache.Count - 1; i > 0; i--)
{
SerialDatasCache.Remove(SerialDatasCache[i]);
}
for (int i = breakIndex - 1; i > 0; i--)
{
CacheBuffer.Remove(CacheBuffer[i]);
}
SerialDatasReal.Clear();
}
}
private void EthernetAnalysis(byte[] responseData)
{
//return;
if (responseData[0] == 0xA5 && responseData[1] == 0x5A)
{
IsStart = true;
return;
}
if (IsStart)
{
int _res = 0;
float _angle = 0;
float _dis = 0;
byte origin_q = 0;
byte angle_q1 = 0;
byte angle_q2 = 0;
byte distance_q1 = 0;
byte distance_q2 = 0;
int s = 0;
int _s = 0;
int quality = 0;
int check_bit = 0;
int breakIndex = 0;
CacheBuffer.AddRange(responseData);
List <SerialData> SerialDatas = new List <SerialData>();
for (int i = 0; i < CacheBuffer.Count; i++)
{
if (i + 5 >= CacheBuffer.Count)
{
breakIndex = i; break;
}
origin_q = CacheBuffer[i];
// 判断该字节的0位和1位,是不是一个是0一个是1，如果判断为假则拉取下一个字节继续做同样的判断，直到判断为真
s = SerialCommunicationUtility.GetBit(origin_q, 0);
_s = SerialCommunicationUtility.GetBit(origin_q, 1);
if (s != 0 || _s != 1)
{
continue;
}
quality = origin_q >> 2;
angle_q1 = CacheBuffer[i + 1];
// 如果字节校验位不为1,那后面就不用了算了,重头再来
check_bit = SerialCommunicationUtility.GetBit(angle_q1, 0);
if (check_bit != 1)
{
continue;
}
angle_q2 = CacheBuffer[i + 2];
distance_q1 = CacheBuffer[i + 3];
distance_q2 = CacheBuffer[i + 4];
// 计算角度和距离
_res = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(angle_q2, angle_q1), 16);
_angle = (_res >> 1) / 64f;
_dis = Convert.ToInt32(SerialCommunicationUtility.BinaryConversion(distance_q2, distance_q1), 16) / 4f;
if (_dis > 10 && quality > 0)
{
SerialDatas.Add(new SerialData()
{
angle = _angle, distance = _dis
});
}
}
DataQueueCache.Enqueue(SerialDatas);
for (int i = breakIndex - 1; i > 0; i--)
{
CacheBuffer.Remove(CacheBuffer[i]);
}
}
}
