优化计算速度，待测试

encTest/Form1.cs

@@ -20,9 +20,14 @@ namespace Test
     {
         private SickTimEnc laserLeft;
         private SickTimEnc laserRight;
-        private int max_axis_Y = 4000;//2250;//画图时Y轴最大量程
+        private int max_axis_Y = 2600;//2250;//画图时Y轴最大量程
         private List<DispMsg> dataDispList;
         private List<string> colors;//雷达数据颜色
+        private bool obstacleDetected;
+        private const string leftTrue = "left: True ";
+        private const string leftFalse = "left: False ";
+        private const string rightTrue = "right: True ";
+        private const string rightFalse = "right: False ";
         //************** plc相关参数 *************
         /// <summary>
         /// PLC 连接状态flag
@@ -31,7 +36,7 @@ namespace Test
         /// <summary>
         /// plc中cpu类型
         /// </summary>
-        private const CpuType cpu = CpuType.S71200;
+        private const CpuType cpu = CpuType.S71500;
         /// <summary>
         /// PLC的IP地址
         /// </summary>
@@ -63,7 +68,7 @@ namespace Test
         /// <summary>
         /// 读写锁
         /// </summary>
-        private object readWriteLock = new object();
+        private static object readWriteLock = new object();
         //写入频率
         private const int writeFreq = 25;
         private const short DB = 30;
@@ -82,15 +87,17 @@ namespace Test
         //流程控制
         private void Start()
         {
+            obstacleDetected = false;
             colors = new List<string>();
             colors.Add(Color.Red.Name);
             colors.Add(Color.Yellow.Name);
             colors.Add(Color.Cyan.Name);
             colors.Add(Color.Blue.Name);
             dataDispList = new List<DispMsg>();
+            //初始化雷达与数据显示
             Task.Factory.StartNew(() =>
             {
-                laserLeft = new SickTimEnc("192.168.0.77", 2111);
+                //laserLeft = new SickTimEnc("192.168.0.77", 2111);
                 laserRight = new SickTimEnc("192.168.0.88", 2111);
                 while (!IsHandleCreated)
                 {
@@ -103,13 +110,13 @@ namespace Test
                     if (laserLeft != null)
                     {
                         Pause();
-                        laserLeft.SetLeftRange(75);
-                        laserLeft.SetRightRange(105);
-                        laserLeft.ground_truth_height = 2600;
+                        laserLeft.SetLeftRange(65);
+                        laserLeft.SetRightRange(115);
+                        laserLeft.ground_truth_height = 4050;
                         laserLeft.min_num_point_of_hinder = 20;
                         laserLeft.min_obstacle_height = 100;
-                        tb_angleLeft.Text = "75";
-                        tb_angleRight.Text = "105";
+                        tb_angleLeft.Text = "65";
+                        tb_angleRight.Text = "115";
                         Pause();
                         List<List<double>> temp = new List<List<double>>();
                         temp.Add(laserLeft.yFiltered);
@@ -121,11 +128,11 @@ namespace Test
                         Pause();
                         laserRight.SetLeftRange(75);
                         laserRight.SetRightRange(105);
-                        laserRight.ground_truth_height = 2600;
+                        laserRight.ground_truth_height = 2200;
                         laserRight.min_num_point_of_hinder = 20;
                         laserRight.min_obstacle_height = 100;
                         tb_angleLeft.Text = "75";
-                        tb_angleRight.Text = "105";
+                        tb_angleRight.Text = "125";
                         Pause();
                         List<List<double>> temp = new List<List<double>>();
                         temp.Add(laserRight.yFiltered);
@@ -133,24 +140,10 @@ namespace Test
                         chartAddData(laserRight.listx, temp, 2);
                     }
                 }));
-                //Task.Factory.StartNew(()=>
-                //{
-                //    int height = 2600;
-                //    Thread.Sleep(5000);
-                //    while (height >= 200)
-                //    {
-                //        height--;
-                //        if (laserLeft != null)
-                //            laserLeft.ground_truth_height = height;
-                //        if (laserRight != null)
-                //            laserRight.ground_truth_height = height;
-                //        Console.WriteLine(height);
-                //        Thread.Sleep(3);
-                //    }
-                //});
                 while (!closed)
                 {
-                    //System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+                    System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+                    sw.Start();
                     try
                     {
                         this.Invoke(new Action(() =>
@@ -165,70 +158,52 @@ namespace Test
                                 {
                                     updateChartData(laserLeft.listx, laserLeft.yFiltered, 0);
                                     updateChartData(laserLeft.listx, laserLeft.yFilteredKalman, 1);
-                                    lb_obstacle.Text = "  left:" + laserLeft.obstacleDetected+"  ";
+                                    //lb_obstacle.Text = "  left:" + laserLeft.obstacleDetected + "  ";
+                                    lb_obstacle.Text = laserLeft.obstacleDetected ? leftTrue : leftFalse;
+                                    //lb_obstacle.Text = "  left:" + laserLeft.dataCount + "  ";
+                                    obstacleDetected = false || laserLeft.obstacleDetected;
                                 }
-                                if (dataDispList.Count >= 4 && laserRight != null)
+                                if (laserRight != null)
                                 {
-                                    updateChartData(laserRight.listx, laserRight.yFiltered, 2);
-                                    updateChartData(laserRight.listx, laserRight.yFilteredKalman, 3);
-                                    lb_obstacle.Text += "right:" + laserRight.obstacleDetected + "  ";
+                                    updateChartData(laserRight.listx, laserRight.yFiltered, dataDispList.Count >=4 ?2:0);
+                                    updateChartData(laserRight.listx, laserRight.yFilteredKalman, dataDispList.Count >= 4 ?3:1);
+                                    //lb_obstacle.Text = (dataDispList.Count >= 4?lb_obstacle.Text:String.Empty)+"right:" + laserRight.obstacleDetected + "  ";
+                                    if (dataDispList.Count >= 4)
+                                        lb_obstacle.Text += laserRight.obstacleDetected ? rightTrue : rightFalse;
+                                    else
+                                        lb_obstacle.Text = laserRight.obstacleDetected ? rightTrue : rightFalse;
+                                    //lb_obstacle.Text += "right:" + laserRight.dataCount + "  ";
+                                    obstacleDetected = obstacleDetected || laserRight.obstacleDetected;
                                 }
                                 
                             }
                         }));
                     }
                     catch { Stop(); }
-                    //sw.Stop();
-                    //Console.WriteLine(sw.ElapsedMilliseconds + "ms");
+                    sw.Stop();
+                    Console.WriteLine("laser: " + sw.ElapsedMilliseconds + "ms");
                     Thread.Sleep(30);
                 }
             });
-        }
-        /// <summary>
-        /// 暂停雷达数据处理线程
-        /// </summary>
-        private void Pause()
-        {
-            if (laserLeft != null)
-            {
-                laserLeft.Pause();
-            }
-            if (laserRight != null)
-            {
-                laserRight.Pause();
-            }
-        }
-        /// <summary>
-        /// 停止获取雷达数据，断开连接
-        /// </summary>
-        private void Stop()
-        {
-            if (laserLeft != null)
-            {
-                laserLeft.Stop();
-            }
-            if (laserRight != null)
-            {
-                laserRight.Stop();
-            }
-        }
 
-        public obstacleDetectionForm()
-        {
-            InitializeComponent();
-            Start();
-            //************* PLC 相关逻辑 *************
+            ////************* PLC 相关逻辑 *************
             //bool result = PLCConnect();
             //if (!result)
+            //{
+            //    Console.WriteLine("PLC连接失败");
             //    Environment.Exit(0);
+            //}
             //else
             //{
             //    MsgNode msg = new MsgNode();
             //    msg.db = DB;
             //    msg.length = 2;
-            //    Task.Factory.StartNew(() => {
+            //    Task.Factory.StartNew(() =>
+            //    {
             //        while (!closed)
             //        {
+            //            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+            //            sw.Start();
             //            if (plc == null)
             //            {
             //                Thread.Sleep(1000);
@@ -249,29 +224,29 @@ namespace Test
             //            msg.start = ZPause;
             //            ReadFromPLC(ref msg);
             //            ZPauseValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
-            //            Console.WriteLine("position, start, stop, pause: "+ZPositionValue+", "+ZStartValue+", "+ZStopValue+", "+ZPauseValue);
+            //            //Console.WriteLine("position, start, stop, pause: " + ZPositionValue + ", " + ZStartValue + ", " + ZStopValue + ", " + ZPauseValue);
 
             //            if (ZPauseValue == 1)
             //            {
-            //                Pause(true);
-            //                Console.WriteLine("暂停");
+            //                Pause();
+            //                //Console.WriteLine("暂停");
             //            }
             //            else
             //            {
-            //                Pause(false);
+            //                Pause();
             //                //Console.WriteLine("恢复");
             //            }
 
             //            //收到开始下降信号
-            //            if (ZStartValue == 1 && ZStopValue!=1)
+            //            if (ZStartValue == 1 && ZStopValue != 1)
             //            {
             //                startGoingDown = true;
-            //                Console.WriteLine("下降过程");
+            //                //Console.WriteLine("下降过程");
             //            }
             //            else
             //            {
             //                startGoingDown = false;
-            //                Console.WriteLine("非下降过程");
+            //                //Console.WriteLine("非下降过程");
             //            }
 
             //            //进入下降阶段
@@ -285,17 +260,64 @@ namespace Test
             //                }
             //            }
             //            //下降结束复位工作状态
-            //            if(laserWorking && ZStopValue == 1)
+            //            if (laserWorking && ZStopValue == 1)
             //            {
             //                Console.WriteLine("雷达工作结束");
             //                laserWorking = false;
             //            }
 
-            //            Thread.Sleep(50);
+            //            if (laserWorking)
+            //            {
+            //                MsgNode writeMsg = new MsgNode();
+            //                writeMsg.db = 41;
+            //                writeMsg.length = 2;
+            //                writeMsg.start = 60;
+            //                byte[] temp = { 0, 0 };
+            //                if (obstacleDetected)
+            //                    temp[1] = 1;
+            //                WriteToPLC(writeMsg);
+            //            }
+            //            sw.Stop();
+            //            Console.WriteLine("PLC: " + sw.ElapsedMilliseconds + "ms");
+            //            Thread.Sleep(1000);
             //        }
             //    });
             //}
         }
+        /// <summary>
+        /// 暂停雷达数据处理线程
+        /// </summary>
+        private void Pause()
+        {
+            if (laserLeft != null)
+            {
+                laserLeft.Pause();
+            }
+            if (laserRight != null)
+            {
+                laserRight.Pause();
+            }
+        }
+        /// <summary>
+        /// 停止获取雷达数据，断开连接
+        /// </summary>
+        private void Stop()
+        {
+            if (laserLeft != null)
+            {
+                laserLeft.Stop();
+            }
+            if (laserRight != null)
+            {
+                laserRight.Stop();
+            }
+        }
+
+        public obstacleDetectionForm()
+        {
+            InitializeComponent();
+            Start();
+        }
         //界面按钮
         private void button1_Click(object sender, EventArgs e)
         {
@@ -350,9 +372,9 @@ namespace Test
             this.laserDataChart.ChartAreas[0].AxisY.IsReversed = true;
             this.laserDataChart.ChartAreas[0].AxisY.Minimum = 0;
             this.laserDataChart.ChartAreas[0].AxisY.Maximum = max_axis_Y;
-            this.laserDataChart.ChartAreas[0].AxisY.Interval = 100;
-            this.laserDataChart.ChartAreas[0].AxisX.Minimum = -2300;// -200;
-            this.laserDataChart.ChartAreas[0].AxisX.Maximum = 2300;// 200;
+            this.laserDataChart.ChartAreas[0].AxisY.Interval = 50;
+            this.laserDataChart.ChartAreas[0].AxisX.Minimum = -2500;// -200;
+            this.laserDataChart.ChartAreas[0].AxisX.Maximum = 2500;// 200;
             this.laserDataChart.ChartAreas[0].AxisX.Interval = 100;// 100;
             this.laserDataChart.ChartAreas[0].AxisX.MajorGrid.LineColor = System.Drawing.Color.Silver;
             this.laserDataChart.ChartAreas[0].AxisY.MajorGrid.LineColor = System.Drawing.Color.Silver;

encTest/SickTimEnc.cs

@@ -28,6 +28,7 @@ namespace Test
         /// 发送指令返回值
         /// </summary>
         private int flag = 0;
+        public int dataCount = 0;
         //通信参数
         private readonly object Lock = new object();
         private IPAddress ipa;
@@ -37,6 +38,8 @@ namespace Test
         private int bufferSize = 16384;
         private const string startCmd = "02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 03";
         private const string endCmd = "02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 30 03";
+        private static char[] prefix = "sRA LMDscandata 1 ".ToCharArray();
+        private static char[] postfix = " not defined 0 0 0".ToCharArray();
         //防撞参数
         public int maxDataCount { get; set; }//最大保留历史数据个数
         private int left_range = 385;//325;//231;//271;//386;//366;
@@ -44,7 +47,7 @@ namespace Test
         public int ground_truth_height { get; set; } = 2200;//2200;//2300;//标准高度
         public int min_num_point_of_hinder { get; set; } = 30;//20;//判定为障碍物的最少点的数目
         public int min_obstacle_height { get; set; } = 500;//最小障碍物高度
-        private List<string> originDatalist;//原始数据数组
+        private List<byte[]> originDatalist;//原始数据数组
         private List<int> validNum;//811个整型值
         private List<List<double>> validNumBuffer;//保存历史点云数据
         private List<List<double>> validNumBufferKalman;//保存经过kalman滤波的历史点云数据
@@ -59,7 +62,7 @@ namespace Test
         {
             initialized = false;
             maxDataCount = 3;
-            originDatalist = new List<string>();
+            originDatalist = new List<byte[]>();
             validNum = new List<int>();
             validNumBuffer = new List<List<double>>();
             validNumBufferKalman = new List<List<double>>();
@@ -234,7 +237,9 @@ namespace Test
                     {
                         if (working)
                         {
-                        //开辟线程存储雷达原始数据字符串
+                            //System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+                            //sw.Start();
+                            //开辟线程存储雷达原始数据字符串
                             lock (Lock)
                             {
                                 originDatalist.Clear();
@@ -243,17 +248,42 @@ namespace Test
                                 {
                                     byte[] buffer = new byte[bufferSize];
                                     stream.Read(buffer, 0, buffer.Length);
-                                    string str = HexStringToASCII(buffer);
-                                    originDatalist.Add(str);
+                                    //string str = HexStringToASCII(buffer);
+                                    originDatalist.Add(buffer);
                                 }
                                 if (originDatalist.Count != 0)
                                 {
+                                    System.Diagnostics.Stopwatch sw1 = new System.Diagnostics.Stopwatch();
+                                    sw1.Start();
+                                    List<byte> result = new List<byte>();
                                     for (int i = 0; i < originDatalist.Count; i++)
                                     {
-                                        string ss = trimDataStr(originDatalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
-                                        string[] data = ss.Split(' ');
-                                        string number = data[22];
-                                        int length = Convert.ToInt32(number, 16);
+                                        //string ss = trimDataStr(originDatalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
+                                        bool trimResult = trimData(originDatalist[i], out result);
+                                        if (!trimResult)
+                                            continue;
+
+                                        //string[] data = ss.Split(' ');
+                                        //string number = data[22];
+                                        byte[] lengthArr = { 32, 51, 50, 66 };
+                                        int index = 0,index2 = 0, length=0;
+                                        while(index < 500 && index2 < lengthArr.Length)
+                                        {
+                                            if (result[index] == lengthArr[index2])
+                                                index2++;
+                                            index++;
+                                        }
+                                        if (index2 < lengthArr.Length)
+                                            continue;
+                                        else
+                                            length = 811;
+                                        //while (result[index] != ' ')
+                                        //{
+                                        //    byte currByte = result[index];
+                                        //    length = (length << 4) + (currByte<='9'?(currByte-48):(currByte-55));
+                                        //    index++;
+                                        //}
+                                        //result[22]/10 * 16 + result[22] % 10;//Convert.ToInt32(number, 16);
                                         int temp = 0;
                                         //int[] validNum = new int[length];
                                         //string aws = "";
@@ -261,15 +291,30 @@ namespace Test
                                         validNum.Clear();
                                         listy.Clear();
                                         listx.Clear();
+                                        
                                         for (int j = 0; j < length; j++)
                                         {
                                             try
                                             {
-                                                temp = Convert.ToInt32(data[23 + j], 16);
+                                                temp = -1;
+                                                //temp = result[23+j] / 10 * 16 + result[23+j] % 10;//Convert.ToInt32(data[23 + j], 16);
+                                                while(result[index] == ' ')
+                                                {
+                                                    index++;
+                                                }
+                                                while (result[index] != ' ')
+                                                {
+                                                    byte currByte = result[index];
+                                                    temp = ((temp==-1?0:temp) << 4) + (currByte <= '9' ? (currByte - 48) : (currByte - 55));
+                                                    index++;
+                                                }
+                                                if (temp == -1)
+                                                    continue;
                                             }
                                             catch { temp = ground_truth_height; Console.WriteLine("数据转换异常"); }
                                             validNum.Add(temp);
                                         }
+                                        //Console.WriteLine("valid points: "+validNum.Count);
                                         for (int j = 0; j < length; j++)
                                         {
                                             double x = validNum[j] * Math.Cos((-45 + 0.333 * j) * Math.PI / 180);
@@ -277,6 +322,8 @@ namespace Test
                                             listy.Add(y);
                                             listx.Add(x);
                                         }
+                                       
+
                                         validNumBuffer.Add(listy);
                                         validNumBufferKalman.Add(kalmanFilter(listy));
                                         while (validNumBufferKalman.Count > maxDataCount)
@@ -284,14 +331,21 @@ namespace Test
                                             validNumBufferKalman.RemoveAt(0);
                                             validNumBuffer.RemoveAt(0);
                                         }
-                                        if(validNumBufferKalman.Count == maxDataCount)
+                                        
+                                        if (validNumBufferKalman.Count == maxDataCount)
                                         {
                                             bool obstacle = false;
                                             yFiltered = dataFilter(validNumBuffer, out obstacle);
                                             yFilteredKalman = dataFilter(validNumBufferKalman, out obstacle);
                                             obstacleDetected = obstacle;
                                         }
+                                        //sw.Stop();
+                                        //Console.WriteLine("laserData: " + sw.ElapsedMilliseconds + "ms");
+                                        //dataCount++;
+                                        //Console.WriteLine("laser:: "+dataCount);
                                     }
+                                    sw1.Stop();
+                                    Console.WriteLine("laserDataFilter: " + sw1.ElapsedMilliseconds + "ms");
                                 }
                             }
                             Thread.Sleep(1);
@@ -408,6 +462,50 @@ namespace Test
             catch (Exception) { }
             return result;
         }
+        /// <summary>
+        /// 截取数据段
+        /// </summary>
+        /// <param name="source"></param>
+        /// <param name="left"></param>
+        /// <param name="right"></param>
+        /// <returns></returns>
+        private static bool trimData(byte[] source, out List<byte> result)
+        {
+            int i = 0, j = 0, k=postfix.Length-1;
+            int left = 0, right = 0;
+            result = new List<byte>();
+            //find prefix index
+            while (j < prefix.Length && i<source.Length)
+            {
+                if (source[i] == prefix[j])
+                    j++;
+                i++;
+            }
+            if (j >= prefix.Length)
+                left = i;
+            else
+                return false;
+
+            //find postfix index
+            i = source.Length - 1;
+            while(k>=0 && i >= left)
+            {
+                if (postfix[k] == source[i])
+                    k--;
+                i--;
+            }
+            if (k < 0)
+                right = i;
+            else
+                return false;
+
+            for (int x = left; x <= right; x++)
+            {
+                result.Add(source[x]);
+            }
+            return true;
+        }
+
         /// <summary>
         /// 平滑数据，识别障碍物
         /// </summary>