diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp
index aae00ea3f..8934c0412 100644
--- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp
+++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp
@@ -865,37 +865,94 @@ struct HesaiLidarRangeAll
   }
 };
 
-/// @brief struct of PTC_COMMAND_GET_PTP_CONFIG
-struct HesaiPtpConfig
+/// @brief Base struct for PTC_COMMAND_GET_PTP_CONFIG
+struct PtpConfigBase
 {
-  int8_t status;
-  int8_t profile;
-  int8_t domain;
-  int8_t network;
-  int8_t logAnnounceInterval;
-  int8_t logSyncInterval;
-  int8_t logMinDelayReqInterval;
-  // FIXME: this format is not correct for OT128, or for AT128 on 802.1AS
+  struct Internal
+  {
+    int8_t status;
+    int8_t profile;
+    int8_t domain;
+    int8_t network;
+  };
 
-  friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpConfig const & arg)
+  virtual ~PtpConfigBase() = default;
+
+  [[nodiscard]] ordered_json to_json() const
   {
-    os << "status: " << static_cast<int>(arg.status);
-    os << ", ";
-    os << "profile: " << static_cast<int>(arg.profile);
-    os << ", ";
-    os << "domain: " << static_cast<int>(arg.domain);
-    os << ", ";
-    os << "network: " << static_cast<int>(arg.network);
-    if (arg.status == 0) {
-      os << ", ";
-      os << "logAnnounceInterval: " << static_cast<int>(arg.logAnnounceInterval);
-      os << ", ";
-      os << "logSyncInterval: " << static_cast<int>(arg.logSyncInterval);
-      os << ", ";
-      os << "logMinDelayReqInterval: " << static_cast<int>(arg.logMinDelayReqInterval);
+    ordered_json j;
+    j["status"] = static_cast<int>(get().status);
+    j["profile"] = static_cast<int>(get().profile);
+    j["domain"] = static_cast<int>(get().domain);
+    j["network"] = static_cast<int>(get().network);
+    j.update(sensor_specifics_to_json());
+    return j;
+  }
+
+  [[nodiscard]] virtual const Internal & get() const = 0;
+
+protected:
+  [[nodiscard]] virtual ordered_json sensor_specifics_to_json() const = 0;
+
+  friend std::ostream & operator<<(std::ostream & os, const PtpConfigBase & arg)
+  {
+    ordered_json j = arg.to_json();
+    std::vector<std::string> kv_pairs;
+    for (const auto & [key, value] : j.items()) {
+      kv_pairs.emplace_back(key + ": " + to_string(value));
     }
-    return os;
+    return os << boost::algorithm::join(kv_pairs, ", ");
+  }
+};
+
+/// @brief PTP Config for AT128 sensors
+struct HesaiPtpConfig_AT128 : public PtpConfigBase
+{
+  struct Internal : public PtpConfigBase::Internal
+  {
+    int8_t tsn_switch;
+  };
+
+  explicit HesaiPtpConfig_AT128(Internal value) : value(value) {}
+
+  [[nodiscard]] const PtpConfigBase::Internal & get() const override { return value; }
+
+  [[nodiscard]] ordered_json sensor_specifics_to_json() const override
+  {
+    ordered_json j;
+    j["tsn_switch"] = static_cast<int>(value.tsn_switch);
+    return j;
+  }
+
+private:
+  Internal value;
+};
+
+/// @brief PTP Config for XT16, XT32, and 40P sensors
+struct HesaiPtpConfig_XT16_32_40P_OT128 : public PtpConfigBase
+{
+  struct Internal : public PtpConfigBase::Internal
+  {
+    int8_t logAnnounceInterval;
+    int8_t logSyncInterval;
+    int8_t logMinDelayReqInterval;
+  };
+
+  explicit HesaiPtpConfig_XT16_32_40P_OT128(Internal value) : value(value) {}
+
+  [[nodiscard]] const PtpConfigBase::Internal & get() const override { return value; }
+
+  [[nodiscard]] ordered_json sensor_specifics_to_json() const override
+  {
+    ordered_json j;
+    j["logAnnounceInterval"] = static_cast<int>(value.logAnnounceInterval);
+    j["logSyncInterval"] = static_cast<int>(value.logSyncInterval);
+    j["logMinDelayReqInterval"] = static_cast<int>(value.logMinDelayReqInterval);
+    return j;
   }
+
+private:
+  Internal value;
 };
 
 /// @brief struct of PTC_COMMAND_LIDAR_MONITOR
diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
index da7d18e11..b5bbf765e 100644
--- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
+++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
@@ -360,7 +360,7 @@ class HesaiHwInterface
     int logSyncInterval = 1, int logMinDelayReqInterval = 0);
   /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG
   /// @return Resulting status
-  HesaiPtpConfig get_ptp_config();
+  std::unique_ptr<PtpConfigBase> get_ptp_config();
 
   Status set_ptp_lock_offset(uint8_t lock_offset);
 
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
index 8e4f9176d..501479c90 100644
--- 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
@@ -691,25 +691,49 @@ Status HesaiHwInterface::set_ptp_config(
     return Status::ERROR_1;
   }
 
-  // Handle the OT128 differently - it has TSN settings and defines the PTP profile
-  // for automotive as 0x03 instead of 0x02 for other sensors.
-  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) {
-    if (profile != static_cast<int>(PtpProfile::IEEE_802_1AS_AUTO)) {
-      return Status::SENSOR_CONFIG_ERROR;
-    }
-    profile = 3;
-  }
-
   std::vector<unsigned char> request_payload;
-  request_payload.emplace_back(profile & 0xff);
-  request_payload.emplace_back(domain & 0xff);
-  request_payload.emplace_back(network & 0xff);
-  if (profile == 0) {
-    request_payload.emplace_back(logAnnounceInterval & 0xff);
-    request_payload.emplace_back(logSyncInterval & 0xff);
-    request_payload.emplace_back(logMinDelayReqInterval & 0xff);
-  } else if (profile == 2 || profile == 3) {
-    request_payload.emplace_back(switch_type & 0xff);
+
+  // Handle different sensor models with different PTP config formats
+  switch (sensor_configuration_->sensor_model) {
+    case SensorModel::HESAI_PANDARAT128: {
+      if (profile != static_cast<int>(PtpProfile::IEEE_802_1AS_AUTO)) {
+        return Status::SENSOR_CONFIG_ERROR;
+      }
+      // AT128 uses status, profile, domain, network, tsn_switch format
+      request_payload.emplace_back(1);  // status: enabled
+      request_payload.emplace_back(profile & 0xff);
+      request_payload.emplace_back(domain & 0xff);
+      request_payload.emplace_back(network & 0xff);
+      request_payload.emplace_back(switch_type & 0xff);  // tsn_switch
+      break;
+    }
+    case SensorModel::HESAI_PANDAR128_E4X: {
+      // OT128 handling - defines PTP profile for automotive as 0x03
+      if (profile != static_cast<int>(PtpProfile::IEEE_802_1AS_AUTO)) {
+        return Status::SENSOR_CONFIG_ERROR;
+      }
+      int ot128_profile = 3;  // OT128 uses profile 3 for automotive
+      request_payload.emplace_back(ot128_profile & 0xff);
+      request_payload.emplace_back(domain & 0xff);
+      request_payload.emplace_back(network & 0xff);
+      request_payload.emplace_back(switch_type & 0xff);  // tsn_switch
+      break;
+    }
+    default: {
+      // Other sensors use status, profile, domain, network, logAnnounceInterval, logSyncInterval,
+      // logMinDelayReqInterval
+      request_payload.emplace_back(profile & 0xff);
+      request_payload.emplace_back(domain & 0xff);
+      request_payload.emplace_back(network & 0xff);
+      if (profile == 0) {
+        request_payload.emplace_back(logAnnounceInterval & 0xff);
+        request_payload.emplace_back(logSyncInterval & 0xff);
+        request_payload.emplace_back(logMinDelayReqInterval & 0xff);
+      } else if (profile == 2 || profile == 3) {
+        request_payload.emplace_back(switch_type & 0xff);
+      }
+      break;
+    }
   }
 
   auto response_or_err = send_receive(g_ptc_command_set_ptp_config, request_payload);
@@ -717,25 +741,31 @@ Status HesaiHwInterface::set_ptp_config(
   return Status::OK;
 }
 
-HesaiPtpConfig HesaiHwInterface::get_ptp_config()
+std::unique_ptr<PtpConfigBase> HesaiHwInterface::get_ptp_config()
 {
   auto response_or_err = send_receive(g_ptc_command_get_ptp_config);
   auto response =
     response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({})));
 
-  if (response.size() < sizeof(HesaiPtpConfig)) {
-    throw std::runtime_error("HesaiPtpConfig has unexpected payload size");
-  } else if (response.size() > sizeof(HesaiPtpConfig)) {
-    logger_->error("HesaiPtpConfig from Sensor has unknown format. Will parse anyway.");
+  switch (sensor_configuration_->sensor_model) {
+    case SensorModel::HESAI_PANDARAT128: {
+      auto ptp_config = check_size_and_parse<HesaiPtpConfig_AT128::Internal>(response);
+      return std::make_unique<HesaiPtpConfig_AT128>(ptp_config);
+    }
+    default:
+    case SensorModel::HESAI_PANDAR40P:
+    case SensorModel::HESAI_PANDAR64:
+    case SensorModel::HESAI_PANDARQT64:
+    case SensorModel::HESAI_PANDARQT128:
+    case SensorModel::HESAI_PANDARXT16:
+    case SensorModel::HESAI_PANDARXT32:
+    case SensorModel::HESAI_PANDARXT32M:
+    case SensorModel::HESAI_PANDAR128_E3X:
+    case SensorModel::HESAI_PANDAR128_E4X: {
+      auto ptp_config = check_size_and_parse<HesaiPtpConfig_XT16_32_40P_OT128::Internal>(response);
+      return std::make_unique<HesaiPtpConfig_XT16_32_40P_OT128>(ptp_config);
+    }
   }
-
-  HesaiPtpConfig hesai_ptp_config;
-  memcpy(&hesai_ptp_config.status, response.data(), 1);
-
-  size_t bytes_to_parse = (hesai_ptp_config.status == 0) ? sizeof(HesaiPtpConfig) : 4;
-  memcpy(&hesai_ptp_config, response.data(), bytes_to_parse);
-
-  return hesai_ptp_config;
 }
 
 Status HesaiHwInterface::set_ptp_lock_offset(uint8_t lock_offset_us)
@@ -1080,88 +1110,74 @@ HesaiStatus HesaiHwInterface::check_and_set_config(
     std::this_thread::sleep_for(wait_time);
   }
 
-  if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) {
+  set_flg = true;
+  auto sensor_sync_angle = static_cast<int>(hesai_config.sync_angle.value() / 100);
+  auto config_sync_angle = sensor_configuration->sync_angle;
+  int sync_flg = 1;
+  if (config_sync_angle != sensor_sync_angle) {
     set_flg = true;
-    auto sensor_sync_angle = static_cast<int>(hesai_config.sync_angle.value() / 100);
-    auto config_sync_angle = sensor_configuration->sync_angle;
-    int sync_flg = 1;
-    if (config_sync_angle != sensor_sync_angle) {
-      set_flg = true;
-    }
-    if (sync_flg && set_flg) {
-      logger_->info("current lidar sync: " + std::to_string(hesai_config.sync));
-      logger_->info("current lidar sync_angle: " + std::to_string(sensor_sync_angle));
-      logger_->info("current configuration sync_angle: " + std::to_string(config_sync_angle));
-      std::thread t(
-        [this, sync_flg, config_sync_angle] { set_sync_angle(sync_flg, config_sync_angle); });
-      t.join();
-      std::this_thread::sleep_for(wait_time);
-    }
-
-    std::thread t([this, sensor_configuration] {
-      if (
-        sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P ||
-        sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 ||
-        sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 ||
-        sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT16 ||
-        sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 ||
-        sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) {
-        logger_->info("Trying to set Clock source to PTP");
-        set_clock_source(g_hesai_lidar_ptp_clock_source);
-      }
-      std::ostringstream tmp_ostringstream;
-      tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile
-                        << ", Domain: " << std::to_string(sensor_configuration->ptp_domain)
-                        << ", Transport: " << sensor_configuration->ptp_transport_type
-                        << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP";
-      logger_->info(tmp_ostringstream.str());
-      set_ptp_config(
-        static_cast<int>(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain,
-        static_cast<int>(sensor_configuration->ptp_transport_type),
-        static_cast<int>(sensor_configuration->ptp_switch_type), g_ptp_log_announce_interval,
-        g_ptp_sync_interval, g_ptp_log_min_delay_interval);
-      logger_->debug("Setting properties done");
-    });
-    logger_->debug("Waiting for thread to finish");
-
+  }
+  if (sync_flg && set_flg) {
+    logger_->info("current lidar sync: " + std::to_string(hesai_config.sync));
+    logger_->info("current lidar sync_angle: " + std::to_string(sensor_sync_angle));
+    logger_->info("current configuration sync_angle: " + std::to_string(config_sync_angle));
+    std::thread t(
+      [this, sync_flg, config_sync_angle] { set_sync_angle(sync_flg, config_sync_angle); });
     t.join();
-    logger_->debug("Thread finished");
-
-    switch (sensor_configuration_->sensor_model) {
-      case SensorModel::HESAI_PANDAR128_E4X:
-      case SensorModel::HESAI_PANDARQT128:
-      case SensorModel::HESAI_PANDARXT16:
-      case SensorModel::HESAI_PANDARXT32:
-      case SensorModel::HESAI_PANDARXT32M: {
-        uint8_t sensor_ptp_lock_threshold = get_ptp_lock_offset();
-        if (sensor_ptp_lock_threshold != sensor_configuration_->ptp_lock_threshold) {
-          NEBULA_LOG_STREAM(
-            logger_->info, "changing sensor PTP lock offset from "
-                             << static_cast<int>(sensor_ptp_lock_threshold) << " to "
-                             << static_cast<int>(sensor_configuration_->ptp_lock_threshold));
-          set_ptp_lock_offset(sensor_configuration_->ptp_lock_threshold);
-        }
-        break;
-      }
-      default:
-        break;
-    }
-
     std::this_thread::sleep_for(wait_time);
-  } else {  // AT128 only supports PTP setup via HTTP
-    logger_->info("Trying to set SyncAngle via HTTP");
-    set_sync_angle_sync_http(1, sensor_configuration->sync_angle);
+  }
+
+  std::thread t([this, sensor_configuration] {
+    if (
+      sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P ||
+      sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 ||
+      sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 ||
+      sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT16 ||
+      sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 ||
+      sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) {
+      logger_->info("Trying to set Clock source to PTP");
+      set_clock_source(g_hesai_lidar_ptp_clock_source);
+    }
     std::ostringstream tmp_ostringstream;
     tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile
-                      << ", Domain: " << sensor_configuration->ptp_domain
-                      << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP";
+                      << ", Domain: " << std::to_string(sensor_configuration->ptp_domain)
+                      << ", Transport: " << sensor_configuration->ptp_transport_type
+                      << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP";
     logger_->info(tmp_ostringstream.str());
-    set_ptp_config_sync_http(
+    set_ptp_config(
       static_cast<int>(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain,
-      static_cast<int>(sensor_configuration->ptp_transport_type), g_ptp_log_announce_interval,
+      static_cast<int>(sensor_configuration->ptp_transport_type),
+      static_cast<int>(sensor_configuration->ptp_switch_type), g_ptp_log_announce_interval,
       g_ptp_sync_interval, g_ptp_log_min_delay_interval);
+    logger_->debug("Setting properties done");
+  });
+  logger_->debug("Waiting for thread to finish");
+
+  t.join();
+  logger_->debug("Thread finished");
+
+  switch (sensor_configuration_->sensor_model) {
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
+    case SensorModel::HESAI_PANDARXT16:
+    case SensorModel::HESAI_PANDARXT32:
+    case SensorModel::HESAI_PANDARXT32M: {
+      uint8_t sensor_ptp_lock_threshold = get_ptp_lock_offset();
+      if (sensor_ptp_lock_threshold != sensor_configuration_->ptp_lock_threshold) {
+        NEBULA_LOG_STREAM(
+          logger_->info, "changing sensor PTP lock offset from "
+                           << static_cast<int>(sensor_ptp_lock_threshold) << " to "
+                           << static_cast<int>(sensor_configuration_->ptp_lock_threshold));
+        set_ptp_lock_offset(sensor_configuration_->ptp_lock_threshold);
+      }
+      break;
+    }
+    default:
+      break;
   }
 
+  std::this_thread::sleep_for(wait_time);
+
   if (
     sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR128_E3X ||
     sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR128_E4X) {