Plane: Fix level-off and throttle without airspeed sensor

This patch intends to fix two issues with auto takeoff mission item for
planes without airspeed sensor.

Level-off fix:
- Correct the level-off condition by factor 2 because with decreasing pitch,
  also the climb-rate decreses if decreased linearly, it can only reach half of it.
- Add flag for level-off stage and let TECS handle the final climb.

Throttle without airspeed sensor:
- Remove conditional evaluation of airspeed sensor
  to enable traditional throttle range also without airspeed sensor.

It is to be discussed if that flag in takeoff options is to be reversed to keep
the behavior from Plane 4.5.7 consistent with Plane 4.6 onwards.

Author: menschel

ArduPlane/Plane.h

@@ -454,6 +454,9 @@ class Plane : public AP_Vehicle {
         float throttle_lim_min;
         uint32_t throttle_max_timer_ms;
         // Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
+
+        bool in_level_off_phase;
+        // The Flag to indicate the last phase of takeoff, the pitch level-off to neutral.
     } takeoff_state;
 
     // ground steering controller state

ArduPlane/takeoff.cpp

@@ -208,7 +208,7 @@ void Plane::takeoff_calc_pitch(void)
     bool pitch_clipped_max = false;
 
     // If we're using an airspeed sensor, we consult TECS.
-    if (ahrs.using_airspeed_sensor()) {
+    if (ahrs.using_airspeed_sensor() || (takeoff_state.in_level_off_phase == true)) {
         calc_nav_pitch();
         // At any rate, we don't want to go lower than the minimum pitch bound.
         if (nav_pitch_cd < pitch_min_cd) {
@@ -279,7 +279,6 @@ void Plane::takeoff_calc_throttle() {
     // Set the minimum throttle limit.
     const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
     if (!use_throttle_range // We don't want to employ a throttle range.
-        || !ahrs.using_airspeed_sensor() // We don't have an airspeed sensor.
         || below_lvl_alt // We are below TKOFF_LVL_ALT.
         ) { // Traditional takeoff throttle limit.
         takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
@@ -309,13 +308,17 @@ int16_t Plane::get_takeoff_pitch_min_cd(void)
 
         // are we entering the region where we want to start levelling off before we reach takeoff alt?
         if (auto_state.sink_rate < -0.1f) {
-            float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
+            float sec_to_target = (remaining_height_to_target_cm * 0.01f * 2) / MIN(TECS_controller.get_max_climbrate(), -auto_state.sink_rate);
+            /* A linear decrease of pitch at level-off is also a linear decrease of climb-rate, thus the height gained is only half.
+             * Additionally, the climb-rate is limited by TECS value to keep external factors like wind influence at bay.
+             */
             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(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100));
                 auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
+                takeoff_state.in_level_off_phase = true;
             }
         }
     }