diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
index 893e449db2..79f03a4f95 100644
--- a/ArduCopter/Attitude.cpp
+++ b/ArduCopter/Attitude.cpp
@@ -163,6 +163,11 @@ float Copter::get_non_takeoff_throttle()
 float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
 {
 #if RANGEFINDER_ENABLED == ENABLED
+    if (!copter.rangefinder_alt_ok()) {
+        // if rangefinder is not ok, do not use surface tracking
+        return target_rate;
+    }
+
     static uint32_t last_call_ms = 0;
     float distance_error;
     float velocity_correction;
diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp
index 6b0086cba0..aac5dd2cd8 100644
--- a/ArduCopter/mode_althold.cpp
+++ b/ArduCopter/mode_althold.cpp
@@ -142,10 +142,7 @@ void Copter::ModeAltHold::run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
 
         // adjust climb rate using rangefinder
-        if (copter.rangefinder_alt_ok()) {
-            // if rangefinder is ok, use surface tracking
-            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
-        }
+        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp
index d343578858..73eedf3882 100644
--- a/ArduCopter/mode_circle.cpp
+++ b/ArduCopter/mode_circle.cpp
@@ -86,10 +86,8 @@ void Copter::ModeCircle::run()
     }
 
     // adjust climb rate using rangefinder
-    if (copter.rangefinder_alt_ok()) {
-        // if rangefinder is ok, use surface tracking
-        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
-    }
+    target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
+
     // update altitude target and call position controller
     pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
     pos_control->update_z_controller();
diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp
index 185f883901..76e5615a5c 100644
--- a/ArduCopter/mode_flowhold.cpp
+++ b/ArduCopter/mode_flowhold.cpp
@@ -354,10 +354,7 @@ void Copter::ModeFlowHold::run()
         copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
 
         // adjust climb rate using rangefinder
-        if (copter.rangefinder_alt_ok()) {
-            // if rangefinder is ok, use surface tracking
-            target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);
-        }
+        target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);
 
         // get avoidance adjusted climb rate
         target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);
diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp
index 5e0cdc00ed..b822a3bd2a 100644
--- a/ArduCopter/mode_loiter.cpp
+++ b/ArduCopter/mode_loiter.cpp
@@ -197,10 +197,7 @@ void Copter::ModeLoiter::run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
 
         // adjust climb rate using rangefinder
-        if (copter.rangefinder_alt_ok()) {
-            // if rangefinder is ok, use surface tracking
-            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
-        }
+        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp
index 0a7bb38c67..45bd2b6727 100644
--- a/ArduCopter/mode_poshold.cpp
+++ b/ArduCopter/mode_poshold.cpp
@@ -519,10 +519,7 @@ void Copter::ModePosHold::run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
 
         // adjust climb rate using rangefinder
-        if (copter.rangefinder_alt_ok()) {
-            // if rangefinder is ok, use surface tracking
-            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
-        }
+        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp
index 90b0a250df..fafaa548c0 100644
--- a/ArduCopter/mode_sport.cpp
+++ b/ArduCopter/mode_sport.cpp
@@ -150,10 +150,7 @@ void Copter::ModeSport::run()
         attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
 
         // adjust climb rate using rangefinder
-        if (copter.rangefinder_alt_ok()) {
-            // if rangefinder is ok, use surface tracking
-            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
-        }
+        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
 
         // get avoidance adjusted climb rate
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);