ArduPlane: mode AUTOLAND enhancements

Hwurzburg · Jan 7, 2025 · f1a4cb6 · f1a4cb6
1 parent 89c499f
commit f1a4cb6
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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);