From b1f1ace73691f144299c67f05adf190ec661fa5e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 23 Apr 2017 11:35:25 +1000 Subject: [PATCH] Plane: support vectored yaw tiltrotors this adds support for tiltrotors which control yaw by vectoring the forward motors. This avoids the need for the rear motor on a tilt-tricopter to have a tilt servo --- ArduPlane/quadplane.cpp | 17 +++++++++++++++-- ArduPlane/quadplane.h | 6 +++++- ArduPlane/tailsitter.cpp | 3 +++ ArduPlane/tiltrotor.cpp | 32 ++++++++++++++++++++++++++++++++ 4 files changed, 55 insertions(+), 3 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 8775228818..32548453b8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -330,8 +330,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: TILT_TYPE // @DisplayName: Tiltrotor type - // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE - // @Values: 0:Continuous,1:Binary + // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover + // @Values: 0:Continuous,1:Binary,2:VectoredYaw AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS), // @Param: TAILSIT_ANGLE @@ -380,6 +380,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 0 1 // @Increment: 0.01 AP_GROUPINFO("TAILSIT_VHGAIN", 54, QuadPlane, tailsitter.vectored_hover_gain, 0.5), + + // @Param: TILT_YAW_ANGLE + // @DisplayName: Tilt minimum angle for vectored yaw + // @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes. + // @Range: 0 30 + AP_GROUPINFO("TILT_YAW_ANGLE", 55, QuadPlane, tilt.tilt_yaw_angle, 0), AP_GROUPEND }; @@ -587,6 +593,13 @@ bool QuadPlane::setup(void) transition_state = TRANSITION_DONE; + if (tilt.tilt_mask != 0 && tilt.tilt_type == TILT_TYPE_VECTORED_YAW) { + // setup tilt servos for vectored yaw + SRV_Channels::set_range(SRV_Channel::k_tiltMotorLeft, 1000); + SRV_Channels::set_range(SRV_Channel::k_tiltMotorRight, 1000); + } + + setup_defaults(); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "QuadPlane initialised"); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ecac56bef1..dbae5ee546 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -332,7 +332,9 @@ private: uint32_t last_ctrl_log_ms; // types of tilt mechanisms - enum {TILT_TYPE_CONTINUOUS=0, TILT_TYPE_BINARY=1}; + enum {TILT_TYPE_CONTINUOUS=0, + TILT_TYPE_BINARY=1, + TILT_TYPE_VECTORED_YAW=2}; // tiltrotor control variables struct { @@ -341,6 +343,7 @@ private: AP_Int16 max_rate_down_dps; AP_Int8 max_angle_deg; AP_Int8 tilt_type; + AP_Float tilt_yaw_angle; float current_tilt; float current_throttle; bool motors_active:1; @@ -383,6 +386,7 @@ private: void tiltrotor_update(void); void tiltrotor_continuous_update(void); void tiltrotor_binary_update(void); + void tiltrotor_vectored_yaw(void); void tilt_compensate_up(float *thrust, uint8_t num_motors); void tilt_compensate_down(float *thrust, uint8_t num_motors); void tilt_compensate(float *thrust, uint8_t num_motors); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index dca9cf981b..b7cdf9735e 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -39,6 +39,9 @@ bool QuadPlane::tailsitter_active(void) */ void QuadPlane::tailsitter_output(void) { + if (!is_tailsitter()) { + return; + } if (!tailsitter_active()) { if (tailsitter.vectored_forward_gain > 0) { // thrust vectoring in fixed wing flight diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index e36075030f..8822921fed 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -176,11 +176,16 @@ void QuadPlane::tiltrotor_update(void) // no motors to tilt return; } + if (tilt.tilt_type == TILT_TYPE_BINARY) { tiltrotor_binary_update(); } else { tiltrotor_continuous_update(); } + + if (tilt.tilt_type == TILT_TYPE_VECTORED_YAW) { + tiltrotor_vectored_yaw(); + } } @@ -331,3 +336,30 @@ bool QuadPlane::tiltrotor_fully_fwd(void) } return (tilt.current_tilt >= 1); } + +/* + control vectored yaw with tilt multicopters + */ +void QuadPlane::tiltrotor_vectored_yaw(void) +{ + // total angle the tilt can go through + float total_angle = 90 + tilt.tilt_yaw_angle; + // output value (0 to 1) to get motors pointed straight up + float zero_out = tilt.tilt_yaw_angle / total_angle; + + // calculate the basic tilt amount from current_tilt + float base_output = zero_out + (tilt.current_tilt * (1 - zero_out)); + + float tilt_threshold = (tilt.max_angle_deg/90.0f); + bool no_yaw = (tilt.current_tilt > tilt_threshold); + if (no_yaw) { + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * base_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * base_output); + } else { + float yaw_out = motors->get_yaw(); + float yaw_range = zero_out; + + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * (base_output + yaw_out * yaw_range)); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * (base_output - yaw_out * yaw_range)); + } +}