diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
index b0249ab0c5..1ec6fd815e 100644
--- a/ArduCopter/Attitude.cpp
+++ b/ArduCopter/Attitude.cpp
@@ -124,94 +124,6 @@ float Copter::get_non_takeoff_throttle()
     return MAX(0,motors->get_throttle_hover()/2.0f);
 }
 
-// adjust_climb_rate - hold copter at the desired distance above the
-//      ground; returns climb rate (in cm/s) which should be passed to
-//      the position controller
-float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
-{
-#if RANGEFINDER_ENABLED == ENABLED
-    if (!copter.rangefinder_alt_ok()) {
-        // if rangefinder is not ok, do not use surface tracking
-        return target_rate;
-    }
-
-    const float current_alt = copter.inertial_nav.get_altitude();
-    const float current_alt_target = copter.pos_control->get_alt_target();
-    float distance_error;
-    float velocity_correction;
-
-    uint32_t now = millis();
-
-    valid_for_logging = true;
-
-    // reset target altitude if this controller has just been engaged
-    if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
-        target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt;
-    }
-    last_update_ms = now;
-
-    // adjust rangefinder target alt if motors have not hit their limits
-    if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) {
-        target_alt_cm += target_rate * copter.G_Dt;
-    }
-
-    /*
-      handle rangefinder glitches. When we get a rangefinder reading
-      more than RANGEFINDER_GLITCH_ALT_CM different from the current
-      rangefinder reading then we consider it a glitch and reject
-      until we get RANGEFINDER_GLITCH_NUM_SAMPLES samples in a
-      row. When that happens we reset the target altitude to the new
-      reading
-     */
-    int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm;
-    if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
-        copter.rangefinder_state.glitch_count = MAX(copter.rangefinder_state.glitch_count+1,1);
-    } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
-        copter.rangefinder_state.glitch_count = MIN(copter.rangefinder_state.glitch_count-1,-1);
-    } else {
-        copter.rangefinder_state.glitch_count = 0;
-    }
-    if (abs(copter.rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
-        // shift to the new rangefinder reading
-        target_alt_cm = copter.rangefinder_state.alt_cm;
-        copter.rangefinder_state.glitch_count = 0;
-    }
-    if (copter.rangefinder_state.glitch_count != 0) {
-        // we are currently glitching, just use the target rate
-        return target_rate;
-    }
-
-    // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
-    distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt);
-    velocity_correction = distance_error * copter.g.rangefinder_gain;
-    velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);
-
-    // return combined pilot climb rate + rate to correct rangefinder alt error
-    return (target_rate + velocity_correction);
-#else
-    return target_rate;
-#endif
-}
-
-// get surfacing tracking alt
-// returns true if there is a valid target
-bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
-{
-    // check target has been updated recently
-    if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
-        return false;
-    }
-    _target_alt_cm = target_alt_cm;
-    return true;
-}
-
-// set surface tracking target altitude
-void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
-{
-    target_alt_cm = _target_alt_cm;
-    last_update_ms = AP_HAL::millis();
-}
-
 // get target climb rate reduced to avoid obstacles and altitude fence
 float Copter::get_avoidance_adjusted_climbrate(float target_rate)
 {
diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp
new file mode 100644
index 0000000000..dd7be9466d
--- /dev/null
+++ b/ArduCopter/surface_tracking.cpp
@@ -0,0 +1,90 @@
+#include "Copter.h"
+
+// adjust_climb_rate - hold copter at the desired distance above the
+//      ground; returns climb rate (in cm/s) which should be passed to
+//      the position controller
+float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
+{
+#if RANGEFINDER_ENABLED == ENABLED
+    if (!copter.rangefinder_alt_ok()) {
+        // if rangefinder is not ok, do not use surface tracking
+        return target_rate;
+    }
+
+    const float current_alt = copter.inertial_nav.get_altitude();
+    const float current_alt_target = copter.pos_control->get_alt_target();
+    float distance_error;
+    float velocity_correction;
+
+    uint32_t now = millis();
+
+    valid_for_logging = true;
+
+    // reset target altitude if this controller has just been engaged
+    if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
+        target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt;
+    }
+    last_update_ms = now;
+
+    // adjust rangefinder target alt if motors have not hit their limits
+    if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) {
+        target_alt_cm += target_rate * copter.G_Dt;
+    }
+
+    /*
+      handle rangefinder glitches. When we get a rangefinder reading
+      more than RANGEFINDER_GLITCH_ALT_CM different from the current
+      rangefinder reading then we consider it a glitch and reject
+      until we get RANGEFINDER_GLITCH_NUM_SAMPLES samples in a
+      row. When that happens we reset the target altitude to the new
+      reading
+     */
+    int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm;
+    if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
+        copter.rangefinder_state.glitch_count = MAX(copter.rangefinder_state.glitch_count+1,1);
+    } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
+        copter.rangefinder_state.glitch_count = MIN(copter.rangefinder_state.glitch_count-1,-1);
+    } else {
+        copter.rangefinder_state.glitch_count = 0;
+    }
+    if (abs(copter.rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
+        // shift to the new rangefinder reading
+        target_alt_cm = copter.rangefinder_state.alt_cm;
+        copter.rangefinder_state.glitch_count = 0;
+    }
+    if (copter.rangefinder_state.glitch_count != 0) {
+        // we are currently glitching, just use the target rate
+        return target_rate;
+    }
+
+    // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
+    distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt);
+    velocity_correction = distance_error * copter.g.rangefinder_gain;
+    velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);
+
+    // return combined pilot climb rate + rate to correct rangefinder alt error
+    return (target_rate + velocity_correction);
+#else
+    return target_rate;
+#endif
+}
+
+// get surfacing tracking alt
+// returns true if there is a valid target
+bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
+{
+    // check target has been updated recently
+    if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
+        return false;
+    }
+    _target_alt_cm = target_alt_cm;
+    return true;
+}
+
+// set surface tracking target altitude
+void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
+{
+    target_alt_cm = _target_alt_cm;
+    last_update_ms = AP_HAL::millis();
+}
+