public static void LidarPicketToDistance() //720分割
{
double LocationX = 0;
double LocationY = 0;
double LocationZ = 0;
int RotationAngle = 0;
for (int i = 0; i < 12; i++)
{
int VetricalCounter = 0;
//水平角度查表
RotationAngle = ((LidarBuffer[i * 100 + 3]) * 256 + LidarBuffer[i * 100 + 2]) / 100;
//int RoIndex = (int)((double)((LidarBuffer[i * 100 + 3]) * 256 + LidarBuffer[i * 100 + 2]) / (double)200);
int RoIndex = (int)((double)((LidarBuffer[i * 100 + 3]) * 256 + LidarBuffer[i * 100 + 2]) / (LidarFunc.Lidar_Accuracy * 100));
double CosRotationAngle = MathFunc.CosTable(RotationAngle);
double SinRotationAngle = MathFunc.SinTable(RotationAngle);
for (int j = 4; j < 100; j += 3)
{
//距離換算
float Distance = (float)(LidarBuffer[i * 100 + j + 1] * 256 + LidarBuffer[i * 100 + j]) / (float)500; //換成公尺
//反射強度
int Intensity = LidarBuffer[i * 100 + j + 2];
//if (Intensity < 8) continue;
//計算座標
LocationX = Distance * CosVerticalAngle[VetricalCounter] * SinRotationAngle;
LocationY = Distance * CosVerticalAngle[VetricalCounter] * CosRotationAngle;
LocationZ = Distance * SinVerticalAngle[VetricalCounter];
//有效距離
if (Math.Sqrt(LocationX * LocationX + LocationY * LocationY) > 0.1)
{
//資料不等於零 更新資料
if (LocationX != 0 && LocationY != 0 && LocationZ != 0)
{
int index = (j - 1) / 3 - 1;
rLidarData.X[RoIndex * 32 + index] = (float)LocationX;
rLidarData.Y[RoIndex * 32 + index] = (float)LocationY;
rLidarData.Z[RoIndex * 32 + index] = (float)LocationZ;
if (isLidarDataRecode)
{
LidarWriteBufferIndex.Add(RoIndex * 32 + index);
LidarWriteBufferX.Add((float)LocationX);
LidarWriteBufferY.Add((float)LocationY);
LidarWriteBufferZ.Add((float)LocationZ);
}
}
}
VetricalCounter++;
}
}
}
public static void LidarBufferToDistence() //360分割
{
//解析封包
double LocationX = 0;
double LocationY = 0;
double LocationZ = 0;
int RotationAngle = 0;
for (int i = 0; i < 12; i++)
{
int VetricalCounter = 0;
//水平角度查表
RotationAngle = ((LidarBuffer[i * 100 + 3]) * 256 + LidarBuffer[i * 100 + 2]) / 100;
double CosRotationAngle = MathFunc.CosTable(RotationAngle);
double SinRotationAngle = MathFunc.SinTable(RotationAngle);
for (int j = 4; j < 100; j += 3)
{
//距離換算
float Distance = (float)(LidarBuffer[i * 100 + j + 1] * 256 + LidarBuffer[i * 100 + j]) / (float)500; //換成公尺
//反射強度
int Intensity = LidarBuffer[i * 100 + j + 2];
//if (Intensity < 8) continue;
//計算座標
LocationX = Distance * CosVerticalAngle[VetricalCounter] * SinRotationAngle;
LocationY = Distance * CosVerticalAngle[VetricalCounter] * CosRotationAngle;
LocationZ = Distance * SinVerticalAngle[VetricalCounter];
//有效距離
if (Math.Sqrt(LocationX * LocationX + LocationY * LocationY) > 0.1)
{
//資料不等於零 更新資料
if (LocationX != 0 && LocationY != 0 && LocationZ != 0)
{
int index = (j - 1) / 3 - 1;
r_LidarData.X[RotationAngle * 32 + index] = (float)LocationX;
r_LidarData.Y[RotationAngle * 32 + index] = (float)LocationY;
r_LidarData.Z[RotationAngle * 32 + index] = (float)LocationZ;
}
}
VetricalCounter++;
}
}
}