diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index d033692efc..6b2c61ee6a 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -327,29 +327,37 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
 
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
-    if (!sonar.healthy()) {
+    if (!sonar.has_data(0) && !sonar.has_data(1)) {
         // no sonar to report
         return;
     }
 
+    float distance_cm = 0.0f;
+    float voltage = 0.0f;
+
     /*
-      report smaller distance of two sonars if more than one enabled
+      report smaller distance of two sonars
      */
-    float distance_cm, voltage;
-    if (!sonar.healthy(1)) {
-        distance_cm = sonar.distance_cm(0);
-        voltage = sonar.voltage_mv(0) * 0.001f;
-    } else {
-        float dist1 = sonar.distance_cm(0);
-        float dist2 = sonar.distance_cm(1);
-        if (dist1 <= dist2) {
-            distance_cm = dist1;
-            voltage = sonar.voltage_mv(0) * 0.001f;
+    if (sonar.has_data(0) && sonar.has_data(1)) {
+        if (sonar.distance_cm(0) <= sonar.distance_cm(1)) {
+            distance_cm = sonar.distance_cm(0);
+            voltage = sonar.voltage_mv(0);
         } else {
-            distance_cm = dist2;
+            distance_cm = sonar.distance_cm(1);
+            voltage = sonar.voltage_mv(1);
+        }
+    } else {
+        // only sonar 0 or sonar 1 has data
+        if (sonar.has_data(0)) {
+            distance_cm = sonar.distance_cm(0);
+            voltage = sonar.voltage_mv(0) * 0.001f;
+        }
+        if (sonar.has_data(1)) {
+            distance_cm = sonar.distance_cm(1);
             voltage = sonar.voltage_mv(1) * 0.001f;
         }
     }
+
     mavlink_msg_rangefinder_send(
         chan,
         distance_cm * 0.01f,
diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde
index 00631ba081..2bd4202862 100644
--- a/APMrover2/sensors.pde
+++ b/APMrover2/sensors.pde
@@ -33,12 +33,12 @@ static void read_sonars(void)
 {
     sonar.update();
 
-    if (!sonar.healthy()) {
+    if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
         // this makes it possible to disable sonar at runtime
         return;
     }
 
-    if (sonar.healthy(1)) {
+    if (sonar.has_data(1)) {
         // we have two sonars
         obstacle.sonar1_distance_cm = sonar.distance_cm(0);
         obstacle.sonar2_distance_cm = sonar.distance_cm(1);
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index 284e722224..3e84572908 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -459,7 +459,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
     delay(20);
     sonar.update();
 
-    if (!sonar.healthy()) {
+    if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
         cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
     }