Exemple #1
0
        /// <summary>
        /// Получить данные с лидара об определённом секторе
        /// </summary>
        /// <param name="azimuthFrom">Угол начала сектора в градусах</param>
        /// <param name="azimuthTo">Угол конца сектора в градусах</param>
        /// <returns>Возвращает данные с лидара, упакованные в List</returns>
        public List <LidarDataBlock> GetData(double azimuthFrom, double azimuthTo)
        {
            NormalizeAngle(ref azimuthFrom);
            NormalizeAngle(ref azimuthTo);
            List <LidarDataBlock> LidarFormatedData  = new List <LidarDataBlock>();
            UdpClient             receivingUdpClient = new UdpClient(Port);
            IPEndPoint            RemoteIpEndPoint   = null;
            double angle;                  //Текущий азимут
            double oldAngle;               //Предыдущий азимут
            bool   recording      = false; //Флаг записи данных
            bool   firstItterFlag = true;  //Флаг показывает, что обрабатывается первый пакет данных

            //Чтение данных по UDP
            lidarData = receivingUdpClient.Receive(ref RemoteIpEndPoint);
            if (lidarData.Length != 1206)
            {
                receivingUdpClient.Close();
                throw new DataMisalignedException("Incorrect length");
            }
            int pos = 2;

            angle = CalcAzimuth(GetByte(pos), GetByte(pos + 1));
            int i;

            while (true)
            {
                if (!firstItterFlag) //Если проверяется не первый массив данных
                {
                    pos = 2;
                    i   = 0;
                }
                else
                {
                    pos            = 102;
                    i              = 1;
                    firstItterFlag = false;
                }
                //Сканирование массива данных
                for (; i < 12; ++i)
                {
                    oldAngle = angle;
                    if (oldAngle >= 360)
                    {
                        oldAngle -= 360;
                    }
                    angle = CalcAzimuth(GetByte(pos), GetByte(pos + 1));

                    //Решение проблемы со скачком при переходе через 360
                    if (oldAngle > angle) //Переход через 360
                    {
                        if (!recording && azimuthFrom >= 180 || recording && azimuthTo >= 180)
                        {
                            angle += 360;
                        }
                        else
                        {
                            oldAngle -= 360;
                        }
                    }
                    if (!recording)
                    {
                        if (azimuthFrom > oldAngle && azimuthFrom <= angle)
                        {
                            recording = true;
                        }
                    }
                    if (recording)
                    {
                        if (azimuthTo > oldAngle && azimuthTo <= angle)
                        {
                            lidarData = null;
                            receivingUdpClient.Close();
                            return(LidarFormatedData);
                        }
                        LidarDataBlock block = new LidarDataBlock(lidarData, i);
                        LidarFormatedData.Add(block);
                    }
                    pos += 100;
                }
                //Чтение данных по UDP
                lidarData = receivingUdpClient.Receive(ref RemoteIpEndPoint);
                if (lidarData.Length != 1206)
                {
                    lidarData     = null;
                    prevLidarData = null;
                    receivingUdpClient.Close();
                    throw new DataMisalignedException("Incorrect length");
                }
            }
        }
Exemple #2
0
        //Приватные методы

        /// <summary>
        /// Найти точку, до которой дальность максимальна или минимальна
        /// </summary>
        /// <param name="dist">Найденная дальность</param>
        /// <param name="azimuth">Азимут точки</param>
        /// <param name="channel">Канал</param>
        /// <param name="findMaz">true - если ищется максимум</param>
        /// <param name="azimuthFrom">Угол начала сектора в градусах</param>
        /// <param name="azimuthTo">Угол конца сектора в градусах</param>
        /// <param name="channelsList">Каналы, на которых осуществляется поиск</param>
        private void FindExtremum(out int dist, out double azimuth, out int channel,
                                  bool findMax, double azimuthFrom, double azimuthTo, int[] channelsList)
        {
            List <LidarDataBlock> LidarFormatedData = this.GetData(azimuthFrom, azimuthTo);

            if (LidarFormatedData.Count == 0)
            {
                throw new ArgumentOutOfRangeException("Azimuth interval is too small");
            }
            LidarDataBlock block = LidarFormatedData.ElementAt(0);

            azimuth = azimuth = block.Azimuth;
            dist    = block.Distance1[channelsList[0]];
            channel = channelsList[0];
            for (int i = 0; i < LidarFormatedData.Count; ++i)
            {
                block = LidarFormatedData.ElementAt(i);
                for (int j = 0; j < channelsList.Length; j++)
                {
                    if (block.Distance1[channelsList[j]] > dist && findMax ||
                        block.Distance1[channelsList[j]] < dist && !findMax)
                    {
                        azimuth = block.Azimuth;
                        dist    = block.Distance1[channelsList[j]];
                        channel = channelsList[j];
                    }
                }
                for (int j = 0; j < block.Distance2.Length; j++)
                {
                    if (block.Distance2[channelsList[j]] > dist && findMax ||
                        block.Distance2[channelsList[j]] < dist && !findMax)
                    {
                        if (i < LidarFormatedData.Count - 1)
                        {
                            LidarDataBlock nextBlock = LidarFormatedData.ElementAt(i + 1);
                            double         azimuth1  = block.Azimuth;
                            double         azimuth2  = nextBlock.Azimuth;
                            if (azimuth2 < azimuth1)
                            {
                                azimuth2 += 360;
                            }
                            azimuth = (block.Azimuth + nextBlock.Azimuth) / 2;
                        }
                        else
                        {
                            LidarDataBlock firstBlock   = LidarFormatedData.ElementAt(0);
                            double         deltaAzimuth = 0.5; //Шаг азимута
                            if (LidarFormatedData.Count > 1)
                            {
                                LidarDataBlock secondBlock = LidarFormatedData.ElementAt(1);
                                double         azimuth1    = firstBlock.Azimuth;
                                double         azimuth2    = secondBlock.Azimuth;
                                if (azimuth2 < azimuth1)
                                {
                                    azimuth2 += 360;
                                }
                                deltaAzimuth = (azimuth2 - azimuth1) / 2;
                            }
                            azimuth += deltaAzimuth;
                        }
                        dist    = block.Distance1[channelsList[j]];
                        channel = channelsList[j];
                        NormalizeAngle(ref azimuth);
                    }
                }
            }
        }