fix timing offsets calculation

tier4 · Apr 26, 2024 · 2b60694 · 2b60694
1 parent f499a96
commit 2b60694
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 10 deletions.
diff --git a/...rs/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp b/...rs/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp
@@ -83,9 +83,9 @@ class VelodyneDecoder : public VelodyneScanDecoder
         CHANNELS_PER_BLOCK + SensorT::num_maintenance_periods);  // y dir size
     }
 
-    double full_firing_cycle_s = 53.3 * 1e-6;
-    double single_firing_s = 2.665 * 1e-6;
-    double offset_packet_time = 8.7 * 1e-6;
+    double full_firing_cycle_s = SensorT::full_firing_cycle_s;
+    double single_firing_s = SensorT::single_firing_s;
+    double offset_packet_time = SensorT::offset_packet_time;
     bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL;
     double firing_sequence_index, data_point_index;
     // compute timing offsets
@@ -354,9 +354,7 @@ class VelodyneDecoder : public VelodyneScanDecoder
 
                 last_block_timestamp_ = block_timestamp;
 
-                // TODO: replace magic numbers 16 and 64
-                double point_time_offset =
-                  timing_offsets_[block][firing_seq * 16 + firing_order + laser_number / 64];
+                double point_time_offset = timing_offsets_[block][channel];
 
                 // Determine return type.
                 uint8_t return_type;

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp
@@ -29,6 +29,12 @@ class VLP16 : public VelodyneSensor
   constexpr static int channels_per_firing_sequence = 16;
 
   constexpr static float distance_resolution_m = 0.002f;
+
+  constexpr static double full_firing_cycle_s = 55.296 * 1e-6;
+
+  constexpr static double single_firing_s = 2.304 * 1e-6;
+
+  constexpr static double offset_packet_time = 0;
 };
 }  // namespace drivers
 }  // namespace nebula
diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp
@@ -12,13 +12,11 @@ class VLP32 : public VelodyneSensor
   bool fillAzimuthCache()
   {
     for (uint8_t i = 0; i < 16; i++) {
-      laser_azimuth_cache_[i] = (VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION) *
-                                (i + i / 2);  // TODO: better understand this calculation
+      laser_azimuth_cache_[i] = (VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION) * (i + i / 2);
     }
     return true;
   }
 
-  // TODO: implement this function
   uint16_t getAzimuthCorrected(
     uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order)
   {
@@ -35,7 +33,13 @@ class VLP32 : public VelodyneSensor
 
   constexpr static int channels_per_firing_sequence = 32;
 
-  constexpr static float distance_resolution_m = 0.004f;  // TODO: double check this
+  constexpr static float distance_resolution_m = 0.004f;
+
+  constexpr static double full_firing_cycle_s = 55.296 * 1e-6;
+
+  constexpr static double single_firing_s = 2.304 * 1e-6;
+
+  constexpr static double offset_packet_time = 0;
 
 private:
   float laser_azimuth_cache_[16];

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp
@@ -41,6 +41,12 @@ class VLS128 : public VelodyneSensor
 
   constexpr static float distance_resolution_m = 0.004f;
 
+  constexpr static double full_firing_cycle_s = 53.3 * 1e-6;
+
+  constexpr static double single_firing_s = 2.665 * 1e-6;
+
+  constexpr static double offset_packet_time = 8.7 * 1e-6;
+
 private:
   float laser_azimuth_cache_[16];
 };