VTOL: Mode 25 to land at the guided location

magicrub · Oct 12, 2023 · a07330a · a07330a
1 parent 659db36
commit a07330a
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 10 deletions.
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -362,6 +362,8 @@ class ModeLoiterAltQLand : public ModeLoiter
     // handle a guided target request from GCS
     bool handle_guided_request(Location target_loc) override;
 
+    Location loiter_wp;
+
 protected:
     bool _enter() override;
 

diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp
@@ -10,17 +10,12 @@ bool ModeLoiterAltQLand::_enter()
         return true;
     }
 
+    const Location previous_wp = plane.next_WP_loc;
+
     ModeLoiter::_enter();
 
-#if AP_TERRAIN_AVAILABLE
-    if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
-        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
-    } else {
-        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
-    }
-#else
-    plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
-#endif
+    const Location target_loc = (plane.previous_mode->is_guided_mode() && !plane.any_failsafe_triggered()) ? previous_wp : plane.next_WP_loc;
+    handle_guided_request(target_loc);
 
     switch_qland();
 
@@ -38,7 +33,13 @@ void ModeLoiterAltQLand::switch_qland()
 {
     ftype dist;
     if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {
-        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
+        if (plane.previous_mode->is_guided_mode() && !plane.any_failsafe_triggered()) {
+            plane.set_mode(plane.mode_qrtl, ModeReason::LOITER_ALT_REACHED_QLAND);
+            plane.set_next_WP(loiter_wp);
+        } else
+        {
+            plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
+        }
     }
 }
 
@@ -57,6 +58,8 @@ bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)
 
     plane.set_guided_WP(target_loc);
 
+    loiter_wp = target_loc;
+
     return true;
 }