Plane: mode LoiterAltQLand to reuse loiter point if available

ArduPlane/mode_LoiterAltQLand.cpp

@@ -10,17 +10,17 @@ bool ModeLoiterAltQLand::_enter()
         return true;
     }
 
+    const Location previous_wp = plane.next_WP_loc;
+    const bool already_in_a_loiter = plane.reached_loiter_target();
+
     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
+    // at this point plane.next_WP_loc == current_loc which means the instant we change
+    // modes we set that point as the center of the loiter-down circle. If we were already loitering 
+    // then we don't want to change the center of the loiter circle
+    const Location loiter_wp = already_in_a_loiter ? previous_wp : plane.next_WP_loc;
+
+    handle_guided_request(loiter_wp);
 
     switch_qland();