Plane: Quadplane: QRTL climb to QRTL alt first if in Q mode

ArduPlane/mode.h

@@ -599,6 +599,13 @@ public:
 protected:
 
     bool _enter() override;
+
+private:
+
+    enum class SubMode {
+        climb,
+        RTL,
+    } submode;
 };
 
 class ModeQAcro : public Mode

ArduPlane/mode_qrtl.cpp

@@ -11,28 +11,52 @@ bool ModeQRTL::_enter()
        plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
        return true;
     }
-
-    // use do_RTL() to setup next_WP_loc
-    plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL);
+    submode = SubMode::RTL;
     plane.prev_WP_loc = plane.current_loc;
-    pos_control->set_accel_desired_xy_cmss(Vector2f());
-    pos_control->init_xy_controller();
-    quadplane.poscontrol_init_approach();
-    float dist = plane.next_WP_loc.get_distance(plane.current_loc);
+
+    // clear QPOS state
+    quadplane.poscontrol.set_state(QuadPlane::QPOS_NONE);
+
+    const int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
+    if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
+        // VTOL motors are active, either in VTOL flight or assisted flight
+        Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
+
+        const float dist = plane.current_loc.get_distance(destination);
         const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
-    if (dist < 1.5*radius &&
-        quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
-        // we're close to destination and already running VTOL motors, don't transition
+
+        const float dist_to_climb = quadplane.qrtl_alt - plane.relative_ground_altitude(plane.g.rangefinder_landing);
+        if (dist < 1.5*radius) {
+            // we're close to destination and already running VTOL motors, don't transition and don't climb
             gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
             poscontrol.set_state(QuadPlane::QPOS_POSITION1);
+
+        } else if (is_positive(dist_to_climb)) {
+            // climb before returning, only next waypoint altitude is used
+            submode = SubMode::climb;
+            plane.next_WP_loc = plane.current_loc;
+#if AP_TERRAIN_AVAILABLE
+            if (plane.terrain_enabled_in_mode(mode_number())) {
+                plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt * 100UL, Location::AltFrame::ABOVE_TERRAIN);
+                return true;
+            }
+#endif
+            plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame());
+            return true;
+        }
     }
+
+    // use do_RTL() to setup next_WP_loc
+    plane.do_RTL(RTL_alt_abs_cm);
+    quadplane.poscontrol_init_approach();
+
     int32_t from_alt;
     int32_t to_alt;
     if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
         poscontrol.slow_descent = from_alt > to_alt;
         return true;
     }
-    // defualt back to old method
+    // default back to old method
     poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
     return true;
 }
@@ -47,6 +71,46 @@ void ModeQRTL::update()
  */
 void ModeQRTL::run()
 {
+    switch (submode) {
+        case SubMode::climb: {
+            // request zero velocity
+            Vector2f vel, accel;
+            pos_control->input_vel_accel_xy(vel, accel);
+            quadplane.run_xy_controller();
+
+            // nav roll and pitch are controller by position controller
+            plane.nav_roll_cd = pos_control->get_roll_cd();
+            plane.nav_pitch_cd = pos_control->get_pitch_cd();
+            if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
+                pos_control->set_externally_limited_xy();
+            }
+            // weathervane with no pilot input
+            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
+                                                                          plane.nav_pitch_cd,
+                                                                          quadplane.get_weathervane_yaw_rate_cds());
+
+            // climb at full WP nav speed
+            quadplane.set_climb_rate_cms(quadplane.wp_nav->get_default_speed_up(), false);
+            quadplane.run_z_controller();
+
+            ftype alt_diff;
+            if (!plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) {
+                // climb finshed or cant get alt diff, head home
+                submode = SubMode::RTL;
+                plane.prev_WP_loc = plane.current_loc;
+                plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL);
+                quadplane.poscontrol_init_approach();
+                if (plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff)) {
+                    poscontrol.slow_descent = is_positive(alt_diff);
+                } else {
+                    // default back to old method
+                    poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
+                }
+            }
+            break;
+        }
+
+        case SubMode::RTL: {
             quadplane.vtol_position_controller();
             if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) {
                 // change target altitude to home alt
@@ -62,6 +126,9 @@ void ModeQRTL::run()
                 quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) {
                 plane.stabilize_stick_mixing_fbw();
             }
+            break;
+        }
+    }
 }
 
 /*
@@ -72,7 +139,7 @@ bool ModeQRTL::update_target_altitude()
     /*
       update height target in approach
      */
-    if (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH) {
+    if ((submode != SubMode::RTL) || (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH)) {
         return false;
     }
 
@@ -99,7 +166,7 @@ bool ModeQRTL::update_target_altitude()
 // only nudge during approach
 bool ModeQRTL::allows_throttle_nudging() const
 {
-    return plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH;
+    return (submode == SubMode::RTL) && (plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH);
 }
 
 #endif

ArduPlane/quadplane.cpp

@@ -2187,6 +2187,7 @@ void QuadPlane::poscontrol_init_approach(void)
     }
     poscontrol.pilot_correction_done = false;
     poscontrol.xy_correction.zero();
+    poscontrol.slow_descent = false;
 }
 
 /*
@@ -3473,13 +3474,13 @@ void QuadPlane::guided_start(void)
     setup_target_position();
     int32_t from_alt;
     int32_t to_alt;
+    poscontrol_init_approach();
     if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
         poscontrol.slow_descent = from_alt > to_alt;
     } else {
         // default back to old method
         poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
     }
-    poscontrol_init_approach();
 }
 
 /*