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.
- Add 1m to the target altitude of the waypoint to make TECS iterate faster and reliable.

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.

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/commands_logic.cpp

@@ -372,6 +372,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
     auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt;
     next_WP_loc.lat = home.lat + 10;
     next_WP_loc.lng = home.lng + 10;
+    next_WP_loc.alt += 100; // add 1m to the takeoff target altitude for Navigation to faster converge the last meter
     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;

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;
             }
         }
     }