From a5d0f55bdc97e8efa4ab4faad79f8907f1c72460 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 20 Sep 2019 11:07:53 +0900 Subject: [PATCH] Copter: upward surface tracking never closer than avoidance margin --- ArduCopter/surface_tracking.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index b248b515c2..b31c18fdcf 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/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;