Plane: smooth-out the end of takeoff pitch by reducing takeoff pitch min via TKOFF_PLIM_SEC

@Description: This parameter reduces the pitch minimum limit of an auto-takeoff just a few seconds before it reaches the target altitude. This reduces overshoot by allowing the flight controller to start leveling off a few seconds before reaching the target height. When set to zero, the mission pitch min is enforced all the way to and through the target altitude, otherwise the pitch min slowly reduces to zero in the final segment. This is the pitch_min, not the demand. The flight controller should still be commanding to gain altitude to finish the takeoff but with this param it is not forcing it higher than it wants to be. (+1 squashed commits)

ArduPlane/ArduPlane.cpp

@@ -893,7 +893,7 @@ void Plane::update_alt()
                                                  flight_stage,
                                                  is_doing_auto_land,
                                                  distance_beyond_land_wp,
-                                                 auto_state.takeoff_pitch_cd,
+                                                 get_takeoff_pitch_min_cd(),
                                                  throttle_nudge,
                                                  tecs_hgt_afe(),
                                                  aerodynamic_load_factor);

ArduPlane/Parameters.cpp

@@ -199,6 +199,15 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: User
     GSCALAR(takeoff_throttle_slewrate, "TKOFF_THR_SLEW",  0),
 
+    // @Param: TKOFF_PLIM_SEC
+    // @DisplayName: Takeoff pitch limit reduction
+    // @Description: This parameter reduces the pitch minimum limit of an auto-takeoff just a few seconds before it reaches the target altitude. This reduces overshoot by allowing the flight controller to start leveling off a few seconds before reaching the target height. When set to zero, the mission pitch min is enforced all the way to and through the target altitude, otherwise the pitch min slowly reduces to zero in the final segment. This is the pitch_min, not the demand. The flight controller should still be commanding to gain altitude to finish the takeoff but with this param it is not forcing it higher than it wants to be.
+    // @Units: seconds
+    // @Range: 0 10
+    // @Increment: 0.5
+    // @User: Advanced
+    GSCALAR(takeoff_pitch_limit_reduction_sec, "TKOFF_PLIM_SEC",  2),
+
     // @Param: LAND_THR_SLEW
     // @DisplayName: Landing throttle slew rate
     // @Description: This parameter sets the slew rate for the throttle during auto landing. When this is zero the THR_SLEWRATE parameter is used during landing. The value is a percentage throttle change per second, so a value of 20 means to advance the throttle over 5 seconds on landing. Values below 50 are not recommended as it may cause a stall when airspeed is low and you can not throttle up fast enough.

ArduPlane/Parameters.h

@@ -153,6 +153,7 @@ public:
         k_param_fence_autoenable,
         k_param_fence_ret_rally,
         k_param_q_attitude_control,
+        k_param_takeoff_pitch_limit_reduction_sec,
 
         // 110: Telemetry control
         //
@@ -495,6 +496,7 @@ public:
     AP_Float takeoff_tdrag_speed1;
     AP_Float takeoff_rotate_speed;
     AP_Int8 takeoff_throttle_slewrate;
+    AP_Float takeoff_pitch_limit_reduction_sec;
     AP_Int8 land_throttle_slewrate;
     AP_Int8 level_roll_limit;
     AP_Int8 flapin_channel;

ArduPlane/Plane.h

@@ -465,6 +465,9 @@ private:
         // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
         int16_t takeoff_pitch_cd;
 
+        // Begin leveling out the enforced takeoff pitch angle min at this height to reduce/eliminate overshoot
+        int32_t height_below_takeoff_to_level_off_cm;
+
         // the highest airspeed we have reached since entering AUTO. Used
         // to control ground takeoff
         float highest_airspeed;
@@ -946,6 +949,7 @@ private:
     void takeoff_calc_roll(void);
     void takeoff_calc_pitch(void);
     int8_t takeoff_tail_hold(void);
+    int16_t get_takeoff_pitch_min_cd(void);
     void print_hit_enter();
     void ahrs_update();
     void update_speed_height(void);

ArduPlane/commands_logic.cpp

@@ -361,6 +361,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
     next_WP_loc.lng = home.lng + 10;
     auto_state.takeoff_speed_time_ms = 0;
     auto_state.takeoff_complete = false;                            // set flag to use gps ground course during TO.  IMU will be doing yaw drift correction
+    auto_state.height_below_takeoff_to_level_off_cm = 0;
     // Flag also used to override "on the ground" throttle disable
 
     // zero locked course

ArduPlane/takeoff.cpp

@@ -133,9 +133,10 @@ void Plane::takeoff_calc_pitch(void)
     }
 
     if (ahrs.airspeed_sensor_enabled()) {
+        int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd();
         calc_nav_pitch();
-        if (nav_pitch_cd < auto_state.takeoff_pitch_cd) {
-            nav_pitch_cd = auto_state.takeoff_pitch_cd;
+        if (nav_pitch_cd < takeoff_pitch_min_cd) {
+            nav_pitch_cd = takeoff_pitch_min_cd;
         }
     } else {
         nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd;
@@ -143,6 +144,39 @@ void Plane::takeoff_calc_pitch(void)
     }
 }
 
+/*
+ * get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
+ */
+int16_t Plane::get_takeoff_pitch_min_cd(void)
+{
+    if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) {
+        return auto_state.takeoff_pitch_cd;
+    }
+
+    int32_t relative_alt_cm = adjusted_relative_altitude_cm();
+    int32_t remaining_height_to_target_cm = (auto_state.takeoff_altitude_rel_cm - relative_alt_cm);
+
+    // seconds to target alt method
+    if (g.takeoff_pitch_limit_reduction_sec > 0) {
+        // if height-below-target has been initialized then use it to create and apply a scaler to the pitch_min
+        if (auto_state.height_below_takeoff_to_level_off_cm != 0) {
+            float scalar = remaining_height_to_target_cm / (float)auto_state.height_below_takeoff_to_level_off_cm;
+            return auto_state.takeoff_pitch_cd * scalar;
+        }
+
+        // are we entering the region where we want to start leveling off before we reach takeoff alt?
+        float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
+        if (sec_to_target > 0 &&
+            relative_alt_cm >= 1000 &&
+            sec_to_target <= g.takeoff_pitch_limit_reduction_sec) {
+            // make a note of that altitude to use it as a start height for scaling
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100);
+            auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
+        }
+    }
+    return auto_state.takeoff_pitch_cd;
+}
+
 /*
   return a tail hold percentage during initial takeoff for a tail
   dragger