/// <summary>
/// 生产
/// </summary>
/// <param name="name"></param>
void LenthFFTProduct(LenthFFTPacket lenthFFTData, Int16 type)
{
if (type == DATATYPE_LENTH_FFT1)
{
lock (lenthFFT1Locker)
{
new RcvDataProducer(LenthFFT1Container.lenthFFT1DataContainer).Produce(lenthFFTData);
}
}
else if (type == DATATYPE_LENTH_FFT2)
{
lock (lenthFFT2Locker)
{
new RcvDataProducer(LenthFFT2Container.lenthFFT2DataContainer).Produce(lenthFFTData);
}
}
}
public void LenthFFTPktToQueue(LenthFFTPacket rcvData, Int16 type)
{
Action <LenthFFTPacket, Int16> lenthFFTPktProduce = LenthFFTProduct;
lenthFFTPktProduce.Invoke(rcvData, type);
}
public void Produce(LenthFFTPacket lenthFFTData)
{
LenthFFTProducerContainer.Enqueue(lenthFFTData);
}
public void EnQueueLenthFFT2Pkt(LenthFFTPacket rcvData)
{
LenthFFTPktToQueue(rcvData, DATATYPE_LENTH_FFT2);
}
public void DrawSplineBaseRcvData()
{
ChartXy LenthFFTdrawChartXy = LenthFFTChartControlDisplay.LenthFFTChartControl.ChartPanel.ChartContainers[0] as ChartXy;
ChartXy lenthPosdrawChartXy = LenthPosChartControlDisplay.LenthPosChartControl.ChartPanel.ChartContainers[0] as ChartXy;
ChartXy lenthSpeeddrawChartXy = LenthSpeedChartControlDisplay.LenthSpeedChartControl.ChartPanel.ChartContainers[0] as ChartXy;
TargetPosPacket outTargetPosPoint = null;
LenthFFTPacket outLenthFFT1Pkt = null;
LenthFFTPacket outLenthFFT2Pkt = null;
DistancePosSpeedPacket outLenthSpeedPkt = null;
DistancePosSpeedPacket outLenthPosPkt = null;
//if (TargetPosContainer.targetPosContainer.Count == LenthFFT1Container.lenthFFT1DataContainer.Count &&
// TargetPosContainer.targetPosContainer.Count == LenthFFT2Container.lenthFFT2DataContainer.Count &&
// TargetPosContainer.targetPosContainer.Count == DistanceSpeedContainer.distanceSpeedContainer.Count &&
// TargetPosContainer.targetPosContainer.Count == DistancePosContainer.distancePosContainer.Count &&
// TargetPosContainer.targetPosContainer.Count != 0)
if (TargetPosContainer.targetPosContainer.Count > 0 &&
LenthFFT1Container.lenthFFT1DataContainer.Count > 0 &&
LenthFFT2Container.lenthFFT2DataContainer.Count > 0 &&
DistanceSpeedContainer.distanceSpeedContainer.Count > 0 &&
DistancePosContainer.distancePosContainer.Count > 0)
{
outTargetPosPoint = AllInDataProcess.DeQueueTargetPosPkt();
outLenthFFT1Pkt = AllInDataProcess.DeQueueLenthFFT1Pkt();
outLenthFFT2Pkt = AllInDataProcess.DeQueueLenthFFT2Pkt();
outLenthSpeedPkt = AllInDataProcess.DeQueueLenthSpeedPkt();
outLenthPosPkt = AllInDataProcess.DeQueueLenthPosPkt();
}
if (outLenthFFT1Pkt != null)
{
SeriesPoint[] lenthFFT1DrawData = new SeriesPoint[iPointNum - 1];
SeriesPoint[] lenthFFT1AverageDrawData = new SeriesPoint[iPointNum - 1];
SeriesPoint[] lenthFFT1LabelSeriesPoint = new SeriesPoint[TargetPosPacket.iPointCnt];
for (int i = 0; i < iPointNum - 1; i++)
{
double xPointValue = (300000 / iPointNum) * i;
lenthFFT1DrawData[i] = new SeriesPoint(xPointValue, Convert.ToDouble(outLenthFFT1Pkt.InDataArray[i]));
lenthFFT1AverageDrawData[i] = new SeriesPoint(xPointValue, Convert.ToDouble(outLenthFFT1Pkt.InDataArray[iPointNum - 1]));
}
for (int i = 0; i < TargetPosPacket.iPointCnt; i++)
{
lenthFFT1LabelSeriesPoint[i] = new SeriesPoint((300000 / iPointNum) * outTargetPosPoint.InDataArray[i],
Convert.ToDouble(outLenthFFT1Pkt.InDataArray[outTargetPosPoint.InDataArray[i]]));
}
Console.WriteLine("lenthFFT1 draw, data[0] = " + outLenthFFT1Pkt.InDataArray[iPointNum - 1]);
ChartSeries lenthFF1ChartSeries = LenthFFTdrawChartXy.ChartSeries["lenthFFT1"];
ChartSeries lenthFFT1AverageChartSeries = LenthFFTdrawChartXy.ChartSeries["lenthFFT1Average"];
this.Invoke((EventHandler)(delegate
{
lenthFF1ChartSeries.SeriesPoints.Clear();
lenthFFT1AverageChartSeries.SeriesPoints.Clear();
lenthFF1ChartSeries.SeriesPoints.AddRange(lenthFFT1DrawData);
lenthFFT1AverageChartSeries.SeriesPoints.AddRange(lenthFFT1AverageDrawData);
AddSeriesPointLabel(lenthFF1ChartSeries, lenthFFT1LabelSeriesPoint, Color.Green, outLenthPosPkt, outLenthSpeedPkt);
}));
}
if (outLenthFFT2Pkt != null)
{
SeriesPoint[] lenthFFT2DrawData = new SeriesPoint[iPointNum - 1];
SeriesPoint[] lenthFFT2AverageDrawData = new SeriesPoint[iPointNum - 1];
SeriesPoint[] lenthFFT2LabelSeriesPoint = new SeriesPoint[TargetPosPacket.iPointCnt];
for (int i = 0; i < iPointNum - 1; i++)
{
double xPointValue = (300000 / iPointNum) * i;
lenthFFT2DrawData[i] = new SeriesPoint(xPointValue, Convert.ToDouble(outLenthFFT2Pkt.InDataArray[i]));
lenthFFT2AverageDrawData[i] = new SeriesPoint(xPointValue, Convert.ToDouble(outLenthFFT2Pkt.InDataArray[iPointNum - 1]));
}
Console.WriteLine("lenthFFT2 draw, data[0] = " + outLenthFFT2Pkt.InDataArray[iPointNum - 1]);
for (int i = 0; i < TargetPosPacket.iPointCnt; i++)
{
lenthFFT2LabelSeriesPoint[i] = new SeriesPoint((300000 / iPointNum) * outTargetPosPoint.InDataArray[i],
Convert.ToDouble((outLenthFFT2Pkt.InDataArray[outTargetPosPoint.InDataArray[i]])));
}
ChartSeries lenthFF2ChartSeries = LenthFFTdrawChartXy.ChartSeries["lenthFFT2"];
ChartSeries lenthFFT2AverageChartSeries = LenthFFTdrawChartXy.ChartSeries["lenthFFT2Average"];
this.Invoke((EventHandler)(delegate
{
lenthFF2ChartSeries.SeriesPoints.Clear();
lenthFFT2AverageChartSeries.SeriesPoints.Clear();
lenthFF2ChartSeries.SeriesPoints.AddRange(lenthFFT2DrawData);
lenthFFT2AverageChartSeries.SeriesPoints.AddRange(lenthFFT2AverageDrawData);
AddSeriesPointLabel(lenthFF2ChartSeries, lenthFFT2LabelSeriesPoint, Color.Orange, outLenthPosPkt, outLenthSpeedPkt);
}));
}
bool lenthSpeedPosDataNotSorted = false;
StringBuilder displayLenthSpeedstring = null;
if (outLenthSpeedPkt != null)
{
List <SeriesPoint> lenthSpeedDrawPointList = new List <SeriesPoint>();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt; i += 2)
{
lenthSpeedDrawPointList.Add(new SeriesPoint(Convert.ToDouble(outLenthSpeedPkt.InDataArray[i]) / 100, Convert.ToDouble(outLenthSpeedPkt.InDataArray[i + 1]) / 100));
}
displayLenthSpeedstring = new StringBuilder();
displayLenthSpeedstring.Append("speed data start:");
foreach (SeriesPoint point in lenthSpeedDrawPointList)
{
displayLenthSpeedstring.Append("(" + point.ValueX + "," + point.ValueY[0] + ")");
}
displayLenthSpeedstring.Append("speed data end\r\n");
IEnumerable <SeriesPoint> sortedSpeedSeriesPointList =
from singlePoint in lenthSpeedDrawPointList
orderby singlePoint.ValueX
select singlePoint;
for (int i = 0; i < lenthSpeedDrawPointList.Count; i++)
{
if (lenthSpeedDrawPointList.ElementAt(i).ValueX != sortedSpeedSeriesPointList.ElementAt(i).ValueX)
{
lenthSpeedPosDataNotSorted = true;
break;
}
}
ChartSeries lenthSpeedChartSeries = lenthSpeeddrawChartXy.ChartSeries["DistanceSpeed"];
lenthSpeedChartSeries.PointLabelDisplayMode |= PointLabelDisplayMode.AllSeriesPoints;
this.Invoke((EventHandler)(delegate
{
lenthSpeedChartSeries.SeriesPoints.Clear();
lenthSpeedChartSeries.SeriesPoints.AddRange(sortedSpeedSeriesPointList);
}));
}
if (outLenthPosPkt != null)
{
List <SeriesPoint> lenthPosDrawPointList = new List <SeriesPoint>();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt; i += 2)
{
lenthPosDrawPointList.Add(new SeriesPoint(Convert.ToDouble(outLenthPosPkt.InDataArray[i]) / 100, Convert.ToDouble(outLenthPosPkt.InDataArray[i + 1]) / 100));
}
if (displayLenthSpeedstring == null)
{
displayLenthSpeedstring = new StringBuilder();
}
displayLenthSpeedstring.Append("DistanceAzimuth data start:");
foreach (SeriesPoint point in lenthPosDrawPointList)
{
displayLenthSpeedstring.Append("(" + point.ValueX + "," + point.ValueY[0] + ")");
}
displayLenthSpeedstring.Append("DistanceAzimuth data end\r\n");
IEnumerable <SeriesPoint> sortedPosSeriesPointList =
from singlePoint in lenthPosDrawPointList
orderby singlePoint.ValueX
select singlePoint;
for (int i = 0; i < lenthPosDrawPointList.Count; i++)
{
if (lenthPosDrawPointList.ElementAt(i).ValueX != sortedPosSeriesPointList.ElementAt(i).ValueX)
{
lenthSpeedPosDataNotSorted = true;
break;
}
}
ChartSeries lenthPosChartSeries = lenthPosdrawChartXy.ChartSeries["DistanceAzimuth"];
lenthPosChartSeries.PointLabelDisplayMode |= PointLabelDisplayMode.AllSeriesPoints;
if (lenthPosChartSeries != null)
{
this.Invoke((EventHandler)(delegate
{
lenthPosChartSeries.SeriesPoints.Clear();
lenthPosChartSeries.SeriesPoints.AddRange(sortedPosSeriesPointList);
}));
}
}
if (no_debug_output == false)
{
if (displayLenthSpeedstring != null)
{
AppendColorText2RichBox(displayLenthSpeedstring.ToString());
}
if (lenthSpeedPosDataNotSorted == true)
{
MessageBox.Show("收到的方位或速度数据不是按照距离升序的", "提示");
}
}
}
private void TestBtn_Click(object sender, EventArgs e)
{
Random lenthFFT1DataRandom = new Random(DateTime.Now.Millisecond);
lenthFFT1DataRandom.NextBytes(lenthFFT1Data);
lenthFFT1Data[0] = lenthFFT1PacketHead[0];
lenthFFT1Data[1] = lenthFFT1PacketHead[1];
lenthFFT1Data[LeastPacketLenth - 2] = lenthFFTPacketTail[0];
lenthFFT1Data[LeastPacketLenth - 1] = lenthFFTPacketTail[1];
Random lenthFFT2DataRandom = new Random(DateTime.Now.Millisecond + 100);
lenthFFT2DataRandom.NextBytes(lenthFFT2Data);
lenthFFT2Data[0] = lenthFFT2PacketHead[0];
lenthFFT2Data[1] = lenthFFT2PacketHead[1];
lenthFFT2Data[LeastPacketLenth - 2] = lenthFFTPacketTail[0];
lenthFFT2Data[LeastPacketLenth - 1] = lenthFFTPacketTail[1];
Random targetPosDataRandom = new Random(DateTime.Now.Millisecond + 100);
targetPosDataRandom.NextBytes(targetPosData);
targetPosData[0] = targetPosPacketHead[0];
targetPosData[1] = targetPosPacketHead[1];
targetPosData[LeastTargetPosPktLenth - 2] = targetPosPacketTail[0];
targetPosData[LeastTargetPosPktLenth - 1] = targetPosPacketTail[1];
Random lenthSpeedDataRandom = new Random(DateTime.Now.Millisecond + 100);
lenthSpeedDataRandom.NextBytes(lenthSpeedData);
lenthSpeedData[0] = lenthSpeedPacketHead[0];
lenthSpeedData[1] = lenthSpeedPacketHead[1];
lenthSpeedData[LeastLenthSpeedPosPktLenth - 2] = lenthSpeedPacketTail[0];
lenthSpeedData[LeastLenthSpeedPosPktLenth - 1] = lenthSpeedPacketTail[1];
Random lenthPosDataRandom = new Random(DateTime.Now.Millisecond + 200);
lenthPosDataRandom.NextBytes(lenthPosData);
lenthPosData[0] = lenthPosPacketHead[0];
lenthPosData[1] = lenthPosPacketHead[1];
lenthPosData[LeastLenthSpeedPosPktLenth - 2] = lenthPosPacketTail[0];
lenthPosData[LeastLenthSpeedPosPktLenth - 1] = lenthPosPacketTail[1];
LenthFFTPacket lenthFFT1DrawData = new LenthFFTPacket();
LenthFFTPacket lenthFFT2DrawData = new LenthFFTPacket();
for (int i = 0; i < iPointNum; i++)
{
lenthFFT1DrawData.InDataArray[i] = (UInt16)((lenthFFT1Data[2 + i * 2]) | (UInt16)(lenthFFT1Data[2 + i * 2 + 1] << 8));
lenthFFT2DrawData.InDataArray[i] = (UInt16)((lenthFFT2Data[2 + i * 2]) | (UInt16)(lenthFFT2Data[2 + i * 2 + 1] << 8));
}
TargetPosPacket targetPosDrawData = new TargetPosPacket();
for (int i = 0; i < TargetPosPacket.iPointCnt; i++)
{
targetPosDrawData.InDataArray[i] = targetPosData[2 + i];
}
DistancePosSpeedPacket lenthSpeedDrawData = new DistancePosSpeedPacket();
DistancePosSpeedPacket lenthPosDrawData = new DistancePosSpeedPacket();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt; i++)
{
lenthSpeedDrawData.InDataArray[i] = (Int16)((lenthSpeedData[2 + i * 2]) | (UInt16)(lenthSpeedData[2 + i * 2 + 1] << 8));
lenthPosDrawData.InDataArray[i] = (Int16)((lenthPosData[2 + i * 2]) | (UInt16)(lenthPosData[2 + i * 2 + 1] << 8));
}
StringBuilder strDebugFFT1Out = new StringBuilder();
StringBuilder strDebugFFT2Out = new StringBuilder();
for (int i = 0; i < iPointNum + 2; i++)
{
strDebugFFT1Out.AppendFormat("{0:X2} {1:X2} ", lenthFFT1Data[i * 2], lenthFFT1Data[i * 2 + 1]);
strDebugFFT2Out.AppendFormat("{0:X2} {1:X2} ", lenthFFT2Data[i * 2], lenthFFT2Data[i * 2 + 1]);
}
strDebugFFT1Out.Append("\r\n\r\n");
strDebugFFT2Out.Append("\r\n\r\n");
AppendColorText2RichBox(strDebugFFT1Out.ToString());
strDebugFFT1Out.Clear();
AppendColorText2RichBox(strDebugFFT2Out.ToString());
strDebugFFT2Out.Clear();
StringBuilder strDebugTargetPosOut = new StringBuilder();
foreach (byte element in targetPosData)
{
strDebugTargetPosOut.AppendFormat("{0:X2} ", element);
}
strDebugTargetPosOut.Append("\r\n\r\n");
AppendColorText2RichBox(strDebugTargetPosOut.ToString());
StringBuilder strDebugSpeedOut = new StringBuilder();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt + 2; i++)
{
strDebugSpeedOut.AppendFormat("{0:X2} {1:X2} ", lenthSpeedData[i * 2], lenthSpeedData[i * 2 + 1]);
}
strDebugSpeedOut.AppendFormat("\r\n\r\n");
AppendColorText2RichBox(strDebugSpeedOut.ToString());
StringBuilder strDebugPosOut = new StringBuilder();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt + 2; i++)
{
strDebugPosOut.AppendFormat("{0:X2} {1:X2} ", lenthPosData[i * 2], lenthPosData[i * 2 + 1]);
}
strDebugPosOut.AppendFormat("\r\n\r\n");
AppendColorText2RichBox(strDebugPosOut.ToString());
AllInDataProcess.EnQueueLenthFFT1Pkt(lenthFFT1DrawData);
AllInDataProcess.EnQueueLenthFFT2Pkt(lenthFFT2DrawData);
AllInDataProcess.EnQueueTargetPosPkt(targetPosDrawData);
AllInDataProcess.EnQueueLenthSpeedPkt(lenthSpeedDrawData);
AllInDataProcess.EnQueueLenthPosPkt(lenthPosDrawData);
}
private bool CheckAnalysisRcvPacket(byte[] rcvPacket, int len)
{
//Console.WriteLine("arrRecvmsglist.Count = " + arrRecvmsglist.Count + ", len = " + len);
for (int i = 0; i < len; i++)
{
arrRecvmsglist.Add(rcvPacket[i]);
}
if (arrRecvmsglist.Count < LeastPacketLenth)
{
Console.WriteLine("len {0} < LeastPacketLenth {1}", len, LeastPacketLenth);
return(false);
}
int index, head_index, lenthFFT2HeadIndex, targetPosHeadIndex, lenthSpeedHeadIndex;
for (index = 0; index < arrRecvmsglist.Count; index++)
{
head_index = arrRecvmsglist.IndexOf(lenthFFT1PacketHead[0]);
if (head_index + LeastPacketLenth > arrRecvmsglist.Count)
{
Console.WriteLine("data lenth is too short, head_index = " + len + "Count = " + arrRecvmsglist.Count);
//AppendColorText2RichBox("effective data is too short, count: " + (arrRecvmsglist.Count - head_index) + "\r\n", false);
//return false;
}
else
{
arrRecvmsglist.CopyTo(head_index, lenthFFT1Data, 0, LeastPacketLenth);
if (lenthFFT1Data[0] == lenthFFT1PacketHead[0] && lenthFFT1Data[1] == lenthFFT1PacketHead[1] &&
lenthFFT1Data[LeastPacketLenth - 2] == lenthFFTPacketTail[0] &&
lenthFFT1Data[LeastPacketLenth - 1] == lenthFFTPacketTail[1])
{
LenthFFTPacket lenthFFT1DrawData = new LenthFFTPacket();
for (int i = 0; i < iPointNum; i++)
{
lenthFFT1DrawData.InDataArray[i] = (UInt16)((lenthFFT1Data[2 + i * 2]) | (UInt16)(lenthFFT1Data[2 + i * 2 + 1] << 8));
}
AllInDataProcess.EnQueueLenthFFT1Pkt(lenthFFT1DrawData);
if (no_debug_output == false)
{
StringBuilder strDebugFFT1Out = new StringBuilder();
for (int i = 0; i < iPointNum + 2; i++)
{
strDebugFFT1Out.AppendFormat("{0:X2} {1:X2} ", lenthFFT1Data[i * 2], lenthFFT1Data[i * 2 + 1]);
}
strDebugFFT1Out.Append("\r\n\r\n");
AppendColorText2RichBox(strDebugFFT1Out.ToString());
strDebugFFT1Out.Clear();
}
arrRecvmsglist.RemoveRange(0, head_index + LeastPacketLenth);
}
else
{
if (head_index == 0)
{
arrRecvmsglist.RemoveAt(0);
}
else
{
arrRecvmsglist.RemoveRange(0, head_index);
}
}
}
lenthFFT2HeadIndex = arrRecvmsglist.IndexOf(lenthFFT2PacketHead[0]);
if (lenthFFT2HeadIndex + LeastPacketLenth > arrRecvmsglist.Count)
{
Console.WriteLine("data lenth is too short, lenthFFT2HeadIndex = " + len + "Count = " + arrRecvmsglist.Count);
//AppendColorText2RichBox("effective data is too short, count: " + (arrRecvmsglist.Count - lenthFFT2HeadIndex) + "\r\n", false);
//return false;
}
else
{
arrRecvmsglist.CopyTo(lenthFFT2HeadIndex, lenthFFT2Data, 0, LeastPacketLenth);
if (lenthFFT2Data[0] == lenthFFT2PacketHead[0] && lenthFFT2Data[1] == lenthFFT2PacketHead[1] &&
lenthFFT2Data[LeastPacketLenth - 2] == lenthFFTPacketTail[0] &&
lenthFFT2Data[LeastPacketLenth - 1] == lenthFFTPacketTail[1])
{
LenthFFTPacket lenthFFT2DrawData = new LenthFFTPacket();
for (int i = 0; i < iPointNum; i++)
{
lenthFFT2DrawData.InDataArray[i] = (UInt16)((lenthFFT2Data[2 + i * 2]) | (UInt16)(lenthFFT2Data[2 + i * 2 + 1] << 8));
}
AllInDataProcess.EnQueueLenthFFT2Pkt(lenthFFT2DrawData);
if (no_debug_output == false)
{
StringBuilder strDebugFFT2Out = new StringBuilder();
for (int i = 0; i < iPointNum + 2; i++)
{
strDebugFFT2Out.AppendFormat("{0:X2} {1:X2} ", lenthFFT2Data[i * 2], lenthFFT2Data[i * 2 + 1]);
}
strDebugFFT2Out.Append("\r\n\r\n");
AppendColorText2RichBox(strDebugFFT2Out.ToString());
strDebugFFT2Out.Clear();
}
arrRecvmsglist.RemoveRange(0, lenthFFT2HeadIndex + LeastPacketLenth);
}
else
{
if (lenthFFT2HeadIndex == 0)
{
arrRecvmsglist.RemoveAt(0);
}
else
{
arrRecvmsglist.RemoveRange(0, lenthFFT2HeadIndex);
}
}
}
targetPosHeadIndex = arrRecvmsglist.IndexOf(targetPosPacketHead[0]);
if (targetPosHeadIndex + LeastTargetPosPktLenth > arrRecvmsglist.Count)
{
Console.WriteLine("too short, targetPosHeadIndex = " + targetPosHeadIndex);
}
else
{
arrRecvmsglist.CopyTo(targetPosHeadIndex, targetPosData, 0, LeastTargetPosPktLenth);
if (targetPosData[0] == targetPosPacketHead[0] && targetPosData[1] == targetPosPacketHead[1] &&
targetPosData[LeastTargetPosPktLenth - 2] == targetPosPacketTail[0] &&
targetPosData[LeastTargetPosPktLenth - 1] == targetPosPacketTail[1])
{
TargetPosPacket targetPosDrawData = new TargetPosPacket();
for (int i = 0; i < TargetPosPacket.iPointCnt; i++)
{
targetPosDrawData.InDataArray[i] = targetPosData[2 + i];
}
AllInDataProcess.EnQueueTargetPosPkt(targetPosDrawData);
if (no_debug_output == false)
{
StringBuilder strDebugTargetPosOut = new StringBuilder();
foreach (byte element in targetPosData)
{
strDebugTargetPosOut.AppendFormat("{0:X2} ", element);
}
strDebugTargetPosOut.Append("\r\n\r\n");
AppendColorText2RichBox(strDebugTargetPosOut.ToString());
strDebugTargetPosOut.Clear();
}
arrRecvmsglist.RemoveRange(0, targetPosHeadIndex + LeastTargetPosPktLenth);
}
else
{
if (targetPosHeadIndex == 0)
{
arrRecvmsglist.RemoveAt(0);
}
else
{
arrRecvmsglist.RemoveRange(0, targetPosHeadIndex);
}
}
}
lenthSpeedHeadIndex = arrRecvmsglist.IndexOf(lenthSpeedPacketHead[0]);
if (lenthSpeedHeadIndex + LeastLenthSpeedPosPktLenth > arrRecvmsglist.Count)
{
Console.WriteLine("too short, lenthSpeedHeadIndex = " + lenthSpeedHeadIndex);
}
else
{
arrRecvmsglist.CopyTo(lenthSpeedHeadIndex, lenthSpeedData, 0, LeastLenthSpeedPosPktLenth);
if (lenthSpeedData[0] == lenthSpeedPacketHead[0] && lenthSpeedData[1] == lenthSpeedPacketHead[1] &&
lenthSpeedData[LeastLenthSpeedPosPktLenth - 2] == lenthSpeedPacketTail[0] &&
lenthSpeedData[LeastLenthSpeedPosPktLenth - 1] == lenthSpeedPacketTail[1])
{
DistancePosSpeedPacket DistanceSpeedDrawData = new DistancePosSpeedPacket();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt; i++)
{
DistanceSpeedDrawData.InDataArray[i] = (byte)((lenthSpeedData[2 + i * 2]) | (byte)(lenthSpeedData[2 + i * 2 + 1] << 8));
}
AllInDataProcess.EnQueueLenthSpeedPkt(DistanceSpeedDrawData);
if (no_debug_output == false)
{
StringBuilder strDebugSpeedOut = new StringBuilder();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt + 2; i++)
{
strDebugSpeedOut.AppendFormat("{0:X2} {1:X2} ", lenthSpeedData[i * 2], lenthSpeedData[i * 2 + 1]);
}
strDebugSpeedOut.AppendFormat("\r\n\r\n");
AppendColorText2RichBox(strDebugSpeedOut.ToString());
strDebugSpeedOut.Clear();
}
arrRecvmsglist.RemoveRange(0, lenthSpeedHeadIndex + LeastLenthSpeedPosPktLenth);
}
else
{
if (lenthSpeedHeadIndex == 0)
{
arrRecvmsglist.RemoveAt(0);
}
else
{
arrRecvmsglist.RemoveRange(0, lenthSpeedHeadIndex);
}
}
}
int lenthPosHeadIndex = arrRecvmsglist.IndexOf(lenthPosPacketHead[0]);
if (lenthPosHeadIndex + LeastLenthSpeedPosPktLenth > arrRecvmsglist.Count)
{
Console.WriteLine("too short, lenthSpeedHeadIndex = " + lenthPosHeadIndex);
}
else
{
arrRecvmsglist.CopyTo(lenthPosHeadIndex, lenthPosData, 0, LeastLenthSpeedPosPktLenth);
if (lenthPosData[0] == lenthPosPacketHead[0] && lenthPosData[1] == lenthPosPacketHead[1] &&
lenthPosData[LeastLenthSpeedPosPktLenth - 2] == lenthPosPacketTail[0] &&
lenthPosData[LeastLenthSpeedPosPktLenth - 1] == lenthPosPacketTail[1])
{
DistancePosSpeedPacket DistancePosDrawData = new DistancePosSpeedPacket();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt; i++)
{
DistancePosDrawData.InDataArray[i] = (Int16)((lenthPosData[2 + i * 2]) | (UInt16)(lenthPosData[2 + i * 2 + 1] << 8));
}
AllInDataProcess.EnQueueLenthPosPkt(DistancePosDrawData);
if (no_debug_output == false)
{
StringBuilder strDebugPosOut = new StringBuilder();
for (int i = 0; i < DistancePosSpeedPacket.iPointCnt + 2; i++)
{
strDebugPosOut.AppendFormat("{0:X2} {1:X2} ", lenthPosData[i * 2], lenthPosData[i * 2 + 1]);
}
strDebugPosOut.AppendFormat("\r\n\r\n");
AppendColorText2RichBox(strDebugPosOut.ToString());
strDebugPosOut.Clear();
}
arrRecvmsglist.RemoveRange(0, lenthPosHeadIndex + LeastLenthSpeedPosPktLenth);
}
else
{
if (lenthPosHeadIndex == 0)
{
arrRecvmsglist.RemoveAt(0);
}
else
{
arrRecvmsglist.RemoveRange(0, lenthPosHeadIndex);
}
}
}
}
return(true);
}