From 3b247a346ad1f35adc49fcd8db00e91fa1569174 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 1 Aug 2024 16:58:11 +0200 Subject: [PATCH] Plane: TKOFF_THR_MIN is applied to SLT transitions Also split in_transition() to forward and backward. --- ArduPlane/Attitude.cpp | 6 +++--- ArduPlane/quadplane.cpp | 32 +++++++++++++++++++++++++++++--- ArduPlane/quadplane.h | 15 ++++++++++++--- ArduPlane/servos.cpp | 16 +++++++++++++--- ArduPlane/tailsitter.cpp | 2 ++ ArduPlane/tailsitter.h | 4 +++- ArduPlane/tiltrotor.cpp | 2 ++ ArduPlane/transition.h | 8 ++++++-- 8 files changed, 70 insertions(+), 15 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 788fe97dba..17cc78bc17 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -204,9 +204,9 @@ float Plane::stabilize_pitch_get_pitch_out() #endif // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_DEG if flight option FORCE_FLARE_ATTITUDE is set #if HAL_QUADPLANE_ENABLED - const bool quadplane_in_transition = quadplane.in_transition(); + const bool quadplane_in_frwd_transition = quadplane.in_frwd_transition(); #else - const bool quadplane_in_transition = false; + const bool quadplane_in_frwd_transition = false; #endif int32_t demanded_pitch = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; @@ -219,7 +219,7 @@ float Plane::stabilize_pitch_get_pitch_out() - throttle stick at zero thrust - in fixed wing non auto-throttle mode */ - if (!quadplane_in_transition && + if (!quadplane_in_frwd_transition && !control_mode->is_vtol_mode() && !control_mode->does_auto_throttle() && flare_mode == FlareMode::ENABLED_PITCH_TARGET && diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index fb07554926..f32c8e189d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -813,6 +813,10 @@ bool QuadPlane::setup(void) pilot_speed_z_max_dn.convert_centi_parameter(AP_PARAM_INT16); pilot_accel_z.convert_centi_parameter(AP_PARAM_INT16); + // Provisionally assign the SLT thrust type. + // It will be overwritten by tailsitter or tiltorotor setups. + thrust_type = ThrustType::SLT; + tailsitter.setup(); tiltrotor.setup(); @@ -3968,11 +3972,28 @@ bool QuadPlane::is_vtol_land(uint16_t id) const } /* - return true if we are in a transition to fwd flight from hover + return true if we are in any kind of transition */ bool QuadPlane::in_transition(void) const { - return available() && transition->active(); + return in_frwd_transition() || in_back_transition(); +} + +/* + return true if we are in a transition to fwd flight from hover + */ +bool QuadPlane::in_frwd_transition(void) const +{ + return available() && transition->active_frwd(); +} + +/* + return true if we are in a transition to hover from fwd flight + */ +bool QuadPlane::in_back_transition(void) const +{ + // By default the + return available() && transition->active_back(); } /* @@ -4351,11 +4372,16 @@ bool SLT_Transition::allow_update_throttle_mix() const return !(quadplane.assisted_flight && (transition_state == TRANSITION_AIRSPEED_WAIT || transition_state == TRANSITION_TIMER)); } -bool SLT_Transition::active() const +bool SLT_Transition::active_frwd() const { return quadplane.assisted_flight && ((transition_state == TRANSITION_AIRSPEED_WAIT) || (transition_state == TRANSITION_TIMER)); } +bool SLT_Transition::active_back() const +{ + return false; +} + /* limit VTOL roll/pitch in POSITION1, POSITION2 and waypoint controller. This serves three roles: 1) an expanding envelope limit on pitch to prevent sudden pitch at the start of a back transition diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 2ced92498b..24f25c06e8 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -76,6 +76,13 @@ public: void control_auto(void); bool setup(void); + enum class ThrustType : uint8_t { + SLT=0, // Traditional quadplane, with a pusher motor and independent multicopter lift. + TAILSITTER, + TILTROTOR, + }; + ThrustType get_thrust_type(void) {return thrust_type;} + void vtol_position_controller(void); void setup_target_position(void); void takeoff_controller(void); @@ -103,10 +110,9 @@ public: // abort landing, only valid when in a VTOL landing descent bool abort_landing(void); - /* - return true if we are in a transition to fwd flight from hover - */ bool in_transition(void) const; + bool in_frwd_transition(void) const; + bool in_back_transition(void) const; bool handle_do_vtol_transition(enum MAV_VTOL_STATE state) const; @@ -198,6 +204,9 @@ private: AP_Enum frame_class; AP_Enum frame_type; + // Types of different "quadplane" configurations. + ThrustType thrust_type; + // Initialise motors to allow passing it to tailsitter in its constructor AP_MotorsMulticopter *motors = nullptr; const struct AP_Param::GroupInfo *motors_var_info; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index aa2c7ce373..92662c0748 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -51,7 +51,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) slewrate = g.takeoff_throttle_slewrate; } #if HAL_QUADPLANE_ENABLED - if (g.takeoff_throttle_slewrate != 0 && quadplane.in_transition()) { + if (g.takeoff_throttle_slewrate != 0 && quadplane.in_frwd_transition()) { slewrate = g.takeoff_throttle_slewrate; } #endif @@ -555,10 +555,20 @@ float Plane::apply_throttle_limits(float throttle_in) // Handle throttle limits for transition conditions. #if HAL_QUADPLANE_ENABLED - if (quadplane.in_transition()) { + if (quadplane.in_frwd_transition()) { if (aparm.takeoff_throttle_max != 0) { max_throttle = aparm.takeoff_throttle_max.get(); } + + // Apply minimum throttle limits only for SLT thrust types. + // The other types don't support it well. + if (quadplane.get_thrust_type() == QuadPlane::ThrustType::SLT + && control_mode->does_auto_throttle() + ) { + if (aparm.takeoff_throttle_min.get() != 0) { + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get()); + } + } } #endif @@ -793,7 +803,7 @@ void Plane::servos_twin_engine_mix(void) void Plane::force_flare(void) { #if HAL_QUADPLANE_ENABLED - if (quadplane.in_transition() && plane.arming.is_armed()) { //allows for ground checking of flare tilts + if (quadplane.in_frwd_transition() && plane.arming.is_armed()) { //allows for ground checking of flare tilts return; } if (control_mode->is_vtol_mode()) { diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index b530dacce5..33d111db3d 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -208,6 +208,8 @@ void Tailsitter::setup() return; } + quadplane.thrust_type = QuadPlane::ThrustType::TAILSITTER; + // Set tailsitter transition rate to match old calculation if (!transition_rate_fw.configured()) { transition_rate_fw.set_and_save(transition_angle_fw / (quadplane.transition_time_ms/2000.0f)); diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index fbf26702f8..422c9bd017 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -175,7 +175,9 @@ public: uint8_t get_log_transition_state() const override { return static_cast(transition_state); } - bool active() const override { return transition_state != TRANSITION_DONE; } + bool active_frwd() const override { return transition_state == TRANSITION_ANGLE_WAIT_FW; } + + bool active_back() const override { return transition_state == TRANSITION_ANGLE_WAIT_VTOL; } bool show_vtol_view() const override; diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 9fd1f8a2cb..fe893b01cd 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -106,6 +106,8 @@ void Tiltrotor::setup() return; } + quadplane.thrust_type = QuadPlane::ThrustType::TILTROTOR; + _is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW; // true if a fixed forward motor is configured, either throttle, throttle left or throttle right. diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index b31e45ff04..75d33948e5 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -36,7 +36,9 @@ public: virtual uint8_t get_log_transition_state() const = 0; - virtual bool active() const = 0; + virtual bool active_frwd() const = 0; + + virtual bool active_back() const = 0; virtual bool show_vtol_view() const = 0; @@ -85,7 +87,9 @@ public: uint8_t get_log_transition_state() const override { return static_cast(transition_state); } - bool active() const override; + bool active_frwd() const override; + + bool active_back() const override; bool show_vtol_view() const override;