From 87cd9a4791dd24e0c66a7330ed7da4b3b19c5770 Mon Sep 17 00:00:00 2001 From: Peter Hall <33176108+IamPete1@users.noreply.github.com> Date: Thu, 2 Jan 2020 19:07:09 +0000 Subject: [PATCH] Plane: allow Qassist for tailsitters --- ArduPlane/quadplane.cpp | 109 +++++++++++++++++++++------------------ ArduPlane/quadplane.h | 3 ++ ArduPlane/tailsitter.cpp | 23 +++++++-- 3 files changed, 82 insertions(+), 53 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 8f35a8231b..a6a53d6825 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -827,52 +827,53 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) { check_attitude_relax(); - // 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() && - (tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) { - - if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) { - // In multicopter input mode, the roll and yaw stick axes are independent of pitch - attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false, - plane.nav_roll_cd, - plane.nav_pitch_cd, - yaw_rate_cds); - return; - } else { - // In plane input mode, the roll and yaw sticks are swapped - // and their effective axes rotate from yaw to roll and vice versa - // as pitch goes from zero to 90. - // So it is necessary to also rotate their scaling. - - // Get the roll angle and yaw rate limits - int16_t roll_limit = aparm.angle_max; - // separate limit for tailsitter roll, if set - if (tailsitter.max_roll_angle > 0) { - roll_limit = tailsitter.max_roll_angle * 100.0f; + // 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() && + (tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) { + + if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) { + // In multicopter input mode, the roll and yaw stick axes are independent of pitch + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false, + plane.nav_roll_cd, + plane.nav_pitch_cd, + yaw_rate_cds); + return; + } else { + // In plane input mode, the roll and yaw sticks are swapped + // and their effective axes rotate from yaw to roll and vice versa + // as pitch goes from zero to 90. + // So it is necessary to also rotate their scaling. + + // Get the roll angle and yaw rate limits + int16_t roll_limit = aparm.angle_max; + // separate limit for tailsitter roll, if set + if (tailsitter.max_roll_angle > 0) { + roll_limit = tailsitter.max_roll_angle * 100.0f; + } + // Prevent a divide by zero + float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f; + float yaw2roll_scale = roll_limit / yaw_rate_limit; + + // Rotate as a function of Euler pitch and swap roll/yaw + float euler_pitch = radians(.01f * plane.nav_pitch_cd); + float spitch = fabsf(sinf(euler_pitch)); + float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1); + + float p_yaw_rate = plane.nav_roll_cd / y2r_scale; + float p_roll_angle = -y2r_scale * yaw_rate_cds; + + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true, + p_roll_angle, + plane.nav_pitch_cd, + p_yaw_rate); + return; } - // Prevent a divide by zero - float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f; - float yaw2roll_scale = roll_limit / yaw_rate_limit; - - // Rotate as a function of Euler pitch and swap roll/yaw - float euler_pitch = radians(.01f * plane.nav_pitch_cd); - float spitch = fabsf(sinf(euler_pitch)); - float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1); - - float p_yaw_rate = plane.nav_roll_cd / y2r_scale; - float p_roll_angle = -y2r_scale * yaw_rate_cds; - - attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true, - p_roll_angle, - plane.nav_pitch_cd, - p_yaw_rate); - return; } - } - // 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,7 +882,12 @@ 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; - attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds); + 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); + } } } @@ -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) || - plane.get_throttle_input()>0 || - plane.is_flying()); + 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()) { diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index eec82784b3..33a64dfdc9 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -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); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index e7e3060047..c02046d42a 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -26,11 +26,20 @@ */ bool QuadPlane::is_tailsitter(void) const { - return available() + return available() && ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (tailsitter.motor_mask != 0)) && (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() - motors_output(false); + // 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();