diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a6a53d6825..9a0c26360b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1449,7 +1449,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const */ bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed) { - if (assist_speed <= 0) { + if (assist_speed <= 0 || is_contol_surface_tailsitter()) { // assistance disabled in_angle_assist = false; angle_error_start_ms = 0; @@ -1530,6 +1530,14 @@ bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed) return ret; } +// return true if it is safe to provide assistance +bool QuadPlane::assistance_safe() +{ + return hal.util->get_soft_armed() && ( (plane.auto_throttle_mode && !plane.throttle_suppressed) + || plane.get_throttle_input()>0 + || plane.is_flying() ); +} + /* update for transition from quadplane to fixed wing mode */ @@ -1574,12 +1582,8 @@ void QuadPlane::update_transition(void) /* see if we should provide some assistance */ - 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) && !is_contol_surface_tailsitter()))) { + if (assistance_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)))) { // the quad should provide some assistance to the plane assisted_flight = true; if (!is_tailsitter()) { @@ -1802,7 +1806,7 @@ void QuadPlane::update(void) const uint32_t now = AP_HAL::millis(); assisted_flight = false; - + // output to motors motors_output(); @@ -1817,6 +1821,13 @@ void QuadPlane::update(void) transition_start_ms = now; } else if (is_tailsitter() && transition_state == TRANSITION_ANGLE_WAIT_VTOL) { + float aspeed; + bool have_airspeed = ahrs.airspeed_estimate(aspeed); + // provide asistance in forward flight portion of tailsitter transision + if (assistance_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)))) { + assisted_flight = true; + } if (tailsitter_transition_vtol_complete()) { /* we have completed transition to VTOL as a tailsitter, @@ -1982,7 +1993,7 @@ void QuadPlane::motors_output(bool run_rate_controller) return; } - if (in_tailsitter_vtol_transition()) { + if (in_tailsitter_vtol_transition() && !assisted_flight) { /* don't run the motor outputs while in tailsitter->vtol transition. That is taken care of by the fixed wing diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 33a64dfdc9..894cd0b34e 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -191,6 +191,9 @@ private: // check for quadplane assistance needed bool assistance_needed(float aspeed, bool have_airspeed); + // check if it is safe to provide assistance + bool assistance_safe(); + // update transition handling void update_transition(void); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index c02046d42a..5364d66862 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -70,49 +70,51 @@ void QuadPlane::tailsitter_output(void) float tilt_left = 0.0f; float tilt_right = 0.0f; - uint16_t mask = tailsitter.motor_mask; + + // handle forward flight modes and transition to VTOL modes - 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 - float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); - float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); - tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain; - tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain; - } - SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); - SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); - + if (!tailsitter_active() || in_tailsitter_vtol_transition()) { // get FW controller throttle demand and mask of motors enabled during forward flight float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - if (hal.util->get_soft_armed()) { - if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) { - /* - during transitions to vtol mode set the throttle to - hover thrust, center the rudder and set the altitude controller - integrator to the same throttle level - */ - throttle = motors->get_throttle_hover() * 100; - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); - pos_control->get_accel_z_pid().set_integrator(throttle*10); + if (hal.util->get_soft_armed() && in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) { + /* + during transitions to vtol mode set the throttle to + hover thrust, center the rudder and set the altitude controller + integrator to the same throttle level + */ + throttle = motors->get_throttle_hover() * 100; + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + pos_control->get_accel_z_pid().set_integrator(throttle*10); - // override AP_MotorsTailsitter throttles during back transition - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); - } + // override AP_MotorsTailsitter throttles during back transition + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); + } + + if (!assisted_flight) { + // set AP_MotorsMatrix throttles for forward flight + motors->output_motor_mask(throttle * 0.01f, tailsitter.motor_mask, plane.rudder_dt); + + // in forward flight: set motor tilt servos and throttles using FW controller + if (tailsitter.vectored_forward_gain > 0) { + // thrust vectoring in fixed wing flight + float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); + float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); + tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain; + tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain; + } + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); + return; } - // set AP_MotorsMatrix throttles for forward flight - motors->output_motor_mask(throttle * 0.01f, mask, plane.rudder_dt); - return; } // handle VTOL modes // the MultiCopter rate controller has already been run in an earlier call // to motors_output() from quadplane.update(), unless we are in assisted flight - if (assisted_flight && tailsitter_transition_fw_complete()) { + if (assisted_flight) { hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f); motors_output(true); } else {