From 5e0b13bb680beeb0c2d0f1399f2f7363663faacd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 16 Aug 2024 13:54:58 +1000
Subject: [PATCH] Plane: allow QRTL to terrain follow with approach disabled

when Q_OPTIONS has approach disabled this allows QRTL to track
terrain, allowing for safer approach over hilly terrain
---
 ArduPlane/quadplane.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 0bb00459ee6c8..54c0338c16163 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2793,6 +2793,13 @@ void QuadPlane::vtol_position_controller(void)
             loc2.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
             float target_z = loc2.alt;
             float zero = 0;
+#if AP_TERRAIN_AVAILABLE
+            float terrain_altitude_offset;
+            if (plane.terrain_enabled_in_current_mode() && plane.terrain.height_terrain_difference_home(terrain_altitude_offset, true)) {
+                // Climb if current terrain is above home, target_altitude_cm is reltive to home
+                target_z += MAX(terrain_altitude_offset*100,0);
+            }
+#endif
             pos_control->input_pos_vel_accel_z(target_z, zero, 0);
         } else {
             set_climb_rate_cms(0);