Copter: upward surface tracking never closer than avoidance margin

ArduCopter/surface_tracking.cpp

@@ -39,6 +39,12 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
     }
     valid_for_logging = true;
 
+    // upward facing terrain following never gets closer than avoidance margin
+    if (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) {
+        const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f;
+        target_dist_cm = MAX(target_dist_cm, margin_cm);
+    }
+
     // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
     const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error);
     float velocity_correction = dir * distance_error * copter.g.rangefinder_gain;