From df1632054e2ddeda18645f28b43ce5c5530a9ff8 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Sat, 26 Jan 2019 16:30:53 -0700
Subject: [PATCH] Plane: Update throttle mix

---
 ArduPlane/ArduPlane.cpp |  5 ++++
 ArduPlane/quadplane.cpp | 54 ++++++++++++++++++++++++++++++++++++++---
 ArduPlane/quadplane.h   |  2 ++
 3 files changed, 58 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 730e1b1e2b..049b489e95 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -184,6 +184,11 @@ void Plane::update_speed_height(void)
 	    // takeoff detection
         SpdHgt_Controller->update_50hz();
     }
+
+    if (quadplane.in_vtol_mode() ||
+        quadplane.in_assisted_flight()) {
+        quadplane.update_throttle_thr_mix();
+    }
 }
 
 
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 34d6775937..c20355aa77 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -1491,9 +1491,6 @@ void QuadPlane::update(void)
 
         assisted_flight = false;
         
-        // give full authority to attitude control
-        attitude_control->set_throttle_mix_max();
-
         // output to motors
         motors_output();
 
@@ -2687,3 +2684,54 @@ float QuadPlane::stopping_distance(void)
     // control the transition distance in a reasonable way
     return plane.ahrs.groundspeed_vector().length_squared() / (2 * transition_decel);
 }
+
+#define LAND_CHECK_ANGLE_ERROR_DEG  30.0f       // maximum angle error to be considered landing
+#define LAND_CHECK_LARGE_ANGLE_CD   1500.0f     // maximum angle target to be considered landing
+#define LAND_CHECK_ACCEL_MOVING     3.0f        // maximum acceleration after subtracting gravity
+
+void QuadPlane::update_throttle_thr_mix(void)
+{
+    // transition will directly manage the mix
+    if (in_transition()) {
+      return;
+    }
+
+    // if disarmed or landed prioritise throttle
+    if(!motors->armed()) {
+        attitude_control->set_throttle_mix_min();
+        return;
+    }
+
+    if (plane.control_mode == QSTABILIZE) {
+        // manual throttle
+        if (plane.get_throttle_input() <= 0) {
+            attitude_control->set_throttle_mix_min();
+        } else {
+            attitude_control->set_throttle_mix_man();
+        }
+    } else {
+        // autopilot controlled throttle
+
+        // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
+        const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
+        bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);
+
+        // check for large external disturbance - angle error over 30 degrees
+        const float angle_error = attitude_control->get_att_error_angle_deg();
+        bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
+
+        // check for large acceleration - falling or high turbulence
+        Vector3f accel_ef = plane.ahrs.get_accel_ef_blended();
+        accel_ef.z += GRAVITY_MSS;
+        bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);
+
+        // check for requested decent
+        bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
+
+        if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
+            attitude_control->set_throttle_mix_max();
+        } else {
+            attitude_control->set_throttle_mix_min();
+        }
+    }
+}
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index b0406a7508..5b4c962648 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -39,6 +39,8 @@ public:
     void setup_target_position(void);
     void takeoff_controller(void);
     void waypoint_controller(void);
+
+    void update_throttle_thr_mix(void);
     
     // update transition handling
     void update(void);