Plane: allow Qassist for tailsitters

This commit is contained in:
Peter Hall 2020-01-02 19:07:09 +00:00 committed by Andrew Tridgell
parent 2467d7b5c7
commit 87cd9a4791
3 changed files with 84 additions and 55 deletions

View File

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

View File

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

View File

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