From 0a7461cbb95d4e6e75649baae9fcae0dd7a7ecdc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 1 Nov 2024 13:55:20 +0900
Subject: [PATCH] Copter: RTL build-path uses find-nearest-rally-with-dijkstras

---
 ArduCopter/mode.h       |  4 +--
 ArduCopter/mode_rtl.cpp | 59 +++++++++++++++++++++++++++++------------
 2 files changed, 44 insertions(+), 19 deletions(-)

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index fdfd8846767b79..86de6f3feb136d 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -1327,7 +1327,7 @@ class ModeRTL : public Mode {
 
     // RTL states
     enum class SubMode : uint8_t {
-        STARTING,
+        BUILD_PATH,
         INITIAL_CLIMB,
         RETURN_HOME,
         LOITER_AT_HOME,
@@ -1375,7 +1375,7 @@ class ModeRTL : public Mode {
     void loiterathome_start();
     void loiterathome_run();
     void build_path();
-    void compute_return_target();
+    bool compute_return_target();
 
     SubMode _state = SubMode::INITIAL_CLIMB;  // records state of rtl (initial climb, returning home, etc)
     bool _state_complete = false; // set to true if the current state is completed
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index d62a9cbe4b9ae7..af1127e3c535f5 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/ArduCopter/mode_rtl.cpp
@@ -19,9 +19,14 @@ bool ModeRTL::init(bool ignore_checks)
     }
     // initialise waypoint and spline controller
     wp_nav->wp_and_spline_init(g.rtl_speed_cms);
-    _state = SubMode::STARTING;
-    _state_complete = true; // see run() method below
+
+    // initialise state
+    _state = SubMode::BUILD_PATH;
+    _state_complete = false;
+
+    // allow terrain following unless in terrain failsafe
     terrain_following_allowed = !copter.failsafe.terrain;
+
     // reset flag indicating if pilot has applied roll or pitch inputs during landing
     copter.ap.land_repo_active = false;
 
@@ -41,8 +46,12 @@ void ModeRTL::restart_without_terrain()
 {
     AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
     terrain_following_allowed = false;
-    _state = SubMode::STARTING;
-    _state_complete = true;
+
+    // initialise state
+    _state = SubMode::BUILD_PATH;
+    _state_complete = false;
+
+    // notify user
     gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
 }
 
@@ -68,8 +77,7 @@ void ModeRTL::run(bool disarm_on_land)
     // check if we need to move to next state
     if (_state_complete) {
         switch (_state) {
-        case SubMode::STARTING:
-            build_path();
+        case SubMode::BUILD_PATH:
             climb_start();
             break;
         case SubMode::INITIAL_CLIMB:
@@ -97,10 +105,9 @@ void ModeRTL::run(bool disarm_on_land)
     // call the correct run function
     switch (_state) {
 
-    case SubMode::STARTING:
-        // should not be reached:
-        _state = SubMode::INITIAL_CLIMB;
-        FALLTHROUGH;
+    case SubMode::BUILD_PATH:
+        build_path();
+        break;
 
     case SubMode::INITIAL_CLIMB:
         climb_return_run();
@@ -392,8 +399,15 @@ void ModeRTL::land_run(bool disarm_on_land)
     land_run_normal_or_precland();
 }
 
+// build RTL return path
+// _state_complete set to true on success
 void ModeRTL::build_path()
 {
+    // compute return target
+    if (!compute_return_target()) {
+        return;
+    }
+
     // origin point is our stopping point
     Vector3p stopping_point;
     pos_control->get_stopping_point_xy_cm(stopping_point.xy());
@@ -401,9 +415,6 @@ void ModeRTL::build_path()
     rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN);
     rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
 
-    // compute return target
-    compute_return_target();
-
     // climb target is above our origin point at the return altitude
     rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
 
@@ -412,15 +423,27 @@ void ModeRTL::build_path()
 
     // set land flag
     rtl_path.land = g.rtl_alt_final <= 0;
+
+    // state is complete
+    _state_complete = true;
 }
 
 // compute the return target - home or rally point
-//   return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
-void ModeRTL::compute_return_target()
+//   rtl_path.return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
+//   returns true on success and fills in rtl_path structure
+bool ModeRTL::compute_return_target()
 {
     // set return target to nearest rally point or home position (Note: alt is absolute)
 #if HAL_RALLY_ENABLED
-    rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt);
+    if (!copter.rally.find_nearest_rally_or_home_with_dijkstras(copter.current_loc, ahrs.get_home().alt, rtl_path.return_target)) {
+        return false;
+    }
+    if (!rtl_path.return_target.initialised()) {
+        // sanity check return target, this should never happen
+        rtl_path.return_target = ahrs.get_home();
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    }
+    rtl_path.return_target.change_alt_frame(Location::AltFrame::ABSOLUTE);
 #else
     rtl_path.return_target = ahrs.get_home();
 #endif
@@ -525,13 +548,15 @@ void ModeRTL::compute_return_target()
 
     // ensure we do not descend
     rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
+
+    return true;
 }
 
 bool ModeRTL::get_wp(Location& destination) const
 {
     // provide target in states which use wp_nav
     switch (_state) {
-    case SubMode::STARTING:
+    case SubMode::BUILD_PATH:
     case SubMode::INITIAL_CLIMB:
     case SubMode::RETURN_HOME:
     case SubMode::LOITER_AT_HOME: