From 6fdaaa7d98fdc80971ab7b48649293f3a8d9513c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 9 May 2016 09:11:11 +1000
Subject: [PATCH] Plane: new tilt compensation method

---
 ArduPlane/quadplane.h   |  1 +
 ArduPlane/tiltrotor.cpp | 95 ++++++++++++++++++++++++++++++++---------
 2 files changed, 75 insertions(+), 21 deletions(-)

diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 83095c0b7e..3fb8ed46a0 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -304,6 +304,7 @@ private:
 
     void tiltrotor_slew(float tilt);
     void tiltrotor_update(void);
+    void tilt_compensate(float *thrust, uint8_t num_motors);
     
 public:
     void motor_test_output();
diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp
index 420613c1f0..328f60a7cc 100644
--- a/ArduPlane/tiltrotor.cpp
+++ b/ArduPlane/tiltrotor.cpp
@@ -19,27 +19,8 @@ void QuadPlane::tiltrotor_slew(float newtilt)
     // translate to 0..1000 range and output
     RC_Channel_aux::set_servo_out(RC_Channel_aux::k_motor_tilt, 1000 * tilt.current_tilt);
 
-    /*
-      we need to tell AP_Motors about the tilt so it can compensate
-      for reduced vertical thrust
-     */
-    float tilt_factor;
-    if (tilt.current_tilt > 0.98f) {
-        tilt_factor = 1.0 / cosf(radians(0.98f*90));
-    } else {
-        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);
-
-    // tell motors library about the tilt
-    motors->set_motor_tilt_factor(tilt_factor, tilt.tilt_mask, equal_thrust);
+    // setup tilt compensation
+    motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t));
 }
 
 /*
@@ -121,3 +102,75 @@ void QuadPlane::tiltrotor_update(void)
         tiltrotor_slew(settilt * tilt.max_angle_deg / 90.0f);
     }
 }
+
+
+/*
+  compensate for tilt in a set of motor outputs
+
+  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().
+
+  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).
+
+  By applying _tilt_equal_thrust the tilted motors effectively become
+  a single pitch control motor.
+ */
+void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
+{
+    float tilt_factor;
+    if (tilt.current_tilt > 0.98f) {
+        tilt_factor = 1.0 / cosf(radians(0.98f*90));
+    } else {
+        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;
+    uint8_t mask = tilt.tilt_mask;
+    
+    // apply _tilt_factor first
+    for (uint8_t i=0; i<num_motors; i++) {
+        if (mask & (1U<<i)) {
+            thrust[i] *= tilt_factor;
+            tilt_total += thrust[i];
+            tilt_count++;
+        }
+    }
+
+    float largest_tilted = 0;
+
+    // now constrain and apply _tilt_equal_thrust if enabled
+    for (uint8_t i=0; i<num_motors; i++) {
+        if (mask & (1U<<i)) {
+            if (equal_thrust) {
+                thrust[i] = tilt_total / tilt_count;
+            }
+            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 (largest_tilted > 1.0f) {
+        float scale = 1.0f / largest_tilted;
+        for (uint8_t i=0; i<num_motors; i++) {
+            thrust[i] *= scale;
+        }
+    }
+}