AP_DDS: Navsat sending all available instances

tizianofiorenzani · Mar 2, 2025 · 7794bf7 · 7794bf7
1 parent 43bf730
commit 7794bf7
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 8 deletions.
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -209,7 +209,6 @@ bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i
 
     auto &gps = AP::gps();
     WITH_SEMAPHORE(gps.get_semaphore());
-
     if (!gps.is_healthy(instance)) {
         msg.status.status = -1; // STATUS_NO_FIX
         msg.status.service = 0; // No services supported
@@ -219,13 +218,12 @@ bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i
 
     // No update is needed
     const auto last_fix_time_ms = gps.last_fix_time_ms(instance);
-    if (last_nav_sat_fix_time_ms == last_fix_time_ms) {
+    if (last_nav_sat_fix_time_ms[instance] == last_fix_time_ms) {
         return false;
     } else {
-        last_nav_sat_fix_time_ms = last_fix_time_ms;
+        last_nav_sat_fix_time_ms[instance] = last_fix_time_ms;
     }
 
-
     update_topic(msg.header.stamp);
     static_assert(GPS_MAX_RECEIVERS <= 9, "GPS_MAX_RECEIVERS is greater than 9");
     hal.util->snprintf(msg.header.frame_id, 2, "%u", instance);
@@ -1682,9 +1680,11 @@ void AP_DDS_Client::update()
     }
 #endif // AP_DDS_TIME_PUB_ENABLED
 #if AP_DDS_NAVSATFIX_PUB_ENABLED
-    constexpr uint8_t gps_instance = 0;
-    if (update_topic(nav_sat_fix_topic, gps_instance)) {
-        write_nav_sat_fix_topic();
+    for (uint8_t gps_instance = 0; gps_instance < GPS_MAX_INSTANCES; gps_instance++) {
+        if (update_topic(nav_sat_fix_topic, gps_instance)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Update Instance %u", gps_instance);
+            write_nav_sat_fix_topic();
+        }
     }
 #endif // AP_DDS_NAVSATFIX_PUB_ENABLED
 #if AP_DDS_BATTERY_STATE_PUB_ENABLED

diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h
@@ -15,6 +15,7 @@
 #endif // AP_DDS_TIME_PUB_ENABLED
 #if AP_DDS_NAVSATFIX_PUB_ENABLED
 #include "sensor_msgs/msg/NavSatFix.h"
+#include <AP_GPS/AP_GPS.h>
 #endif // AP_DDS_NAVSATFIX_PUB_ENABLED
 #if AP_DDS_NEEDS_TRANSFORMS
 #include "tf2_msgs/msg/TFMessage.h"
@@ -172,7 +173,7 @@ class AP_DDS_Client
 #if AP_DDS_NAVSATFIX_PUB_ENABLED
     sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
     // The last ms timestamp AP_DDS wrote a NavSatFix message
-    uint64_t last_nav_sat_fix_time_ms;
+    uint64_t last_nav_sat_fix_time_ms[GPS_MAX_INSTANCES];
     //! @brief Serialize the current nav_sat_fix state and publish to the IO stream(s)
     void write_nav_sat_fix_topic();
     bool update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) WARN_IF_UNUSED;