From ad14e153e82580977ca303bbdb0ac57dcffacc57 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 3 Dec 2020 17:42:39 +1100
Subject: [PATCH] Plane: fixed tilt vectoring to cope with large tilt angles

This uses vectoring for both roll and yaw when tilted, and uses
differential thrust for yaw when tilted
---
 ArduPlane/quadplane.cpp |  26 +++++--
 ArduPlane/quadplane.h   |   4 +-
 ArduPlane/tiltrotor.cpp | 155 ++++++++++++++++------------------------
 3 files changed, 84 insertions(+), 101 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 4a3f505c9d..be24f5865a 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -735,6 +735,15 @@ bool QuadPlane::setup(void)
     if (!motors->initialised_ok()) {
         AP_BoardConfig::config_error("unknown Q_FRAME_TYPE %u", (unsigned)frame_type.get());
     }
+
+    if (motors_var_info == AP_MotorsMatrix::var_info &&
+        tilt.tilt_mask != 0 &&
+        tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {
+        // we will be using vectoring for yaw
+        motors->disable_yaw_torque();
+    }
+
+
     motors->set_throttle_range(thr_min_pwm, thr_max_pwm);
     motors->set_update_rate(rc_speed);
     motors->set_interlock(true);
@@ -1728,11 +1737,18 @@ void QuadPlane::update_transition(void)
         }
         hold_hover(climb_rate_cms);
 
-        // set desired yaw to current yaw in both desired angle and
-        // rate request. This reduces wing twist in transition due to
-        // multicopter yaw demands
-        attitude_control->set_yaw_target_to_current_heading();
-        attitude_control->rate_bf_yaw_target(ahrs.get_gyro().z);
+        if (tilt.tilt_mask == 0 ||
+            tilt.tilt_type != TILT_TYPE_VECTORED_YAW ||
+            now - transition_start_ms < 100) {
+            // set desired yaw to current yaw in both desired angle
+            // and rate request. This reduces wing twist in transition
+            // due to multicopter yaw demands. This is disabled when
+            // using vectored yaw for tilt-rotors as the yaw control
+            // is needed to maintain good control in forward
+            // transitions
+            attitude_control->set_yaw_target_to_current_heading();
+            attitude_control->rate_bf_yaw_target(ahrs.get_gyro().z);
+        }
 
         last_throttle = motors->get_throttle();
 
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 15daf859af..4c333888b3 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -480,6 +480,7 @@ private:
         float current_tilt;
         float current_throttle;
         bool motors_active:1;
+        float transition_yaw;
     } tilt;
 
     // bit 0 enables plane mode and bit 1 enables body-frame roll mode
@@ -542,8 +543,7 @@ private:
     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_angle(float *thrust, uint8_t num_motors, float non_tilted_mul, float tilted_mul);
     void tilt_compensate(float *thrust, uint8_t num_motors);
     bool is_motor_tilting(uint8_t motor) const {
         return (((uint8_t)tilt.tilt_mask.get()) & (1U<<motor));
diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp
index 3732f3575e..09d8e4c5ba 100644
--- a/ArduPlane/tiltrotor.cpp
+++ b/ArduPlane/tiltrotor.cpp
@@ -213,78 +213,66 @@ void QuadPlane::tiltrotor_update(void)
     }
 }
 
-
 /*
-  compensate for tilt in a set of motor outputs
+  tilt compensation for angle of tilt. When the rotors are tilted the
+  roll effect of differential thrust on the tilted rotors is decreased
+  and the yaw effect increased
+  We have two factors we apply.
 
-  Compensation is of two forms. The first is to apply _tilt_factor,
-  which is a compensation for the reduces vertical thrust when
-  tilted. This is supplied by set_motor_tilt_factor().
+  1) when we are transitioning to fwd flight we scale the tilted rotors by 1/cos(angle). This pushes us towards more flight speed
 
-  The second compensation is to use equal thrust on all tilted motors
-  when _tilt_equal_thrust is true. This is used when the motors are
-  tilted by a large angle to prevent the roll and yaw controllers from
-  causing instability. Typically this would be used when the motors
-  are tilted beyond 45 degrees. At this angle it is assumed that roll
-  control can be achieved using fixed wing control surfaces and yaw
-  control with the remaining multicopter motors (eg. tricopter tail).
+  2) when we are transitioning to hover we scale the non-tilted rotors by cos(angle). This pushes us towards lower fwd thrust
 
-  By applying _tilt_equal_thrust the tilted motors effectively become
-  a single pitch control motor.
+  We also apply an equalisation to the tilted motors in proportion to
+  how much tilt we have. This smoothly reduces the impact of the roll
+  gains as we tilt further forward.
 
-  Note that we use a different strategy for when we are transitioning
-  into VTOL as compared to from VTOL flight. The reason for that is
-  we want to lean towards higher tilted motor throttle when
-  transitioning to fixed wing flight, in order to gain airspeed,
-  whereas when transitioning to VTOL flight we want to lean to towards
-  lower fwd throttle. So we raise the throttle on the tilted motors
-  when transitioning to fixed wing, and lower throttle on tilted
-  motors when transitioning to VTOL
+  For yaw, we apply differential thrust in proportion to the demanded
+  yaw control and sin of the tilt angle
+
+  Finally we ensure no requested thrust is over 1 by scaling back all
+  motors so the largest thrust is at most 1.0
  */
-void QuadPlane::tilt_compensate_down(float *thrust, uint8_t num_motors)
+void QuadPlane::tilt_compensate_angle(float *thrust, uint8_t num_motors, float non_tilted_mul, float tilted_mul)
 {
-    float inv_tilt_factor;
-    if (tilt.current_tilt > 0.98f) {
-        inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
-    } else {
-        inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
-    }
-
-    // when we got past Q_TILT_MAX we gang the tilted motors together
-    // to generate equal thrust. This makes them act as a single pitch
-    // control motor while preventing them trying to do roll and yaw
-    // control while angled over. This greatly improves the stability
-    // of the last phase of transitions
-    float tilt_threshold = (tilt.max_angle_deg/90.0f);
-    bool equal_thrust = (tilt.current_tilt > tilt_threshold);
-
     float tilt_total = 0;
     uint8_t tilt_count = 0;
     
-    // apply inv_tilt_factor first
+    // apply tilt_factors first
     for (uint8_t i=0; i<num_motors; i++) {
-        if (is_motor_tilting(i)) {
-            thrust[i] *= inv_tilt_factor;
+        if (!is_motor_tilting(i)) {
+            thrust[i] *= non_tilted_mul;
+        } else {
+            thrust[i] *= tilted_mul;
             tilt_total += thrust[i];
             tilt_count++;
         }
     }
 
     float largest_tilted = 0;
+    const float sin_tilt = sinf(radians(tilt.current_tilt*90));
+    // yaw_gain relates the amount of differential thrust we get from
+    // tilt, so that the scaling of the yaw control is the same at any
+    // tilt angle
+    const float yaw_gain = sinf(radians(tilt.tilt_yaw_angle));
+    const float avg_tilt_thrust = tilt_total / tilt_count;
 
-    // now constrain and apply _tilt_equal_thrust if enabled
     for (uint8_t i=0; i<num_motors; i++) {
         if (is_motor_tilting(i)) {
-            if (equal_thrust) {
-                thrust[i] = tilt_total / tilt_count;
-            }
+            // as we tilt we need to reduce the impact of the roll
+            // controller. This simple method keeps the same average,
+            // but moves us to no roll control as the angle increases
+            thrust[i] = tilt.current_tilt * avg_tilt_thrust + thrust[i] * (1-tilt.current_tilt);
+            // add in differential thrust for yaw control, scaled by tilt angle
+            const float diff_thrust = motors->get_roll_factor(i) * motors->get_yaw() * sin_tilt * yaw_gain;
+            thrust[i] += diff_thrust;
             largest_tilted = MAX(largest_tilted, thrust[i]);
         }
     }
 
-    // if we are saturating one of the tilted motors then reduce all
-    // motors to keep them in proportion to the original thrust. This
-    // helps maintain stability when tilted at a large angle
+    // if we are saturating one of the motors then reduce all motors
+    // to keep them in proportion to the original thrust. This helps
+    // maintain stability when tilted at a large angle
     if (largest_tilted > 1.0f) {
         float scale = 1.0f / largest_tilted;
         for (uint8_t i=0; i<num_motors; i++) {
@@ -293,45 +281,6 @@ void QuadPlane::tilt_compensate_down(float *thrust, uint8_t num_motors)
     }
 }
 
-
-/*
-  tilt compensation when transitioning to VTOL flight
- */
-void QuadPlane::tilt_compensate_up(float *thrust, uint8_t num_motors)
-{
-    float tilt_factor = cosf(radians(tilt.current_tilt*90));
-
-    // when we got past Q_TILT_MAX we gang the tilted motors together
-    // to generate equal thrust. This makes them act as a single pitch
-    // control motor while preventing them trying to do roll and yaw
-    // control while angled over. This greatly improves the stability
-    // of the last phase of transitions
-    float tilt_threshold = (tilt.max_angle_deg/90.0f);
-    bool equal_thrust = (tilt.current_tilt > tilt_threshold);
-
-    float tilt_total = 0;
-    uint8_t tilt_count = 0;
-    
-    // apply tilt_factor first
-    for (uint8_t i=0; i<num_motors; i++) {
-        if (!is_motor_tilting(i)) {
-            thrust[i] *= tilt_factor;
-        } else {
-            tilt_total += thrust[i];
-            tilt_count++;
-        }
-    }
-
-    // now constrain and apply _tilt_equal_thrust if enabled
-    for (uint8_t i=0; i<num_motors; i++) {
-        if (is_motor_tilting(i)) {
-            if (equal_thrust) {
-                thrust[i] = tilt_total / tilt_count;
-            }
-        }
-    }
-}
-
 /*
   choose up or down tilt compensation based on flight mode When going
   to a fixed wing mode we use tilt_compensate_down, when going to a
@@ -345,9 +294,16 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
     }
     if (in_vtol_mode()) {
         // we are transitioning to VTOL flight
-        tilt_compensate_up(thrust, num_motors);
+        const float tilt_factor = cosf(radians(tilt.current_tilt*90));
+        tilt_compensate_angle(thrust, num_motors, tilt_factor, 1);
     } else {
-        tilt_compensate_down(thrust, num_motors);
+        float inv_tilt_factor;
+        if (tilt.current_tilt > 0.98f) {
+            inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
+        } else {
+            inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
+        }
+        tilt_compensate_angle(thrust, num_motors, 1, inv_tilt_factor);
     }
 }
 
@@ -386,8 +342,8 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
             yaw_out /= plane.channel_rudder->get_range();
             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));
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,  1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
+            SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
         }
         return;
     }
@@ -398,11 +354,22 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
         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();
+        const float yaw_out = motors->get_yaw();
+        const float roll_out = motors->get_roll();
         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));
+        // now apply vectored thrust for yaw and roll.
+        const float tilt_rad = radians(tilt.current_tilt*90);
+        const float sin_tilt = sinf(tilt_rad);
+        const float cos_tilt = cosf(tilt_rad);
+        // the MotorsMatrix library normalises roll factor to 0.5, so
+        // we need to use the same factor here to keep the same roll
+        // gains when tilted as we have when not tilted
+        const float avg_roll_factor = 0.5;
+        const float tilt_offset = constrain_float(yaw_out * cos_tilt + avg_roll_factor * roll_out * sin_tilt, -1, 1);
+
+        SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,  1000 * constrain_float(base_output + tilt_offset * yaw_range,0,1));
+        SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - tilt_offset * yaw_range,0,1));
     }
 }