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
|
#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 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
|
#if HAL_QUADPLANE_ENABLED
|
||||||
const bool quadplane_in_transition = quadplane.in_transition();
|
const bool quadplane_in_frwd_transition = quadplane.in_frwd_transition();
|
||||||
#else
|
#else
|
||||||
const bool quadplane_in_transition = false;
|
const bool quadplane_in_frwd_transition = false;
|
||||||
#endif
|
#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;
|
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
|
- throttle stick at zero thrust
|
||||||
- in fixed wing non auto-throttle mode
|
- in fixed wing non auto-throttle mode
|
||||||
*/
|
*/
|
||||||
if (!quadplane_in_transition &&
|
if (!quadplane_in_frwd_transition &&
|
||||||
!control_mode->is_vtol_mode() &&
|
!control_mode->is_vtol_mode() &&
|
||||||
!control_mode->does_auto_throttle() &&
|
!control_mode->does_auto_throttle() &&
|
||||||
flare_mode == FlareMode::ENABLED_PITCH_TARGET &&
|
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_speed_z_max_dn.convert_centi_parameter(AP_PARAM_INT16);
|
||||||
pilot_accel_z.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();
|
tailsitter.setup();
|
||||||
|
|
||||||
tiltrotor.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
|
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));
|
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));
|
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:
|
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
|
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);
|
void control_auto(void);
|
||||||
bool setup(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 vtol_position_controller(void);
|
||||||
void setup_target_position(void);
|
void setup_target_position(void);
|
||||||
void takeoff_controller(void);
|
void takeoff_controller(void);
|
||||||
|
@ -103,10 +110,9 @@ public:
|
||||||
// abort landing, only valid when in a VTOL landing descent
|
// abort landing, only valid when in a VTOL landing descent
|
||||||
bool abort_landing(void);
|
bool abort_landing(void);
|
||||||
|
|
||||||
/*
|
|
||||||
return true if we are in a transition to fwd flight from hover
|
|
||||||
*/
|
|
||||||
bool in_transition(void) const;
|
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;
|
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_class> frame_class;
|
||||||
AP_Enum<AP_Motors::motor_frame_type> frame_type;
|
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
|
// Initialise motors to allow passing it to tailsitter in its constructor
|
||||||
AP_MotorsMulticopter *motors = nullptr;
|
AP_MotorsMulticopter *motors = nullptr;
|
||||||
const struct AP_Param::GroupInfo *motors_var_info;
|
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;
|
slewrate = g.takeoff_throttle_slewrate;
|
||||||
}
|
}
|
||||||
#if HAL_QUADPLANE_ENABLED
|
#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;
|
slewrate = g.takeoff_throttle_slewrate;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -555,10 +555,20 @@ float Plane::apply_throttle_limits(float throttle_in)
|
||||||
|
|
||||||
// Handle throttle limits for transition conditions.
|
// Handle throttle limits for transition conditions.
|
||||||
#if HAL_QUADPLANE_ENABLED
|
#if HAL_QUADPLANE_ENABLED
|
||||||
if (quadplane.in_transition()) {
|
if (quadplane.in_frwd_transition()) {
|
||||||
if (aparm.takeoff_throttle_max != 0) {
|
if (aparm.takeoff_throttle_max != 0) {
|
||||||
max_throttle = aparm.takeoff_throttle_max.get();
|
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
|
#endif
|
||||||
|
|
||||||
|
@ -793,7 +803,7 @@ void Plane::servos_twin_engine_mix(void)
|
||||||
void Plane::force_flare(void)
|
void Plane::force_flare(void)
|
||||||
{
|
{
|
||||||
#if HAL_QUADPLANE_ENABLED
|
#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;
|
return;
|
||||||
}
|
}
|
||||||
if (control_mode->is_vtol_mode()) {
|
if (control_mode->is_vtol_mode()) {
|
||||||
|
|
|
@ -208,6 +208,8 @@ void Tailsitter::setup()
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
quadplane.thrust_type = QuadPlane::ThrustType::TAILSITTER;
|
||||||
|
|
||||||
// Set tailsitter transition rate to match old calculation
|
// Set tailsitter transition rate to match old calculation
|
||||||
if (!transition_rate_fw.configured()) {
|
if (!transition_rate_fw.configured()) {
|
||||||
transition_rate_fw.set_and_save(transition_angle_fw / (quadplane.transition_time_ms/2000.0f));
|
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); }
|
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;
|
bool show_vtol_view() const override;
|
||||||
|
|
||||||
|
|
|
@ -106,6 +106,8 @@ void Tiltrotor::setup()
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
quadplane.thrust_type = QuadPlane::ThrustType::TILTROTOR;
|
||||||
|
|
||||||
_is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW;
|
_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.
|
// 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 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;
|
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); }
|
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;
|
bool show_vtol_view() const override;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue