Plane: support bicopter tiltrotors

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

ArduPlane/quadplane.h

@@ -417,7 +417,9 @@ private:
     // types of tilt mechanisms
     enum {TILT_TYPE_CONTINUOUS    =0,
           TILT_TYPE_BINARY        =1,
-          TILT_TYPE_VECTORED_YAW=2};
+          TILT_TYPE_VECTORED_YAW  =2,
+          TILT_TYPE_BICOPTER      =3
+    };
 
     // tiltrotor control variables
     struct {
@@ -479,6 +481,7 @@ private:
     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);

ArduPlane/servos.cpp

@@ -812,8 +812,9 @@ 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();

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);
 }
 
 /*

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);
+}