Plane: Takeoff improvements

- TAKEOFF and AUTO flight modes now should have identical takeoff
- Prevent behaviour switching past climb altitude in TAKEOFF mode.
- Refactor set_pitch_min/max methods.
  Max was already there, now renamed.
  Min is newly introduced.
  behaviour.
- Remove enforcement of min takeoff throttle logic from servos.cpp.
  It is now handled only by takeoff.cpp.
- Take TKOFF_LVL_ALT into consideration in AUTO as well.
- Fixed pitch setpoint when TKOFF_ROTATE_SPD>0.
- Roll navigation in mode TAKEOFF during climb should now work again.
- Now the TAKEOFF loiter waypoint is set by the bearing of the
aircraft while on TKOFF_LVL_ALT, as in the last stable release, instead
of TKOFF_ALT.
- Using TRIM_THROTTLE in takeoffs, when TKOFF_THR_MIN==0

ArduPlane/ArduPlane.cpp

@@ -493,10 +493,14 @@ void Plane::update_GPS_10Hz(void)
  */
 void Plane::update_control_mode(void)
 {
-    if (control_mode != &mode_auto) {
+    if ((control_mode != &mode_auto) && (control_mode != &mode_takeoff)) {
         // hold_course is only used in takeoff and landing
         steer_state.hold_course_cd = -1;
     }
+    // refresh the throttle limits, to avoid using stale values
+    // they will be updated once takeoff_calc_throttle is called
+    takeoff_state.throttle_lim_max = 100.0f;
+    takeoff_state.throttle_lim_min = -100.0f;
 
     update_fly_forward();
 

ArduPlane/Plane.h

@@ -446,6 +446,10 @@ class Plane : public AP_Vehicle {
         uint32_t accel_event_ms;
         uint32_t start_time_ms;
         bool waiting_for_rudder_neutral;
+        float throttle_lim_max;
+        float throttle_lim_min;
+        uint32_t throttle_max_timer_ms;
+        // Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
     } takeoff_state;
 
     // ground steering controller state
@@ -1131,7 +1135,7 @@ class Plane : public AP_Vehicle {
     bool auto_takeoff_check(void);
     void takeoff_calc_roll(void);
     void takeoff_calc_pitch(void);
-    void takeoff_calc_throttle(const bool use_max_throttle=false);
+    void takeoff_calc_throttle();
     int8_t takeoff_tail_hold(void);
     int16_t get_takeoff_pitch_min_cd(void);
     void landing_gear_update(void);

ArduPlane/commands_logic.cpp

@@ -393,7 +393,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
     set_next_WP(cmd.content.location);
 
     // configure abort altitude and pitch
-    // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m
+    // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 30m
     if (cmd.p1 > 0) {
         auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100;
     } else if (auto_state.takeoff_altitude_rel_cm <= 0) {

ArduPlane/mode_auto.cpp

@@ -89,7 +89,7 @@ void ModeAuto::update()
         (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
         plane.takeoff_calc_roll();
         plane.takeoff_calc_pitch();
-        plane.calc_throttle();
+        plane.takeoff_calc_throttle();
     } else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
         plane.calc_nav_roll();
         plane.calc_nav_pitch();

ArduPlane/mode_takeoff.cpp

@@ -81,6 +81,7 @@ void ModeTakeoff::update()
     const float alt = target_alt;
     const float dist = target_dist;
     if (!takeoff_mode_setup) {
+        plane.auto_state.takeoff_altitude_rel_cm = alt * 100;
         const uint16_t altitude = plane.relative_ground_altitude(false,true);
         const float direction = degrees(ahrs.get_yaw());
         // see if we will skip takeoff as already flying
@@ -92,11 +93,12 @@ void ModeTakeoff::update()
                 plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
             } else {
                 gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering");
+                start_loc = plane.current_loc;
                 plane.next_WP_loc = plane.current_loc;
                 plane.next_WP_loc.alt += ((alt - altitude) *100);
                 plane.next_WP_loc.offset_bearing(direction, dist);
                 takeoff_mode_setup = true;
-                plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
+                plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
             }
             // not flying so do a full takeoff sequence
         } else {
@@ -117,57 +119,59 @@ void ModeTakeoff::update()
                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
                                 alt, dist, direction);
                 plane.takeoff_state.start_time_ms = millis();
+                plane.takeoff_state.throttle_max_timer_ms = millis();
                 takeoff_mode_setup = true;
-
+                plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
             }
         }
     }
     // check for optional takeoff timeout
     if (plane.check_takeoff_timeout()) {
         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
         takeoff_mode_setup = false;
-
     }
-
-    // we finish the initial level takeoff if we climb past
-    // TKOFF_LVL_ALT or we pass the target location. The check for
-    // target location prevents us flying forever if we can't climb
-    // reset the loiter waypoint target to be correct bearing and dist
-    // from starting location in case original yaw used to set it was off due to EKF
-    // reset or compass interference from max throttle
+
+    // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we
+    // pass the target location. The check for target location prevents us
+    // flying towards a wrong location if we can't climb.
+    // This will correct any yaw estimation errors (caused by EKF reset
+    // or compass interference from max throttle), since it's using position bearing.
     const float altitude_cm = plane.current_loc.alt - start_loc.alt;
-    if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
-        (altitude_cm >= level_alt*100 ||
-         start_loc.get_distance(plane.current_loc) >= dist)) {
-        // reset the target loiter waypoint using current yaw which should be close to correct starting heading
+    if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF
+        && plane.steer_state.hold_course_cd == -1 // This is the enter-once flag.
+        && (altitude_cm >= (level_alt * 100.0f) || start_loc.get_distance(plane.current_loc) >= dist)
+    ) {
         const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
         plane.next_WP_loc = start_loc;
         plane.next_WP_loc.offset_bearing(direction, dist);
         plane.next_WP_loc.alt += alt*100.0;
+        plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
+    }
+
+    // We finish the initial level takeoff if we climb past
+    // TKOFF_ALT or we pass the target location. The check for
+    // target location prevents us flying forever if we can't climb.
+    if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
+        (altitude_cm >= (alt*100 - 200) ||
+        start_loc.get_distance(plane.current_loc) >= dist)) {
         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
     }
 
     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
-        //below TAKOFF_LVL_ALT
+        //below TKOFF_ALT
         plane.takeoff_calc_roll();
         plane.takeoff_calc_pitch();
-        plane.takeoff_calc_throttle(true);
+        plane.takeoff_calc_throttle();
     } else {
-        if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering
 #if AP_FENCE_ENABLED
-            if (!have_autoenabled_fences) {
-                plane.fence.auto_enable_fence_after_takeoff();
-                have_autoenabled_fences = true;
-            }
-#endif
-            plane.calc_nav_roll();
-            plane.calc_nav_pitch();
-            plane.calc_throttle();
-        } else { // still climbing to TAKEOFF_ALT; may be loitering
-            plane.takeoff_calc_throttle();
-            plane.takeoff_calc_roll();
-            plane.takeoff_calc_pitch();
+        if (!have_autoenabled_fences) {
+            plane.fence.auto_enable_fence_after_takeoff();
+            have_autoenabled_fences = true;
         }
+#endif
+        plane.calc_nav_roll();
+        plane.calc_nav_pitch();
+        plane.calc_throttle();
 
         //check if in long failsafe due to being in initial TAKEOFF stage; if it is, recall long failsafe now to get fs action via events call
         if (plane.long_failsafe_pending) {

ArduPlane/quadplane.cpp

@@ -3401,7 +3401,8 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
         return false;
     }
     transition->restart();
-    plane.TECS_controller.set_pitch_max_limit(transition_pitch_max);
+    plane.TECS_controller.set_pitch_max(transition_pitch_max);
+    plane.TECS_controller.set_pitch_min(-transition_pitch_max);
 
     // todo: why are you doing this, I want to delete it.
     set_alt_target_current();
@@ -4551,7 +4552,8 @@ void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_
     }
 
     // set a single loop pitch limit in TECS
-    plane.TECS_controller.set_pitch_max_limit(max_pitch);
+    plane.TECS_controller.set_pitch_max(max_pitch);
+    plane.TECS_controller.set_pitch_min(-max_pitch);
 
     // ensure pitch is constrained to limit
     nav_pitch_cd = constrain_int32(nav_pitch_cd, -max_pitch*100.0, max_pitch*100.0);

ArduPlane/servos.cpp

@@ -519,34 +519,17 @@ float Plane::apply_throttle_limits(float throttle_in)
         min_throttle = 0;
     }
 
+    // Handle throttle limits for takeoff conditions.
     // Query the conditions where TKOFF_THR_MAX applies.
     const bool use_takeoff_throttle =
         (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
         (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
 
     // Handle throttle limits for takeoff conditions.
     if (use_takeoff_throttle) {
-        if (aparm.takeoff_throttle_max != 0) {
-            // Replace max throttle with the takeoff max throttle setting.
-            // This is typically done to protect against long intervals of large power draw.
-            // Or (in contrast) to give some extra throttle during the initial climb.
-            max_throttle = aparm.takeoff_throttle_max.get();
-        }
-        // Do not allow min throttle to go below a lower threshold.
-        // This is typically done to protect against premature stalls close to the ground.
-        const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
-        if (!use_throttle_range || !ahrs.using_airspeed_sensor()) {
-            // Use a constant max throttle throughout the takeoff or when airspeed readings are not available.
-            if (aparm.takeoff_throttle_max.get() == 0) {
-                min_throttle = MAX(min_throttle, aparm.throttle_max.get());
-            } else {
-                min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get());
-            }
-        } else if (use_throttle_range) { // Use a throttle range through the takeoff.
-            if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1.
-                min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
-            }
-        }
+        // Read from takeoff_state
+        max_throttle = takeoff_state.throttle_lim_max;
+        min_throttle = takeoff_state.throttle_lim_min;
     } else if (landing.is_flaring()) {
         // Allow throttle cutoff when flaring.
         // This is to allow the aircraft to bleed speed faster and land with a shut off thruster.
@@ -587,6 +570,7 @@ float Plane::apply_throttle_limits(float throttle_in)
     min_throttle = MIN(min_throttle, max_throttle);
 
     // Let TECS know about the updated throttle limits.
+    // These will be taken into account on the next iteration.
     TECS_controller.set_throttle_min(0.01f*min_throttle);
     TECS_controller.set_throttle_max(0.01f*max_throttle);
     return constrain_float(throttle_in, min_throttle, max_throttle);

ArduPlane/takeoff.cpp

@@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void)
         takeoff_state.launchTimerStarted = false;
         takeoff_state.last_tkoff_arm_time = 0;
         takeoff_state.start_time_ms = now;
+        takeoff_state.throttle_max_timer_ms = now;
         steer_state.locked_course_err = 0; // use current heading without any error offset
         return true;
     }
@@ -179,66 +180,111 @@ void Plane::takeoff_calc_roll(void)
  */
 void Plane::takeoff_calc_pitch(void)
 {
-    if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
-        // we have not reached rotate speed, use the specified takeoff target pitch angle
-        nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch);
-        return;
+    // First see if TKOFF_ROTATE_SPD applies.
+    // This will set the pitch for the first portion of the takeoff, up until cruise speed is reached.
+    if (g.takeoff_rotate_speed > 0) {
+        // A non-zero rotate speed is recommended for ground takeoffs.
+        if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
+            // We have not reached rotate speed, use the specified takeoff target pitch angle.
+            nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch);
+            TECS_controller.set_pitch_min(0.01f*nav_pitch_cd);
+            TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);
+            return;
+        } else if (gps.ground_speed() <= (float)aparm.airspeed_cruise) {
+            // If rotate speed applied, gradually transition from TKOFF_GND_PITCH to the climb angle.
+            // This is recommended for ground takeoffs, so delay rotation until ground speed indicates adequate airspeed.
+            const uint16_t min_pitch_cd = 500; // Set a minimum of 5 deg climb angle.
+            nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd;
+            nav_pitch_cd = constrain_int32(nav_pitch_cd, min_pitch_cd, auto_state.takeoff_pitch_cd); 
+            TECS_controller.set_pitch_min(0.01f*nav_pitch_cd);
+            TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);
+            return;
+        }
     }
 
+    // We are now past the rotation.
+    // Initialize pitch limits for TECS.
+    int16_t pitch_min_cd = get_takeoff_pitch_min_cd();
+    bool pitch_clipped_max = false;
+
+    // If we're using an airspeed sensor, we consult TECS.
     if (ahrs.using_airspeed_sensor()) {
-        int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd();
         calc_nav_pitch();
-        if (nav_pitch_cd < takeoff_pitch_min_cd) {
-            nav_pitch_cd = takeoff_pitch_min_cd;
+        // At any rate, we don't want to go lower than the minimum pitch bound.
+        if (nav_pitch_cd < pitch_min_cd) {
+            nav_pitch_cd = pitch_min_cd;
         }
     } else {
-        if (g.takeoff_rotate_speed > 0) {
-            // Rise off ground takeoff so delay rotation until ground speed indicates adequate airspeed
-            nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd;
-            nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); 
-        } else {
-            // Doing hand or catapult launch so need at least 5 deg pitch to prevent initial height loss
-            nav_pitch_cd = MAX(auto_state.takeoff_pitch_cd, 500);
-        }
+        // If not, we will use the minimum allowed angle.
+        nav_pitch_cd = pitch_min_cd;
+
+        pitch_clipped_max = true;
     }
 
+    // Check if we have trouble with roll control.
     if (aparm.stall_prevention != 0) {
-        if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF ||
-            control_mode == &mode_takeoff) {
-            // during takeoff we want to prioritise roll control over
-            // pitch. Apply a reduction in pitch demand if our roll is
-            // significantly off. The aim of this change is to
-            // increase the robustness of hand launches, particularly
-            // in cross-winds. If we start to roll over then we reduce
-            // pitch demand until the roll recovers
-            float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90));
-            float reduction = sq(cosf(roll_error_rad));
-            nav_pitch_cd *= reduction;
+        // during takeoff we want to prioritise roll control over
+        // pitch. Apply a reduction in pitch demand if our roll is
+        // significantly off. The aim of this change is to
+        // increase the robustness of hand launches, particularly
+        // in cross-winds. If we start to roll over then we reduce
+        // pitch demand until the roll recovers
+        float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90));
+        float reduction = sq(cosf(roll_error_rad));
+        nav_pitch_cd *= reduction;
+
+        if (nav_pitch_cd < pitch_min_cd) {
+            pitch_min_cd = nav_pitch_cd;
         }
     }
+    // Notify TECS about the external pitch setting, for the next iteration.
+    TECS_controller.set_pitch_min(0.01f*pitch_min_cd);
+    if (pitch_clipped_max) {TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);}
 }
 
 /*
- * Set the throttle limits to run at during a takeoff.
+ * Calculate the throttle limits to run at during a takeoff.
+ * These limits are meant to be used exclusively by Plane::apply_throttle_limits().
  */
-void Plane::takeoff_calc_throttle(const bool use_max_throttle) {
-    // This setting will take effect at the next run of TECS::update_pitch_throttle().
-
-    // Set the maximum throttle limit.
+void Plane::takeoff_calc_throttle() {
+    // Initialize the maximum throttle limit.
     if (aparm.takeoff_throttle_max != 0) {
-        TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max);
+        takeoff_state.throttle_lim_max = aparm.takeoff_throttle_max;
+    } else {
+        takeoff_state.throttle_lim_max = aparm.throttle_max;
+    }
+
+    // Initialize the minimum throttle limit.
+    if (aparm.takeoff_throttle_min != 0) {
+        takeoff_state.throttle_lim_min = aparm.takeoff_throttle_min;
+    } else {
+        takeoff_state.throttle_lim_min = aparm.throttle_cruise;
+    }
+
+    // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff.
+    // It only applies if the timer has been started externally.
+    if (takeoff_state.throttle_max_timer_ms != 0) {
+        const uint32_t dt = AP_HAL::millis() - takeoff_state.throttle_max_timer_ms;
+        if (dt*0.001 < aparm.takeoff_throttle_max_t) {
+            takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
+        } else {
+            // Reset the timer for future use.
+            takeoff_state.throttle_max_timer_ms = 0;
+        }
     }
 
+    // Enact the TKOFF_OPTIONS logic.
+    const float current_baro_alt = barometer.get_altitude();
+    const bool below_lvl_alt = current_baro_alt < auto_state.baro_takeoff_alt + mode_takeoff.level_alt;
     // Set the minimum throttle limit.
     const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
-    if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit.
-        float min_throttle = (aparm.takeoff_throttle_max != 0) ? 0.01f*aparm.takeoff_throttle_max : 0.01f*aparm.throttle_max;
-        TECS_controller.set_throttle_min(min_throttle);
-    } else { // TKOFF_MODE == 1, allow for a throttle range.
-        if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN.
-            TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min);
-        }
+    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;
     }
+
     calc_throttle();
 }