Skip to content

fix(nebula_hw_interfaces_hesai): correct ptp config format for AT128P tcp api #360

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Ayrton2718 wants to merge 3 commits into
base: main
Choose a base branch
from
Open

fix(nebula_hw_interfaces_hesai): correct ptp config format for AT128P tcp api #360

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e1f9890
feat(hesai): refactor PTP config handling for AT128.
Ayrton2718 Sep 25, 2025
e058d7e
ci(pre-commit): autofix
pre-commit-ci[bot] Sep 25, 2025
abbb152
Merge branch 'beta/v0.3.2.2' into fix/at128-ptp-config
Ayrton2718 May 12, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
109 changes: 83 additions & 26 deletions ...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion ...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
222 changes: 119 additions & 103 deletions nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -691,51 +691,81 @@ 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);
response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({})));
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)
Expand Down Expand Up @@ -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) {
Expand Down
Loading