From b2fe3e71e670ef9a3de8f1c7b2f46739ee716195 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 31 Jul 2020 19:12:33 +1000
Subject: [PATCH] Plane: fixed LOITER_TO_ALT with terrain target

many thanks to Pompecukor for finding this!

Fixes #14951
---
 ArduPlane/navigation.cpp | 33 +++++++++++++++++++++++++++++++--
 1 file changed, 31 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index a1366dadb6..c81cc7d004 100644
--- a/ArduPlane/navigation.cpp
+++ b/ArduPlane/navigation.cpp
@@ -36,8 +36,9 @@ void Plane::loiter_angle_update(void)
 
     const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
     int32_t loiter_delta_cd;
+    const bool reached_target = reached_loiter_target();
 
-    if (loiter.sum_cd == 0 && !reached_loiter_target()) {
+    if (loiter.sum_cd == 0 && !reached_target) {
         // we don't start summing until we are doing the real loiter
         loiter_delta_cd = 0;
     } else if (loiter.sum_cd == 0) {
@@ -53,7 +54,35 @@ void Plane::loiter_angle_update(void)
     loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
     loiter.sum_cd += loiter_delta_cd * loiter.direction;
 
-    if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
+    bool reached_target_alt = false;
+
+    if (reached_target) {
+        // once we reach the position target we start checking the
+        // altitude target
+        bool terrain_status_ok = false;
+#if AP_TERRAIN_AVAILABLE
+        /*
+          if doing terrain following then we check against terrain
+          target, fetch the terrain information
+        */
+        float altitude_agl = 0;
+        if (target_altitude.terrain_following) {
+            if (terrain.status() == AP_Terrain::TerrainStatusOK &&
+                terrain.height_above_terrain(altitude_agl, true)) {
+                terrain_status_ok = true;
+            }
+        }
+        if (terrain_status_ok &&
+            fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) {
+            reached_target_alt = true;
+        } else
+#endif
+        if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) {
+            reached_target_alt = true;
+        }
+    }
+
+    if (reached_target_alt) {
         loiter.reached_target_alt = true;
         loiter.unable_to_acheive_target_alt = false;
         loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;