Plane: implement slow descent in QRTL approach

this approaches in QRTL at ALT_HOLD_RTL alt, then descends to
Q_RTL_ALT close to the destination

ArduPlane/Plane.h

@@ -814,7 +814,7 @@ private:
     void update_load_factor(void);
     void adjust_altitude_target();
     void setup_glide_slope(void);
-    int32_t get_RTL_altitude() const;
+    int32_t get_RTL_altitude_cm() const;
     float relative_ground_altitude(bool use_rangefinder_if_available);
     void set_target_altitude_current(void);
     void set_target_altitude_current_adjusted(void);

ArduPlane/altitude.cpp

@@ -52,7 +52,9 @@ void Plane::adjust_altitude_target()
         set_target_altitude_location(temp);
     } else 
 #endif // OFFBOARD_GUIDED == ENABLED
-      if (landing.is_flaring()) {
+      if (control_mode->update_target_altitude()) {
+          // handled in mode specific code
+    } else if (landing.is_flaring()) {
         // during a landing flare, use TECS_LAND_SINK as a target sink
         // rate, and ignores the target altitude
         set_target_altitude_location(next_WP_loc);
@@ -135,9 +137,9 @@ void Plane::setup_glide_slope(void)
 }
 
 /*
-  return RTL altitude as AMSL altitude
+  return RTL altitude as AMSL cm
  */
-int32_t Plane::get_RTL_altitude() const
+int32_t Plane::get_RTL_altitude_cm() const
 {
     if (g.RTL_altitude_cm < 0) {
         return current_loc.alt;

ArduPlane/commands_logic.cpp

@@ -302,12 +302,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 //  Nav (Must) commands
 /********************************************************************************/
 
-void Plane::do_RTL(int32_t rtl_altitude)
+void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm)
 {
     auto_state.next_wp_crosstrack = false;
     auto_state.crosstrack = false;
     prev_WP_loc = current_loc;
-    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude);
+    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm);
     setup_terrain_target_alt(next_WP_loc);
     set_target_altitude_location(next_WP_loc);
 

ArduPlane/fence.cpp

@@ -71,7 +71,7 @@ void Plane::fence_check()
             g.auto_trim.set(saved_auto_trim);
 
             if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
-                guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
+                guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm());
             } else {
                 //return to fence return point, not a rally point
                 guided_WP_loc = {};

ArduPlane/mode.h

@@ -101,6 +101,9 @@ public:
     // whether control input is ignored with STICK_MIXING=0
     virtual bool does_auto_throttle() const { return false; }
 
+    // method for mode specific target altitude profiles
+    virtual bool update_target_altitude() { return false; }
+    
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -526,6 +529,8 @@ public:
 
     bool does_auto_throttle() const override { return true; }
 
+    bool update_target_altitude() override;
+
 protected:
 
     bool _enter() override;

ArduPlane/mode_qrtl.cpp

@@ -11,3 +11,35 @@ void ModeQRTL::update()
     plane.mode_qstabilize.update();
 }
 
+/*
+  update target altitude for QRTL profile
+ */
+bool ModeQRTL::update_target_altitude()
+{
+    /*
+      update height target in approach
+     */
+    if (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH) {
+        return false;
+    }
+
+    /*
+      initially approach at RTL_ALT_CM, then drop down to QRTL_ALT based on maximum sink rate from TECS,
+      giving time to lose speed before we transition
+     */
+    const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
+    const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt);
+    const float sink_time = rtl_alt_delta / MAX(0.6*plane.SpdHgt_Controller->get_max_sinkrate(), 1);
+    const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time;
+    const float dist = plane.auto_state.wp_distance;
+    const float rad_min = 2*radius;
+    const float rad_max = 20*radius;
+    float alt = linear_interpolate(0, rtl_alt_delta,
+                                   dist,
+                                   rad_min, MAX(rad_min, MIN(rad_max, rad_min+sink_dist)));
+    Location loc = plane.next_WP_loc;
+    loc.alt += alt*100;
+    plane.set_target_altitude_location(loc);
+    return true;
+}
+

ArduPlane/mode_rtl.cpp

@@ -4,7 +4,7 @@
 bool ModeRTL::_enter()
 {
     plane.prev_WP_loc = plane.current_loc;
-    plane.do_RTL(plane.get_RTL_altitude());
+    plane.do_RTL(plane.get_RTL_altitude_cm());
     plane.rtl.done_climb = false;
     plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;