From 6152ba8d31eba41f85bd15dc28924b74b5a6d50f Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 15 Aug 2024 16:26:48 +0200 Subject: [PATCH] Plane: Throttle limits refactor --- ArduPlane/Plane.h | 6 ++++- ArduPlane/mode.h | 1 - ArduPlane/mode_takeoff.cpp | 1 + ArduPlane/servos.cpp | 16 ++++--------- ArduPlane/takeoff.cpp | 47 +++++++++++++++++++++++++++----------- 5 files changed, 45 insertions(+), 26 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..eeb58ec03b70ca 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -435,6 +435,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 @@ -1115,7 +1119,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); diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 281d4f0f57d615..955a66bbc8b065 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -813,7 +813,6 @@ class ModeTakeoff: public Mode AP_Int8 level_pitch; bool takeoff_mode_setup; - bool climb_complete; Location start_loc; bool _enter() override; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 7b5f68e3c5a4b8..687ba05d2f9db7 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -119,6 +119,7 @@ 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. } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 02aa6fbf76952c..80c9c6da4a9f5d 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -519,6 +519,7 @@ 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) || @@ -526,17 +527,9 @@ float Plane::apply_throttle_limits(float throttle_in) // 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(); - } - if (aparm.takeoff_throttle_min.get() != 0) { - // Replace min throttle with the takeoff min throttle setting. - // This is typically done to protect against premature stalls close to the ground. - 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. @@ -577,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); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 91757ba75684d1..e0f87f21d091f9 100644 --- a/ArduPlane/takeoff.cpp +++ b/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; } @@ -242,28 +243,48 @@ void Plane::takeoff_calc_pitch(void) } /* - * 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) { - // These limits 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_min; } + // 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() || below_lvl_alt || 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(); }