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); }