/// <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");
}
}
}
//Приватные методы
/// <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);
}
}
}
}
