From 2b606941d0c1c2997918aba7ed961e8f2982ab50 Mon Sep 17 00:00:00 2001
From: Benjamin Gilby <gilbybenjamin@gmail.com>
Date: Fri, 26 Apr 2024 11:44:26 +0900
Subject: [PATCH] fix timing offsets calculation

---
 .../decoders/velodyne_generic_decoder.hpp            | 10 ++++------
 .../nebula_decoders_velodyne/decoders/vlp_16.hpp     |  6 ++++++
 .../nebula_decoders_velodyne/decoders/vlp_32.hpp     | 12 ++++++++----
 .../nebula_decoders_velodyne/decoders/vls_128.hpp    |  6 ++++++
 4 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp
index b265a4cb1..f1d689855 100644
--- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp
+++ b/nebula_decoders/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
index 04ece09aa..f82376488 100644
--- 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
\ No newline at end of file
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
index 71c9eabaf..01d5e76a0 100644
--- 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
index 037b1651b..78772a428 100644
--- 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];
 };