Plane: climb directly to cruise altitude ignoring glide slope

Add the ability for quadplanes to climb immediately to the cruise
altitude as opposed to using the glide slope calculation.

ArduPlane/Plane.h

@@ -1004,6 +1004,7 @@ class Plane : public AP_Vehicle {
     bool start_command_callback(const AP_Mission::Mission_Command &cmd);
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     float get_wp_radius() const;
+    void maybe_do_full_rate_climbout();
 
     bool is_land_command(uint16_t cmd) const;
 

ArduPlane/commands_logic.cpp

@@ -613,6 +613,31 @@ bool Plane::verify_takeoff()
     }
 }
 
+/*
+  When enabled, if the previous waypoint is VTOL Takeoff set the flag to do the full rate climbout.
+  This will give a climbing transition and reach the cruise waypoint immediately as opposed to
+  calculating the glide slope based on proportion to the waypoint.
+ */
+#if HAL_QUADPLANE_ENABLED
+void Plane::maybe_do_full_rate_climbout()
+{
+    AP_Mission::Mission_Command prev_cmd;
+
+    // Check the previous mission item.
+    bool prev_read_success = (plane.mission.read_cmd_from_storage(
+        plane.mission.get_current_nav_index() - 1, prev_cmd));
+
+    // If the previous command is takeoff we will do the full rate climbout.
+    if (prev_read_success && quadplane.is_vtol_takeoff(prev_cmd.id)) {
+        quadplane.set_do_full_rate_climbout(true);
+        return;
+    }
+
+    // Default to off.
+    quadplane.set_do_full_rate_climbout(false);
+}
+#endif
+
 /*
   update navigation for normal mission waypoints. Return true when the
   waypoint is complete
@@ -643,6 +668,10 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
     }
 
+#if HAL_QUADPLANE_ENABLED
+    maybe_do_full_rate_climbout();
+#endif
+
     // see if the user has specified a maximum distance to waypoint
     // If override with p3 - then this is not used as it will overfly badly
     if (g.waypoint_max_radius > 0 &&

ArduPlane/mode.cpp

@@ -201,7 +201,22 @@ void Mode::update_target_altitude()
     } else if (plane.target_altitude.offset_cm != 0 && 
                !plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) {
         // control climb/descent rate
+#if HAL_QUADPLANE_ENABLED
+        // On climbout we will give the ability to do a full rate climb after the VTOL Takeoff. We
+        // will set the glide slope proportion to the destination to achieve this.
+        if (quadplane.get_do_full_rate_climbout_enable() && quadplane.get_do_full_rate_climbout()) {
+            // Set us to the completed portion to get full rate climb. Here, 0 is completed, see
+            // the docstring for set_target_altitude_proportion.
+            plane.set_target_altitude_proportion(plane.next_WP_loc, 0.0f);
+
+            // Always reset the flag.
+            quadplane.set_do_full_rate_climbout(false);
+        } else {
+            plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);
+        }
+#else
         plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);
+#endif
 
         // stay within the range of the start and end locations in altitude
         plane.constrain_target_altitude_location(plane.next_WP_loc, plane.prev_WP_loc);

ArduPlane/quadplane.cpp

@@ -547,6 +547,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("APPROACH_DIST", 39, QuadPlane, approach_distance, 0),
 
+    // @Param: FR_CLMBOUT
+    // @DisplayName: Flag to enable, disable full rate climbout from takeoff
+    // @Description: This flag controls the ability to disable the glide slope and instead climbout at full rate once the VTOL Takeoff waypoint altitude is achieved.
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("FR_CLMBOUT", 40, QuadPlane, enable_full_rate_climbout, 0),
+
     AP_GROUPEND
 };
 

ArduPlane/quadplane.h

@@ -192,6 +192,11 @@ class QuadPlane
     // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe!
     float get_throttle_input() const;
 
+    // Do a full rate climbout after VTOL Takeoff as if we were at the destination waypoint.
+    bool get_do_full_rate_climbout_enable() const { return enable_full_rate_climbout == 1; }
+    bool get_do_full_rate_climbout() const { return _do_full_rate_climbout; }
+    void set_do_full_rate_climbout(bool flag) { _do_full_rate_climbout = flag; }
+
 private:
     AP_AHRS &ahrs;
 
@@ -396,6 +401,11 @@ class QuadPlane
     // limit applied to back pitch to prevent wing producing excessive lift
     AP_Float q_bck_pitch_lim;
 
+    // This flag controls the ability to disable the glide slope and instead climbout at full rate
+    // once the VTOL Takeoff waypoint altitude is achieved.
+    AP_Int8 enable_full_rate_climbout;
+    bool _do_full_rate_climbout;  // set by the code
+
     // which fwd throttle handling method is active
     enum class ActiveFwdThr : uint8_t {
         NONE = 0,

Tools/autotest/ArduPlane_Tests/FullRateClimbout/mission.txt

@@ -0,0 +1,6 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-27.274439	151.290064	343.100006	1
+1	0	3	84	0.000000	0.000000	0.000000	0.000000	-27.274449	151.290081	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-27.264608	151.286680	100.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-27.270692	151.290099	200.000000	1
+4	0	3	85	1.000000	0.000000	0.000000	0.000000	-27.274428	151.290091	30.000000	1

Tools/autotest/quadplane.py

@@ -2164,6 +2164,90 @@ def WindEstimateConsistency(self):
                 self.progress("Wind estimates correlated")
                 break
 
+    def FullRateClimbout(self):
+        '''test the full rate climbout after VTOL Takeoff functions'''
+        # Set from examining the autotest with enable/disable flag.
+        ALTITUDE_CUTOFF_M = 35
+        CRUISE_ALTITUDE_CUTOFF = 120
+        WAIT_AFTER_CRUISE_DELAY_S = 10
+
+        # Mission planning items.
+        CRUISE_WP_NUM = 2
+
+        self.progress("Testing full rate climbout.")
+
+        def run_test(flag):
+            filename = "mission.txt"
+            self.progress("Testing %s with flag %s" % (filename, bool(flag)))
+            self.progress("Setting parameters.")
+
+            self.set_parameter("Q_FR_CLMBOUT", int(flag))
+
+            self.reboot_sitl()
+
+            # Load and fly mission.
+            self.progress(f"Flying mission {filename}")
+            self.load_mission(filename)
+            self.wait_ready_to_arm()
+
+            # Get the starting altitude to convert to agl.
+            starting_alt_m = self.mav.messages['VFR_HUD'].alt
+
+            self.arm_vehicle()
+            self.change_mode('AUTO')
+
+            self.progress("Waiting for Transition done")
+            self.wait_statustext("Transition done", timeout=60)
+
+            # Current altitude.
+            altitude_m = self.mav.messages['VFR_HUD'].alt - starting_alt_m
+
+            is_over_alt_cutoff = (altitude_m > ALTITUDE_CUTOFF_M)
+
+            # Should be under the cutoff when not doing full rate climb.
+            fail_test = False
+            if not flag and is_over_alt_cutoff:
+                notification_str = "too high"
+                sign_str = ">"
+                fail_test = True
+
+            if flag and not is_over_alt_cutoff:
+                notification_str = "too low"
+                sign_str = "<"
+                fail_test = True
+
+            if fail_test:
+                raise NotAchievedException(
+                    "Altitude %s for rate climbout feautre: %s file %s: %.2f %s %.2f."
+                    % (notification_str,
+                       bool(flag),
+                       filename,
+                       altitude_m,
+                       sign_str,
+                       ALTITUDE_CUTOFF_M))
+
+            # Make sure the next waypoint glideslope is not affected.
+            self.wait_waypoint(CRUISE_WP_NUM - 1, CRUISE_WP_NUM, max_dist=100, timeout=100)
+
+            # Wait n seconds for the climb.
+            self.delay_sim_time(WAIT_AFTER_CRUISE_DELAY_S, "waypoint altitude change")
+
+            # Check the altitude is under a limit.
+            cruise_altitude_m = self.mav.messages['VFR_HUD'].alt - starting_alt_m
+
+            # Should be under the cutoff when doing cruise climbs.
+            if not (cruise_altitude_m < CRUISE_ALTITUDE_CUTOFF):
+                raise NotAchievedException(
+                    "Cruise altitude too high for feature: %s, test_file: %s: %.2f exceeds %.2f."
+                    % (bool(flag), filename, cruise_altitude_m, CRUISE_ALTITUDE_CUTOFF))
+
+            self.disarm_vehicle_expect_fail() # don't wait for landing to save test time.
+
+        for flag in [0, 1]:  # disable and enable
+            run_test(flag)
+
+        self.progress("Full rate climbout OK")
+
     def tests(self):
         '''return list of all tests'''
 
@@ -2216,5 +2300,6 @@ def tests(self):
             self.RTL_AUTOLAND_1,  # as in fly-home then go to landing sequence
             self.RTL_AUTOLAND_1_FROM_GUIDED,  # as in fly-home then go to landing sequence
             self.AHRSFlyForwardFlag,
+            self.FullRateClimbout,
         ])
         return ret