feat: implemented missing features (although nothing has been tested)

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

nebula_common/include/nebula_common/continental/continental_srr520.hpp

@@ -43,6 +43,7 @@ namespace continental_srr520
 struct ContinentalSRR520SensorConfiguration : CANSensorConfigurationBase
 {
   std::string base_frame{};
+  bool sync_use_bus_time{};
   uint8_t new_sensor_id;
   bool new_plug_bottom{};
   float new_longitudinal_cog{};
@@ -60,8 +61,10 @@ inline std::ostream & operator<<(
   std::ostream & os, ContinentalSRR520SensorConfiguration const & arg)
 {
   os << (CANSensorConfigurationBase)(arg) << ", BaseFrame: " << arg.base_frame
-     << ", NewSensorId: " << arg.new_sensor_id << ", NewPlugBottom: " << arg.new_plug_bottom
-     << ", NewLongitudinalCog: " << static_cast<uint16_t>(arg.new_longitudinal_cog)
+     << ", SyncUseBusTime: " << arg.sync_use_bus_time
+     << ", NewSensorId: " << static_cast<uint16_t>(arg.new_sensor_id)
+     << ", NewPlugBottom: " << arg.new_plug_bottom
+     << ", NewLongitudinalCog: " << arg.new_longitudinal_cog
      << ", NewVehicleWheelbase: " << arg.new_vehicle_wheelbase
      << ", NewCoverDamping: " << arg.new_cover_damping
      << ", ResetSensorConfiguration: " << arg.reset_sensor_configuration;

nebula_common/include/nebula_common/continental/crc.hpp

@@ -0,0 +1,63 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+template <typename Iterator>
+int crc16_packets(Iterator begin, Iterator end)
+{
+  uint16_t crc_word = 0xffff;
+
+  for (Iterator it = begin; it != end; ++it) {
+    for (const uint8_t byte : it->data) {
+      crc_word ^= byte << 8;
+      for (int i = 0; i < 8; i++)
+        crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1;
+    }
+  }
+
+  return crc_word;
+}
+
+template <typename Iterator>
+int crc16_packet(Iterator begin, Iterator end)
+{
+  uint16_t crc_word = 0xffff;
+  for (Iterator it = begin; it != end; ++it) {
+    crc_word ^= (*it) << 8;
+    for (int i = 0; i < 8; i++)
+      crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1;
+  }
+
+  return crc_word;
+}
+
+template <typename Iterator>
+uint8_t crc8h2f(Iterator begin, Iterator end)
+{
+  uint8_t crc_word = 0xFF;
+  uint8_t bit = 0;
+
+  for (Iterator it = begin; it != end; ++it) {
+    crc_word ^= *it;
+    for (bit = 0; bit < 8; bit++) {
+      if ((crc_word & 0x80) != 0) {
+        crc_word <<= 1;
+        crc_word ^= 0x2F;
+      } else {
+        crc_word <<= 1;
+      }
+    }
+  }
+
+  return crc_word ^ 0xFF;
+}

nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp

@@ -16,6 +16,8 @@
 
 #include "nebula_common/continental/continental_srr520.hpp"
 
+#include <nebula_common/continental/crc.hpp>
+
 #include <cmath>
 #include <utility>
 
@@ -337,20 +339,6 @@ bool ContinentalSRR520Decoder::ParseObjectsListPacket(
   return true;
 }
 
-template <typename Iterator>
-int crc16(Iterator begin, Iterator end)
-{
-  uint16_t w_crc = 0xffff;
-  int counter = 0;
-  for (Iterator it = begin; it != end; ++it) {
-    counter++;
-    w_crc ^= (*it) << 8;
-    for (int i = 0; i < 8; i++) w_crc = w_crc & 0x8000 ? (w_crc << 1) ^ 0x1021 : w_crc << 1;
-  }
-
-  return w_crc;
-}
-
 bool ContinentalSRR520Decoder::ParseStatusPacket(
   const nebula_msgs::msg::NebulaPackets & nebula_packets)
 {
@@ -536,7 +524,7 @@ bool ContinentalSRR520Decoder::ParseStatusPacket(
   diagnostic_values.push_back(key_value);
 
   uint16_t computed_crc =
-    crc16(nebula_packets.packets[1].data.begin(), nebula_packets.packets[1].data.end() - 3);
+    crc16_packet(nebula_packets.packets[1].data.begin(), nebula_packets.packets[1].data.end() - 3);
   key_value.key = "crc_check";
   key_value.value =
     std::to_string(status_packet.u_crc.value()) + "|" + std::to_string(computed_crc);

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp

@@ -79,8 +79,16 @@ class ContinentalSRR520HwInterface : NebulaHwInterfaceBase
   bool first_rdi_hrr_packet_{true};
   bool first_object_packet_{true};
 
+  uint8_t sync_counter_{0};
+  bool sync_fup_sent_{false};
+
   std::shared_ptr<rclcpp::Logger> parent_node_logger;
 
+  /// @brief Send a Fd frame
+  /// @param data a buffer containing the data to send
+  template <std::size_t N>
+  bool SendFrame(const std::array<uint8_t, N> & data, int can_frame_id);
+
   /// @brief Printing the string to RCLCPP_INFO_STREAM
   /// @param info Target string
   void PrintInfo(std::string info);
@@ -97,10 +105,6 @@ class ContinentalSRR520HwInterface : NebulaHwInterfaceBase
   /// @param bytes Target byte vector
   void PrintDebug(const std::vector<uint8_t> & bytes);
 
-public:
-  /// @brief Constructor
-  ContinentalSRR520HwInterface();
-
   /// @brief Main loop of the CAN receiver thread
   void ReceiveLoop();
 
@@ -164,8 +168,6 @@ class ContinentalSRR520HwInterface : NebulaHwInterfaceBase
   /// @param stamp The stamp in nanoseconds
   void ProcessSyncFupPacket(const std::vector<uint8_t> & buffer, const uint64_t stamp);
 
-  void SyncTimerCallback();
-
   /// @brief Process a new filter status packet
   /// @param buffer The buffer containing the status packet
   void ProcessFilterStatusPacket(const std::vector<uint8_t> & buffer);
@@ -178,6 +180,10 @@ class ContinentalSRR520HwInterface : NebulaHwInterfaceBase
   /// @param buffer Buffer containing the data received from the UDP socket
   void ReceiveSensorPacketCallback(const std::vector<uint8_t> & buffer, int id, uint64_t stamp);
 
+public:
+  /// @brief Constructor
+  ContinentalSRR520HwInterface();
+
   /// @brief Starting the interface that handles UDP streams
   /// @return Resulting status
   Status SensorInterfaceStart() final;
@@ -203,6 +209,9 @@ class ContinentalSRR520HwInterface : NebulaHwInterfaceBase
   Status RegisterScanCallback(
     std::function<void(std::unique_ptr<nebula_msgs::msg::NebulaPackets>)> scan_callback);
 
+  /// @brief Sensor synchronization routine
+  void SensorSync();
+
   /// @brief Configure the sensor
   /// @param sensor_id Desired sensor id
   /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates