From 4ad2972fd8a5bbd7d2b4f58fcfb875124ff398b1 Mon Sep 17 00:00:00 2001 From: Patrick Menschel Date: Thu, 28 Nov 2024 11:13:37 +0100 Subject: [PATCH] Plane: Fix level-off and throttle during takeoff This patch intends to fix two issues with auto takeoff mission item for planes without an airspeed sensor. Level-off fix: - Correct the level-off condition by factor 2 because during level-off, the pitch and therefore the climb-rate decreses linearly. Mathmatically the plane therefore only climbs half of the altitude in the expected time and is not guaranteed to converge to target altitude by itself. - 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 TECS-controlled throttle range also without airspeed sensor. This restores the behavior of ArduPlane 4.5.7. It is to be discussed if the flag THROTTLE_RANGE in parameter TKOFF_OPTIONS is to be reversed to keep the behavior from Plane 4.5.7 consistent with Plane 4.6 onwards. It could be something like FORCE_MAX_THROTTLE instead. --- ArduPlane/Plane.h | 3 +++ ArduPlane/takeoff.cpp | 9 ++++++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d2d35a7b14e0a..362c5468db96e 100644 --- a/ArduPlane/Plane.h +++ b/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 diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index dda412d5ee05f..47476cfc107f0 100644 --- a/ArduPlane/takeoff.cpp +++ b/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.0f) / 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; } } }