AP_DDS: Set GPS instance ID in the GPS frame ID

Signed-off-by: Ryan Friedman <[email protected]>
ArduPilot · Oct 21, 2024 · 7fd7608 · 7fd7608
1 parent a22e28c
commit 7fd7608
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git 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
@@ -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,