Skip to content

Commit

Permalink
ArduPlane: Added minimum throttle during TAKEOFF mode
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
Georacer committed Jul 22, 2024
1 parent 395fde4 commit b4d3c86
Show file tree
Hide file tree
Showing 9 changed files with 85 additions and 16 deletions.
5 changes: 5 additions & 0 deletions ArduPlane/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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);
}
Expand Down
18 changes: 17 additions & 1 deletion ArduPlane/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/altitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
8 changes: 4 additions & 4 deletions ArduPlane/mode_takeoff.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -148,19 +148,19 @@ 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
plane.calc_nav_roll();
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();
}
Expand Down
38 changes: 31 additions & 7 deletions ArduPlane/servos.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down
23 changes: 22 additions & 1 deletion ArduPlane/takeoff.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down

0 comments on commit b4d3c86

Please sign in to comment.