mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
Plane: allow Qassist for tailsitters
This commit is contained in:
parent
2467d7b5c7
commit
87cd9a4791
@ -827,6 +827,9 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
|
||||
{
|
||||
check_attitude_relax();
|
||||
|
||||
// normal control modes for VTOL and FW flight
|
||||
if (in_vtol_mode()) {
|
||||
|
||||
// tailsitter-only body-frame roll control options
|
||||
// Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
|
||||
if (is_tailsitter() &&
|
||||
@ -871,8 +874,6 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
|
||||
}
|
||||
}
|
||||
|
||||
// normal control modes for VTOL and FW flight
|
||||
if (in_vtol_mode() || is_tailsitter()) {
|
||||
// use euler angle attitude control
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
|
||||
plane.nav_pitch_cd,
|
||||
@ -881,8 +882,13 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
|
||||
// use the fixed wing desired rates
|
||||
float roll_rate = plane.rollController.get_pid_info().target;
|
||||
float pitch_rate = plane.pitchController.get_pid_info().target;
|
||||
if (is_tailsitter()) {
|
||||
// tailsitter roll and yaw swapped due to change in reference frame
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw_2(yaw_rate_cds,pitch_rate*100.0f, -roll_rate*100.0f);
|
||||
} else {
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// hold in stabilize with given throttle
|
||||
@ -900,7 +906,12 @@ void QuadPlane::hold_stabilize(float throttle_in)
|
||||
}
|
||||
} else {
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||
attitude_control->set_throttle_out(throttle_in, true, 0);
|
||||
bool should_boost = true;
|
||||
if (is_tailsitter() && assisted_flight) {
|
||||
// tailsitters in forward flight should not use angle boost
|
||||
should_boost = false;
|
||||
}
|
||||
attitude_control->set_throttle_out(throttle_in, should_boost, 0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1563,12 +1574,12 @@ void QuadPlane::update_transition(void)
|
||||
/*
|
||||
see if we should provide some assistance
|
||||
*/
|
||||
bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
|
||||
const bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
|
||||
plane.get_throttle_input()>0 ||
|
||||
plane.is_flying());
|
||||
if (q_assist_safe &&
|
||||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
|
||||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) {
|
||||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed) && !is_contol_surface_tailsitter()))) {
|
||||
// the quad should provide some assistance to the plane
|
||||
assisted_flight = true;
|
||||
if (!is_tailsitter()) {
|
||||
|
@ -110,6 +110,9 @@ public:
|
||||
// return true when tailsitter frame configured
|
||||
bool is_tailsitter(void) const;
|
||||
|
||||
// return true when flying a control surface only tailsitter tailsitter
|
||||
bool is_contol_surface_tailsitter(void) const;
|
||||
|
||||
// return true when flying a tailsitter in VTOL
|
||||
bool tailsitter_active(void);
|
||||
|
||||
|
@ -31,6 +31,15 @@ bool QuadPlane::is_tailsitter(void) const
|
||||
&& (tilt.tilt_type != TILT_TYPE_BICOPTER);
|
||||
}
|
||||
|
||||
/*
|
||||
return true when flying a control surface only tailsitter tailsitter
|
||||
*/
|
||||
bool QuadPlane::is_contol_surface_tailsitter(void) const
|
||||
{
|
||||
return frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
|
||||
&& ( is_zero(tailsitter.vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
|
||||
}
|
||||
|
||||
/*
|
||||
check if we are flying as a tailsitter
|
||||
*/
|
||||
@ -64,7 +73,7 @@ void QuadPlane::tailsitter_output(void)
|
||||
uint16_t mask = tailsitter.motor_mask;
|
||||
|
||||
// handle forward flight modes and transition to VTOL modes
|
||||
if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
|
||||
if ((!tailsitter_active() || in_tailsitter_vtol_transition()) && !assisted_flight) {
|
||||
// in forward flight: set motor tilt servos and throttles using FW controller
|
||||
if (tailsitter.vectored_forward_gain > 0) {
|
||||
// thrust vectoring in fixed wing flight
|
||||
@ -102,8 +111,14 @@ void QuadPlane::tailsitter_output(void)
|
||||
|
||||
// handle VTOL modes
|
||||
// the MultiCopter rate controller has already been run in an earlier call
|
||||
// to motors_output() from quadplane.update()
|
||||
// to motors_output() from quadplane.update(), unless we are in assisted flight
|
||||
if (assisted_flight && tailsitter_transition_fw_complete()) {
|
||||
hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
|
||||
motors_output(true);
|
||||
} else {
|
||||
motors_output(false);
|
||||
}
|
||||
|
||||
plane.pitchController.reset_I();
|
||||
plane.rollController.reset_I();
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user