diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 630b78850b1637..d3e1fbb6b670f3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4064,6 +4064,21 @@ bool QuadPlane::in_transition(void) const return available() && transition->active(); } +/* + return true if we are in a fwd transition for a SLT quadplane + and in an auto-throttle mode + */ +bool QuadPlane::in_slt_fwd_transition(void) const +{ + if (!in_transition() || tiltrotor.enabled() || tailsitter.enabled()) { + return false; + } + if (!plane.control_mode->does_auto_throttle()) { + return false; + } + return transition->in_fwd_transition(); +} + /* calculate current stopping distance for a quadplane in fixed wing flight */ @@ -4554,6 +4569,15 @@ void SLT_Transition::set_last_fw_pitch() last_fw_nav_pitch_cd = plane.nav_pitch_cd; } +/* + return true if in a forward transition + */ +bool SLT_Transition::in_fwd_transition() const +{ + return transition_state == TRANSITION_AIRSPEED_WAIT || + transition_state == TRANSITION_TIMER; +} + void SLT_Transition::force_transition_complete() { transition_state = TRANSITION_DONE; in_forced_transition = false; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5b2b35fa92dffe..a743a5fd25921f 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -106,6 +106,12 @@ class QuadPlane */ bool in_transition(void) const; + /* + return true if we are in a fwd transition for a SLT quadplane + and in an auto-throttle mode + */ + bool in_slt_fwd_transition(void) const; + bool handle_do_vtol_transition(enum MAV_VTOL_STATE state) const; bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index e3a1cd1c62f60d..dfc489dbe8b281 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -517,7 +517,7 @@ float Plane::apply_throttle_limits(float throttle_in) const bool use_takeoff_throttle_max = #if HAL_QUADPLANE_ENABLED - quadplane.in_transition() || + quadplane.in_slt_fwd_transition() || #endif (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 4e28d534440a3e..c9b03b94f0613c 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -1034,4 +1034,12 @@ bool Tailsitter_Transition::allow_weathervane() return !tailsitter.in_vtol_transition() && (vtol_limit_start_ms == 0); } +/* + return true if in a forward transition + */ +bool Tailsitter_Transition::in_fwd_transition() const +{ + return transition_state == TRANSITION_ANGLE_WAIT_FW; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index fbf26702f8ca9f..b072bcb076ed16 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -189,6 +189,8 @@ friend class Tailsitter; bool allow_weathervane() override; + bool in_fwd_transition() const override; + private: enum { diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index b31e45ff040ece..156e63a4654873 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -58,6 +58,8 @@ class Transition virtual bool allow_stick_mixing() const { return true; } + virtual bool in_fwd_transition() const = 0; + protected: // refences for convenience @@ -101,6 +103,8 @@ class SLT_Transition : public Transition void set_last_fw_pitch(void) override; + bool in_fwd_transition() const override; + protected: enum {