From 5384ab3b3b345c1288d0f98062a394172e827539 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 3 Oct 2023 17:33:54 +1100
Subject: [PATCH] Plane: fixed terrain RTL with rally points

this fixes a bug where if the terrain database cache does not have the
tile for the location of a rally point then RTL to the rally point
with TERRAIN_FOLLOW=1 will not track terrain

The underlying issue is that Location::loc.change_alt_frame() will
return false if the location is not in the terrain memory cache. We
can't just extrapolate as the rally point could be in a totally
different terrain area to the current location. So instead we set it
as terrain_following_pending and fix it as soon as the terrain cache
is filled.

fixes https://github.com/ArduPilot/ardupilot/issues/25157
---
 ArduPlane/Plane.h            |  5 ++++-
 ArduPlane/altitude.cpp       | 14 ++++++++++++--
 ArduPlane/commands_logic.cpp |  6 +++++-
 ArduPlane/mode.cpp           |  4 ++++
 4 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 0cdb615814..ab13b54e5e 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -723,6 +723,9 @@ private:
         // are we trying to follow terrain?
         bool terrain_following;
 
+        // are we waiting to load terrain data to init terrain following
+        bool terrain_following_pending;
+
         // target altitude above terrain in cm, valid if terrain_following
         // is set
         int32_t terrain_alt_cm;
@@ -836,7 +839,7 @@ private:
     void reset_offset_altitude(void);
     void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
     bool above_location_current(const Location &loc);
-    void setup_terrain_target_alt(Location &loc) const;
+    void setup_terrain_target_alt(Location &loc);
     int32_t adjusted_altitude_cm(void);
     int32_t adjusted_relative_altitude_cm(void);
     float mission_alt_offset(void);
diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
index 6387edef12..e65676979b 100644
--- a/ArduPlane/altitude.cpp
+++ b/ArduPlane/altitude.cpp
@@ -191,6 +191,12 @@ void Plane::set_target_altitude_location(const Location &loc)
         target_altitude.amsl_cm += home.alt;
     }
 #if AP_TERRAIN_AVAILABLE
+    if (target_altitude.terrain_following_pending) {
+        /* we didn't get terrain data to init when we started on this
+           target, retry
+        */
+        setup_terrain_target_alt(next_WP_loc);
+    }
     /*
       if this location has the terrain_alt flag set and we know the
       terrain altitude of our current location then treat it as a
@@ -448,12 +454,16 @@ bool Plane::above_location_current(const Location &loc)
   modify a destination to be setup for terrain following if
   TERRAIN_FOLLOW is enabled
  */
-void Plane::setup_terrain_target_alt(Location &loc) const
+void Plane::setup_terrain_target_alt(Location &loc)
 {
 #if AP_TERRAIN_AVAILABLE
     if (terrain_enabled_in_current_mode()) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN);
+        if (!loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
+            target_altitude.terrain_following_pending = true;
+            return;
+        }
     }
+    target_altitude.terrain_following_pending = false;
 #endif
 }
 
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 299f5e004c..5444a5c93d 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -8,7 +8,11 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     // default to non-VTOL loiter
     auto_state.vtol_loiter = false;
 
-        // log when new commands start
+#if AP_TERRAIN_AVAILABLE
+    plane.target_altitude.terrain_following_pending = false;
+#endif
+
+    // log when new commands start
     if (should_log(MASK_LOG_CMD)) {
         logger.Write_Mission_Cmd(mission, cmd);
     }
diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
index 36883707eb..09564caed3 100644
--- a/ArduPlane/mode.cpp
+++ b/ArduPlane/mode.cpp
@@ -91,6 +91,10 @@ bool Mode::enter()
     quadplane.mode_enter();
 #endif
 
+#if AP_TERRAIN_AVAILABLE
+    plane.target_altitude.terrain_following_pending = false;
+#endif
+
     bool enter_result = _enter();
 
     if (enter_result) {