mirror of https://github.com/ArduPilot/ardupilot
Plane: TKOFF_THR_MIN is applied to SLT transitions
Also split in_transition() to forward and backward.
This commit is contained in:
parent
8d292def15
commit
3b247a346a
|
@ -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 &&
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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()) {
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue