Plane: improve tiltrotor transition strategy

avoid running forward motors at high throttle when transitioning to a
VTOL mode

ArduPlane/quadplane.h

@@ -370,7 +370,12 @@ private:
     void tiltrotor_update(void);
     void tiltrotor_continuous_update(void);
     void tiltrotor_binary_update(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);
+    bool is_motor_tilting(uint8_t motor) const {
+        return (((uint8_t)tilt.tilt_mask.get()) & (1U<<motor));
+    }
     bool tiltrotor_fully_fwd(void);
     float tilt_max_change(bool up);
 

ArduPlane/tiltrotor.cpp

@@ -198,14 +198,23 @@ void QuadPlane::tiltrotor_update(void)
 
   By applying _tilt_equal_thrust the tilted motors effectively become
   a single pitch control motor.
+
+  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
  */
-void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
+void QuadPlane::tilt_compensate_down(float *thrust, uint8_t num_motors)
 {
-    float tilt_factor;
+    float inv_tilt_factor;
     if (tilt.current_tilt > 0.98f) {
-        tilt_factor = 1.0 / cosf(radians(0.98f*90));
+        inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
     } else {
-        tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
+        inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
     }
 
     // when we got past Q_TILT_MAX we gang the tilted motors together
@@ -218,12 +227,11 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
 
     float tilt_total = 0;
     uint8_t tilt_count = 0;
-    uint8_t mask = tilt.tilt_mask;
     
-    // apply _tilt_factor first
+    // apply inv_tilt_factor first
     for (uint8_t i=0; i<num_motors; i++) {
-        if (mask & (1U<<i)) {
+        if (is_motor_tilting(i)) {
-            thrust[i] *= tilt_factor;
+            thrust[i] *= inv_tilt_factor;
             tilt_total += thrust[i];
             tilt_count++;
         }
@@ -233,7 +241,7 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
 
     // now constrain and apply _tilt_equal_thrust if enabled
     for (uint8_t i=0; i<num_motors; i++) {
-        if (mask & (1U<<i)) {
+        if (is_motor_tilting(i)) {
             if (equal_thrust) {
                 thrust[i] = tilt_total / tilt_count;
             }
@@ -252,6 +260,64 @@ void QuadPlane::tilt_compensate(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
+  VTOL mode we use tilt_compensate_up
+ */
+void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
+{
+    if (tilt.current_tilt <= 0) {
+        // the motors are not tilted, no compensation needed
+        return;
+    }
+    if (in_vtol_mode()) {
+        // we are transitioning to VTOL flight
+        tilt_compensate_up(thrust, num_motors);
+    } else {
+        tilt_compensate_down(thrust, num_motors);
+    }
+}
+
 /*
   return true if the rotors are fully tilted forward
  */