diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 7b665eb4d3d0949967719661722c7b06003527dc..cb795c3fd9bb868befe27ee0d994988e0d9cd663 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -581,6 +581,10 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 #if CONFIG_SONAR == ENABLED
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
+    // exit immediately if sonar is disabled
+    if (!g.sonar_enabled) {
+        return;
+    }
     mavlink_msg_rangefinder_send(chan, sonar_alt * 0.01f, 0);
 }
 #endif