diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index e7c7e0272e..62fb373700 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -418,10 +418,33 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
 
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
+    if (!sonar.enabled()) {
+        // no sonar to report
+        return;
+    }
+
+    /*
+      report smaller distance of two sonars if more than one enabled
+     */
+    float distance_cm, voltage;
+    if (!sonar2.enabled()) {
+        distance_cm = sonar.distance_cm();
+        voltage = sonar.voltage();
+    } else {
+        float dist1 = sonar.distance_cm();
+        float dist2 = sonar2.distance_cm();
+        if (dist1 <= dist2) {
+            distance_cm = dist1;
+            voltage = sonar.voltage();
+        } else {
+            distance_cm = dist2;
+            voltage = sonar2.voltage();
+        }
+    }
     mavlink_msg_rangefinder_send(
         chan,
-        sonar.distance_cm() * 0.01,
-        sonar.voltage());
+        distance_cm * 0.01f,
+        voltage);
 }
 
 static void NOINLINE send_current_waypoint(mavlink_channel_t chan)