chore(hesai_hw_interface): add set/get_ptp_lock_offset member functions

Signed-off-by: Max SCHMELLER <[email protected]>
tier4 · Nov 26, 2024 · 5a49fa1 · 5a49fa1
1 parent a49ebf8
commit 5a49fa1
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git a/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
@@ -75,6 +75,8 @@ const uint8_t PTC_COMMAND_SET_LIDAR_RANGE = 0x22;
 const uint8_t PTC_COMMAND_GET_LIDAR_RANGE = 0x23;
 const uint8_t PTC_COMMAND_SET_PTP_CONFIG = 0x24;
 const uint8_t PTC_COMMAND_GET_PTP_CONFIG = 0x26;
+const uint8_t g_ptp_command_set_ptp_lock_offset = 0x39;
+const uint8_t g_ptp_command_get_ptp_lock_offset = 0x3a;
 const uint8_t PTC_COMMAND_RESET = 0x25;
 const uint8_t PTC_COMMAND_SET_ROTATE_DIRECTION = 0x2a;
 const uint8_t PTC_COMMAND_LIDAR_MONITOR = 0x27;
@@ -364,6 +366,11 @@ class HesaiHwInterface
   /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG
   /// @return Resulting status
   HesaiPtpConfig GetPtpConfig();
+
+Status set_ptp_lock_offset(uint8_t lock_offset);
+
+uint8_t get_ptp_lock_offset();
+
   /// @brief Sending command with PTC_COMMAND_RESET
   /// @return Resulting status
   Status SendReset();

diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
@@ -684,6 +684,23 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig()
   return hesai_ptp_config;
 }
 
+Status HesaiHwInterface::set_ptp_lock_offset(uint8_t lock_offset_us)
+{
+  std::vector<uint8_t> request_payload;
+  request_payload.emplace_back(lock_offset_us);
+
+  auto response_or_err = SendReceive(g_ptp_command_set_ptp_lock_offset, request_payload);
+  response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
+  return Status::OK;
+}
+
+uint8_t HesaiHwInterface::get_ptp_lock_offset()
+{
+  auto response_or_err = SendReceive(g_ptp_command_get_ptp_lock_offset);
+  auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
+  return CheckSizeAndParse<uint8_t>(response);
+}
+
 Status HesaiHwInterface::SendReset()
 {
   auto response_or_err = SendReceive(PTC_COMMAND_RESET);