diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp
index 61ace7ff21..8f5d379479 100644
--- a/ArduCopter/control_althold.cpp
+++ b/ArduCopter/control_althold.cpp
@@ -74,7 +74,7 @@ void Copter::althold_run()
         // body-frame rate controller is run directly from 100hz loop
 
         // call throttle controller
-        if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
+        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
             // if sonar is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp
index 9b7b45e9f0..2e6670678f 100644
--- a/ArduCopter/control_circle.cpp
+++ b/ArduCopter/control_circle.cpp
@@ -73,7 +73,7 @@ void Copter::circle_run()
     }
 
     // run altitude controller
-    if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
+    if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
         // if sonar is ok, use surface tracking
         target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
     }
diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp
index 6001e7f396..a97651f42d 100644
--- a/ArduCopter/control_loiter.cpp
+++ b/ArduCopter/control_loiter.cpp
@@ -98,7 +98,7 @@ void Copter::loiter_run()
         // body-frame rate controller is run directly from 100hz loop
 
         // run altitude controller
-        if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
+        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
             // if sonar is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp
index 66946ca081..b8c02b4f24 100644
--- a/ArduCopter/control_poshold.cpp
+++ b/ArduCopter/control_poshold.cpp
@@ -505,7 +505,7 @@ void Copter::poshold_run()
         attitude_control.angle_ef_roll_pitch_rate_ef_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
 
         // throttle control
-        if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
+        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
             // if sonar is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp
index c0fabd4345..54d993f8ae 100644
--- a/ArduCopter/control_sport.cpp
+++ b/ArduCopter/control_sport.cpp
@@ -95,7 +95,7 @@ void Copter::sport_run()
         attitude_control.rate_ef_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
 
         // call throttle controller
-        if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
+        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
             // if sonar is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp
index 4713518814..d6ed0cc0f2 100644
--- a/ArduCopter/sensors.cpp
+++ b/ArduCopter/sensors.cpp
@@ -40,7 +40,7 @@ int16_t Copter::read_sonar(void)
     sonar.update();
 
     // exit immediately if sonar is disabled
-    if (!sonar_enabled || (sonar.status() != RangeFinder::RangeFinder_Good)) {
+    if (sonar.status() != RangeFinder::RangeFinder_Good) {
         sonar_alt_health = 0;
         return 0;
     }