From f1a4cb6299257cde48de884913603e180de57e7d Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Wed, 1 Jan 2025 17:25:12 -0600
Subject: [PATCH] ArduPlane: mode AUTOLAND enhancements

---
 ArduPlane/AP_Arming.cpp |  1 +
 ArduPlane/Plane.cpp     | 23 +++++++++++++++++++++++
 ArduPlane/Plane.h       |  6 ++++++
 3 files changed, 30 insertions(+)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index d8ac7d4c09007e..d71b496e667af6 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -390,6 +390,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
 #if MODE_AUTOLAND_ENABLED
     // takeoff direction always cleared on disarm
     plane.takeoff_state.initial_direction.initialized = false;
+    plane.takeoff_direction_check_completed = false;
 #endif
     send_arm_disarm_statustext("Throttle disarmed");
     return true;
diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp
index ed451a403dbf89..4e731017b208c2 100644
--- a/ArduPlane/Plane.cpp
+++ b/ArduPlane/Plane.cpp
@@ -1062,7 +1062,30 @@ void Plane::update_quicktune(void)
     quicktune.update(control_mode->supports_quicktune());
 }
 #endif
+#if MODE_AUTOLAND_ENABLED
+/*
+  In certain pilot controlled modes other than NAV_TAKEOFF or Mode TAKEOFF,takeoff direction is initialized after arm when sufficient altitude and ground speed is obtained, then captured takeoff direction + offset used as landing direction in AUTOLAND
+*/
+void Plane::check_takeoff_direction()
+{
+    if (takeoff_direction_check_completed || takeoff_state.initial_direction.initialized) {
+        return;
+    }
+    //set autoland direction to GPS course over ground
+    if (control_mode->allows_autoland_direction_capture() && (gps.ground_speed() > GPS_GND_CRS_MIN_SPD)) {
+        set_autoland_direction();
+    }      
+}
 
+// Sets autoland direction using ground course + offest parameter
+void Plane::set_autoland_direction()
+{
+    takeoff_state.initial_direction.heading = wrap_360(gps.ground_course() + mode_autoland.landing_dir_off);
+    takeoff_state.initial_direction.initialized = true;
+    takeoff_direction_check_completed = true;
+    gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading));
+}
+#endif
 /*
   constructor for main Plane class
  */
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 1dda8aa741f40e..eaeece728856c5 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1311,6 +1311,12 @@ class Plane : public AP_Vehicle {
 
     // last target alt we passed to tecs
     int32_t tecs_target_alt_cm;
+#if MODE_AUTOLAND_ENABLED
+    // used to set takeoff direction for AUTOLAND mode in some cases
+    void check_takeoff_direction(void);
+    bool takeoff_direction_check_completed;
+    void set_autoland_direction(void);
+#endif
 
 public:
     void failsafe_check(void);