public void frame_VLP16_decode(byte[] VLP16_frame)
{
//check frame size [frames_number,frame_value]
int frame_size = (VLP16_frame.Length / BYTES_IN_FRAME) - 1;
//init tables and variables
double[,] frame_decoded = new double[7, DATA_BLOCKS_IN_FRAME *POINTS_IN_READ *frame_size];
if (cloudX.cloudX.Count >= (frame_size * 12 * 32))
{
cloudX.cloudX.Clear();
}
for (int frame_index = 0; frame_index < frame_size; frame_index++)
{
//time stamp calc
byte timestamp_HHbyte = VLP16_frame[(1204 + frame_index * 1206)];
byte timestamp_HLbyte = VLP16_frame[(1203 + frame_index * 1206)];
byte timestamp_LHbyte = VLP16_frame[(1202 + frame_index * 1206)];
byte timestamp_LLbyte = VLP16_frame[(1201 + frame_index * 1206)];
int time_stamp = (timestamp_HHbyte << 24) | (timestamp_HLbyte << 16) | (timestamp_LHbyte << 8) | timestamp_LLbyte;
//Calc all data block
for (int DataBlock_index = 0; DataBlock_index < DATA_BLOCKS_IN_FRAME; DataBlock_index++)
{
//Calc azimuth ACTUAL
byte azimuth_Hbyte = VLP16_frame[((3 + DataBlock_index * 100) + BYTES_IN_FRAME * frame_index)];
byte azimuth_Lbyte = VLP16_frame[((2 + DataBlock_index * 100) + BYTES_IN_FRAME * frame_index)];
int azimuthINT = (azimuth_Hbyte << 8) | azimuth_Lbyte;
azimuth = Convert.ToDouble(azimuthINT) / 100.0;
//Calc azimuth NEXT
if (DataBlock_index < 11)
{
byte azimuth_HbyteN = VLP16_frame[((3 + (DataBlock_index + 1) * 100) + BYTES_IN_FRAME * frame_index)];
byte azimuth_LbyteN = VLP16_frame[((2 + (DataBlock_index + 1) * 100) + BYTES_IN_FRAME * frame_index)];
int azimuthINTN = (azimuth_HbyteN << 8) | azimuth_LbyteN;
azimuthN = Convert.ToDouble(azimuthINTN) / 100.0;
}
else if (DataBlock_index == 11)
{
byte azimuth_HbyteN = VLP16_frame[((3 + (DataBlock_index - 11) * 100) + BYTES_IN_FRAME * (frame_index + 1))];
byte azimuth_LbyteN = VLP16_frame[((2 + (DataBlock_index - 11) * 100) + BYTES_IN_FRAME * (frame_index + 1))];
int azimuthINTN = (azimuth_HbyteN << 8) | azimuth_LbyteN;
azimuthN = Convert.ToDouble(azimuthINTN) / 100.0;
}
azimuth_interpolated = azimuth_interpolation(azimuthN, azimuth);
for (int point_index = 0; point_index < POINTS_IN_READ; point_index++)
{
var index_rest = ((3 * point_index + 5) + 100 * DataBlock_index + BYTES_IN_FRAME * frame_index);
var index_restX = ((3 * point_index + 4) + 100 * DataBlock_index + BYTES_IN_FRAME * frame_index);
//Calc Distance
byte distance_Hbyte = VLP16_frame[((3 * point_index + 5) + 100 * DataBlock_index + BYTES_IN_FRAME * frame_index)];
byte distance_Lbyte = VLP16_frame[((3 * point_index + 4) + 100 * DataBlock_index + BYTES_IN_FRAME * frame_index)];
int distanceINT = (distance_Hbyte << 8) | distance_Lbyte;
double distance = (Convert.ToDouble(distanceINT) * 2.0) / 1000.0;
//Set azimuth
if (point_index > 15)
{
azimuth = azimuth_interpolated;
}
//Asing to 2d Array
//frame_decoded[0, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index;
//frame_decoded[1, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = azimuth;
//frame_decoded[2, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = distance;
//frame_decoded[3, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = time_stamp;
//frame_decoded[4, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = calc_X_cooridnates(distance, azimuth, point_index);
//frame_decoded[5, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = calc_Y_cooridnates(distance, azimuth, point_index);
//frame_decoded[6, point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = calc_Z_cooridnates(distance, azimuth, point_index);
//TEST VARIABLES
//azimuth_1DA.Add(azimuth);
azimuth_1DA[point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = azimuth;
X[point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = calc_X_cooridnates(distance, azimuth, point_index);
Y[point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = calc_Y_cooridnates(distance, azimuth, point_index);
Z[point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index] = calc_Z_cooridnates(distance, azimuth, point_index);
cloud_point point = new cloud_point(point_index + POINTS_IN_READ * DataBlock_index + POINTS_IN_READ * DATA_BLOCKS_IN_FRAME * frame_index, azimuth, calc_X_cooridnates(distance, azimuth, point_index), calc_Y_cooridnates(distance, azimuth, point_index), calc_Z_cooridnates(distance, azimuth, point_index));
cloudX.cloudX.Add(point);
}
}
}
frame_ready();
}
public void cloud_add(cloud_point point)
{
cloudX.Add(point);
}
