From af87248c1fb976620c5f5c2897c90491a310ec7d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 16 Dec 2024 12:06:33 +1100
Subject: [PATCH] Rover: cycle through rangefinders when sending WATER_DEPTH

similarly to the way we do batteries, do not scale the amount of telemetry sent according to the number of backends we have.
---
 Rover/GCS_Mavlink.cpp | 16 ++++++++++++----
 Rover/GCS_Mavlink.h   | 10 +++++++---
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp
index 3590f8a1ec8d6..fcfa89c1db45e 100644
--- a/Rover/GCS_Mavlink.cpp
+++ b/Rover/GCS_Mavlink.cpp
@@ -186,7 +186,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
         voltage);
 }
 
-void GCS_MAVLINK_Rover::send_water_depth() const
+void GCS_MAVLINK_Rover::send_water_depth()
 {
     if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
         return;
@@ -213,9 +213,15 @@ void GCS_MAVLINK_Rover::send_water_depth() const
     Location loc;
     IGNORE_RETURN(ahrs.get_location(loc));
 
-    for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
-        const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);
-        
+    const auto num_sensors = rangefinder->num_sensors();
+    for (uint8_t i=0; i<num_sensors; i++) {
+        last_WATER_DEPTH_index += 1;
+        if (last_WATER_DEPTH_index >= num_sensors) {
+            last_WATER_DEPTH_index = 0;
+        }
+
+        const AP_RangeFinder_Backend *s = rangefinder->get_backend(last_WATER_DEPTH_index);
+
         if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
             continue;
         }
@@ -241,6 +247,8 @@ void GCS_MAVLINK_Rover::send_water_depth() const
             ahrs.get_yaw(),     // yaw in radians
             s->distance(),    // distance in meters
             temp_C);            // temperature in degC
+
+        break;  // only send one WATER_DEPTH message per loop
     }
 
 }
diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h
index 75d4534b1ba33..ab73ee647d3c6 100644
--- a/Rover/GCS_Mavlink.h
+++ b/Rover/GCS_Mavlink.h
@@ -44,9 +44,6 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK
     // Index starts at 1
     uint8_t send_available_mode(uint8_t index) const override;
 
-    // send WATER_DEPTH - metres and temperature
-    void send_water_depth() const;
-
 private:
 
     void handle_message(const mavlink_message_t &msg) override;
@@ -71,6 +68,13 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK
 
 #if AP_RANGEFINDER_ENABLED
     void send_rangefinder() const override;
+
+    // send WATER_DEPTH - metres and temperature
+    void send_water_depth();
+    // state variable for the last rangefinder we sent a WATER_DEPTH
+    // message for.  We cycle through the rangefinder backends to
+    // limit the amount of telemetry bandwidth we consume.
+    uint8_t last_WATER_DEPTH_index;
 #endif
 
 #if HAL_HIGH_LATENCY2_ENABLED