From b4d3c867c0f05260beba7c40c7aa2c7de38dac90 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 24 May 2024 11:38:42 +0200 Subject: [PATCH] ArduPlane: Added minimum throttle during TAKEOFF mode This is a rework so that servos.cpp is responsible for setting the throttle limits under more circumstances and always notifies TECS when it does so. Additionally, the TAKEOFF mode has been improved with a new parameters TKOFF_MODE and TKOFF_THR_MIN that extend the throttle behaviour. --- ArduPlane/Attitude.cpp | 5 +++++ ArduPlane/Parameters.cpp | 18 ++++++++++++++++- ArduPlane/Parameters.h | 2 ++ ArduPlane/Plane.h | 1 + ArduPlane/altitude.cpp | 4 ++-- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 8 ++++---- ArduPlane/servos.cpp | 38 +++++++++++++++++++++++++++++------- ArduPlane/takeoff.cpp | 23 +++++++++++++++++++++- 9 files changed, 85 insertions(+), 16 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0996a50b725ab4..788fe97dbada6d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -448,6 +448,10 @@ void Plane::stabilize() } +/* + * Set the throttle output. + * This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc. +*/ void Plane::calc_throttle() { if (aparm.throttle_cruise <= 1) { @@ -458,6 +462,7 @@ void Plane::calc_throttle() return; } + // Read the TECS throttle output and set it to the throttle channel. float commanded_throttle = TECS_controller.get_throttle_demand(); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 941dcba774ac08..3366ca2dd11d7d 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -142,12 +142,28 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_THR_MAX_T // @DisplayName: Takeoff throttle maximum time - // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff without an airspeed sensor. If an airspeed sensor is being used then the throttle is set to maximum until the takeoff airspeed is reached. + // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff. // @Units: s // @Range: 0 10 // @Increment: 0.5 // @User: Standard ASCALAR(takeoff_throttle_max_t, "TKOFF_THR_MAX_T", 4), + + // @Param: TKOFF_THR_MIN + // @DisplayName: Takeoff minimum throttle + // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_MODE=1. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. + // @Units: % + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60), + + // @Param: TKOFF_MODE + // @DisplayName: Takeoff mode + // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 0: During the takeoff, the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX). 1: During the takeoff TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. + // @Values: 0:Traditional,1:Throttle range + // @User: Advanced + ASCALAR(takeoff_mode, "TKOFF_MODE", 0), // @Param: TKOFF_TDRAG_ELEV // @DisplayName: Takeoff tail dragger elevator diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ba89c457728112..7eeb5459ec88dc 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -356,6 +356,8 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, + k_param_takeoff_throttle_min, + k_param_takeoff_mode, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1af208397ff30..4a78dd32e9e011 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1114,6 +1114,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); int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 8b7970e6315d1a..2844f2fc9164ba 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -513,9 +513,9 @@ int32_t Plane::adjusted_altitude_cm(void) } /* - return home-relative altitude adjusted for ALT_OFFSET This is useful + return home-relative altitude adjusted for ALT_OFFSET. This is useful during long flights to account for barometer changes from the GCS, - or to adjust the flying height of a long mission + or to adjust the flying height of a long mission. */ int32_t Plane::adjusted_relative_altitude_cm(void) { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4766cea5c1fbae..58c7878a22ba24 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -391,7 +391,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; 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.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; // Flag also used to override "on the ground" throttle disable diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 37d0203fcf413f..4d193f0f93b06d 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -26,7 +26,7 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Param: LVL_PITCH // @DisplayName: Takeoff mode altitude initial pitch - // @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT + // @Description: This is the target pitch during the takeoff. // @Range: 0 30 // @Increment: 1 // @Units: deg @@ -148,11 +148,11 @@ void ModeTakeoff::update() if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TAKOFF_LVL_ALT - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); + plane.takeoff_calc_throttle(true); } else { - if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering + if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering #if AP_FENCE_ENABLED plane.fence.auto_enable_fence_after_takeoff(); #endif @@ -160,7 +160,7 @@ void ModeTakeoff::update() plane.calc_nav_pitch(); plane.calc_throttle(); } else { // still climbing to TAKEOFF_ALT; may be loitering - plane.calc_throttle(); + plane.takeoff_calc_throttle(); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cc4d4dde104e38..b5555fdca6a2be 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -499,47 +499,71 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) #endif // #if AP_BATTERY_WATT_MAX_ENABLED /* - Apply min/max limits to throttle + Apply min/max safety limits to throttle. */ float Plane::apply_throttle_limits(float throttle_in) { - // convert 0 to 100% (or -100 to +100) into PWM + // Pull the base throttle limits. + // These are usually set to map the ESC operating range. int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); #if AP_ICENGINE_ENABLED - // apply idle governor + // Apply idle governor. g2.ice_control.update_idle_governor(min_throttle); #endif + // If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0. if (min_throttle < 0 && !allow_reverse_thrust()) { // reverse thrust is available but inhibited. min_throttle = 0; } - const bool use_takeoff_throttle_max = + // Query the conditions where TKOFF_THR_MAX applies. + const bool use_takeoff_throttle = #if HAL_QUADPLANE_ENABLED quadplane.in_transition() || #endif (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); - if (use_takeoff_throttle_max) { + 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. + if (aparm.takeoff_mode.get() == 0) { // Use a constant max throttle throughout the takeoff. + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); + } else if (aparm.takeoff_mode.get() == 1) { // 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()); + } + } } 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. min_throttle = 0; } - // compensate for battery voltage drop + // Compensate the limits for battery voltage drop. + // This relaxes the limits when the battery is getting depleted. g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle); #if AP_BATTERY_WATT_MAX_ENABLED - // apply watt limiter + // Ensure that the power draw limits are not exceeded. throttle_watt_limiter(min_throttle, max_throttle); #endif + // Do a sanity check on them. Constrain down if necessary. + min_throttle = MIN(min_throttle, max_throttle); + + // Let TECS know about the updated throttle limits. + 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 faa220412fd914..d6e8ddc4f533d4 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -219,7 +219,28 @@ void Plane::takeoff_calc_pitch(void) } /* - * get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off + * Set the throttle limits to run at during a takeoff. + */ +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. + if (aparm.takeoff_throttle_max != 0) { + TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max); + } + + // Set the minimum throttle limit. + if (aparm.takeoff_mode==0 || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. + TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max); + } 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); + } + } + calc_throttle(); +} + +/* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off */ int16_t Plane::get_takeoff_pitch_min_cd(void) {