fix: cos/sin lookup table out of range bug when rpm slow

RoboSense-LiDAR · Aug 19, 2019 · 761bb9e · 761bb9e
1 parent e427de3
commit 761bb9e
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/rslidar_pointcloud/src/rawdata.cc b/rslidar_pointcloud/src/rawdata.cc
@@ -1020,7 +1020,7 @@ void RawData::unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointClou
         float distance2 = pixelToDistance(distance, dsr);
         distance2 = distance2 * DISTANCE_RESOLUTION_NEW;
 
-        int arg_horiz_orginal = azimuth_corrected_f;
+        int arg_horiz_orginal = (int)azimuth_corrected_f % 36000;
         int arg_horiz = azimuth_corrected;
         int arg_vert = ((VERT_ANGLE[dsr]) % 36000 + 36000) % 36000;
         pcl::PointXYZI point;
@@ -1098,7 +1098,7 @@ void RawData::unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointClou
         float distance2 = pixelToDistance(distance, dsr);
         distance2 = distance2 * DISTANCE_RESOLUTION;
 
-        int arg_horiz_orginal = azimuth_corrected_f;
+        int arg_horiz_orginal = (int)azimuth_corrected_f % 36000;
         int arg_horiz = azimuth_corrected;
         int arg_vert = ((VERT_ANGLE[dsr]) % 36000 + 36000) % 36000;