diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 9ba608c20290d6..9dbc582fd17b30 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -188,9 +188,10 @@ bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i last_nav_sat_fix_time_ms = last_fix_time_ms; } + static_assert(GPS_MAX_RECEIVERS <= 9, "GPS_MAX_RECEIVERS is greater than 9"); update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, WGS_84_FRAME_ID); + hal.util->snprintf(msg.header.frame_id, 1, "%u", instance); msg.status.service = 0; // SERVICE_GPS msg.status.status = -1; // STATUS_NO_FIX diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 92d546dd51290f..cbc87c04457e92 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -96,7 +96,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAREADER_ID}, .topic_rw = Topic_rw::DataWriter, - .topic_name = "rt/ap/navsat/navsat0", + .topic_name = "rt/ap/navsat/navsat", .type_name = "sensor_msgs::msg::dds_::NavSatFix_", .qos = { .durability = UXR_DURABILITY_VOLATILE,