Plane: TKOFF_THR_MIN is applied to SLT transitions

Also split in_transition() to forward and backward.
This commit is contained in:
George Zogopoulos 2024-08-01 16:58:11 +02:00 committed by Andrew Tridgell
parent 8d292def15
commit 3b247a346a
8 changed files with 70 additions and 15 deletions

View File

@ -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 &&

View File

@ -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

View File

@ -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<AP_Motors::motor_frame_class> frame_class;
AP_Enum<AP_Motors::motor_frame_type> 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;

View File

@ -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()) {

View File

@ -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));

View File

@ -175,7 +175,9 @@ public:
uint8_t get_log_transition_state() const override { return static_cast<uint8_t>(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;

View File

@ -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.

View File

@ -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<uint8_t>(transition_state); }
bool active() const override;
bool active_frwd() const override;
bool active_back() const override;
bool show_vtol_view() const override;