diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 4875a108c58b6..fee9fe5f8f70c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -256,8 +256,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. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover - // @Values: 0:Continuous,1:Binary,2:VectoredYaw + // @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, Bicopter tiltrottor must use the tailsitter frame class (10) + // @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS), // @Param: TAILSIT_ANGLE @@ -309,7 +309,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @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. + // @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. This is also used to limit the forwards travel of bicopter tilts when in VTOL modes // @Range: 0 30 AP_GROUPINFO("TILT_YAW_ANGLE", 55, QuadPlane, tilt.tilt_yaw_angle, 0), @@ -615,7 +615,9 @@ bool QuadPlane::setup(void) // this is a duo-motor tailsitter (vectored thrust if tilt.tilt_mask != 0) motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); motors_var_info = AP_MotorsTailsitter::var_info; - rotation = ROTATION_PITCH_90; + if (tilt.tilt_type != TILT_TYPE_BICOPTER) { + rotation = ROTATION_PITCH_90; + } break; default: motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c727d1a9ca04a..26718c0ab3859 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -415,10 +415,12 @@ class QuadPlane uint32_t last_ctrl_log_ms; // types of tilt mechanisms - enum {TILT_TYPE_CONTINUOUS=0, - TILT_TYPE_BINARY=1, - TILT_TYPE_VECTORED_YAW=2}; - + enum {TILT_TYPE_CONTINUOUS =0, + TILT_TYPE_BINARY =1, + TILT_TYPE_VECTORED_YAW =2, + TILT_TYPE_BICOPTER =3 + }; + // tiltrotor control variables struct { AP_Int16 tilt_mask; @@ -479,6 +481,7 @@ class QuadPlane void tiltrotor_continuous_update(void); void tiltrotor_binary_update(void); void tiltrotor_vectored_yaw(void); + void tiltrotor_bicopter(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/servos.cpp b/ArduPlane/servos.cpp index 3f9582486b7ae..25cdf14530eee 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -812,12 +812,13 @@ void Plane::servos_output(void) // support twin-engine aircraft servos_twin_engine_mix(); - // cope with tailsitters + // cope with tailsitters and bicopters quadplane.tailsitter_output(); - + quadplane.tiltrotor_bicopter(); + // the mixers need pwm to be calculated now SRV_Channels::calc_pwm(); - + // run vtail and elevon mixers servo_output_mixers(); @@ -825,11 +826,11 @@ void Plane::servos_output(void) if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) { SRV_Channels::copy_radio_in_out_mask(uint16_t(g2.manual_rc_mask.get())); } - + SRV_Channels::calc_pwm(); SRV_Channels::output_ch_all(); - + SRV_Channels::push(); if (g2.servo_channels.auto_trim_enabled()) { diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index d9bc9db8b318d..7565a998f14fc 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -25,8 +25,9 @@ */ bool QuadPlane::is_tailsitter(void) const { - return available() && ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || - (tailsitter.motor_mask != 0)); + return available() + && ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (tailsitter.motor_mask != 0)) + && (tilt.tilt_type != TILT_TYPE_BICOPTER); } /* diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 4e1ee13c2683b..18bdf6969aac5 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -367,3 +367,50 @@ void QuadPlane::tiltrotor_vectored_yaw(void) SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * (base_output - yaw_out * yaw_range)); } } + +/* + control bicopter tiltrotors + */ +void QuadPlane::tiltrotor_bicopter(void) +{ + if (tilt.tilt_type != TILT_TYPE_BICOPTER) { + return; + } + + if (!in_vtol_mode() && tiltrotor_fully_fwd()) { + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, -SERVO_MAX); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, -SERVO_MAX); + return; + } + + float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + if (assisted_flight) { + hold_stabilize(throttle * 0.01f); + motors_output(true); + } else { + motors_output(false); + } + + // bicopter assumes that trim is up so we scale down so match + float tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft); + float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight); + + if (is_negative(tilt_left)) { + tilt_left *= tilt.tilt_yaw_angle / 90.0f; + } + if (is_negative(tilt_right)) { + tilt_right *= tilt.tilt_yaw_angle / 90.0f; + } + + // reduce authority of bicopter as motors are tilted forwards + const float scaling = cosf(tilt.current_tilt * M_PI_2); + tilt_left *= scaling; + tilt_right *= scaling; + + // add current tilt and constrain + tilt_left = constrain_float(-(tilt.current_tilt * SERVO_MAX) + tilt_left, -SERVO_MAX, SERVO_MAX); + tilt_right = constrain_float(-(tilt.current_tilt * SERVO_MAX) + tilt_right, -SERVO_MAX, SERVO_MAX); + + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); +}