From eadcf9b09eee3a7afcdddce95b7cdd3f8619166a Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Wed, 27 Mar 2024 17:56:58 +0000
Subject: [PATCH] Copter: add `get_stopping_point` helper and pass stopping
 point to `jump_to_landing_sequence`

---
 ArduCopter/mode.cpp      | 9 +++++++++
 ArduCopter/mode.h        | 3 +++
 ArduCopter/mode_auto.cpp | 2 +-
 ArduCopter/mode_rtl.cpp  | 5 +----
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index 3c48cebdd6..fe900d307d 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -1062,3 +1062,12 @@ uint16_t Mode::get_pilot_speed_dn()
 {
     return copter.get_pilot_speed_dn();
 }
+
+// Return stopping point as a location with above origin alt frame
+Location Mode::get_stopping_point() const
+{
+    Vector3p stopping_point_NEU;
+    copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy());
+    copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z);
+    return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
+}
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 9d6526b46d..2e33f121c3 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -197,6 +197,9 @@ protected:
     void zero_throttle_and_hold_attitude();
     void make_safe_ground_handling(bool force_throttle_unlimited = false);
 
+    // Return stopping point as a location with above origin alt frame
+    Location get_stopping_point() const;
+
     // functions to control normal landing.  pause_descent is true if vehicle should not descend
     void land_run_horizontal_control();
     void land_run_vertical_control(bool pause_descent = false);
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 12233d9f48..cd3060fa0f 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -216,7 +216,7 @@ bool ModeAuto::allows_weathervaning() const
 // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
 bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
 {
-    if (mission.jump_to_landing_sequence()) {
+    if (mission.jump_to_landing_sequence(get_stopping_point())) {
         mission.set_force_resume(true);
         // if not already in auto switch to auto
         if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) {
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index b8a6cd9bbe..d3250e4265 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/ArduCopter/mode_rtl.cpp
@@ -387,10 +387,7 @@ void ModeRTL::land_run(bool disarm_on_land)
 void ModeRTL::build_path()
 {
     // origin point is our stopping point
-    Vector3p stopping_point;
-    pos_control->get_stopping_point_xy_cm(stopping_point.xy());
-    pos_control->get_stopping_point_z_cm(stopping_point.z);
-    rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN);
+    rtl_path.origin_point = get_stopping_point();
     rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
 
     // compute return target