From c537c38646f0d5caf291245c0eeee6b5bcfb63df Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Mon, 9 Mar 2015 21:16:08 +0900
Subject: [PATCH] Copter: add update_throttle_low_comp

sets the priority of throttle vs attiude control so that attitude is
favoured (i.e. high throttle-low-comp) during dynamic flight while
throttle is favoured when vehicle may be landing.
---
 ArduCopter/ArduCopter.pde    |  3 +++
 ArduCopter/land_detector.pde | 24 ++++++++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index b6d5dd9587..6408a4db84 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1012,6 +1012,9 @@ static void throttle_loop()
     // check if we've landed
     update_land_detector();
 
+    // update throttle_low_comp value (controls priority of throttle vs attitude control)
+    update_throttle_low_comp();
+
     // check auto_armed status
     update_auto_armed();
 
diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde
index 461737dd01..20544864a6 100644
--- a/ArduCopter/land_detector.pde
+++ b/ArduCopter/land_detector.pde
@@ -41,3 +41,27 @@ static void update_land_detector()
     // set land maybe flag
     set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
 }
+
+// update_throttle_low_comp - sets motors throttle_low_comp value depending upon vehicle state
+//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
+//  has no effect when throttle is above hover throttle
+static void update_throttle_low_comp()
+{
+    // manual throttle
+    if (mode_has_manual_throttle(control_mode)) {
+        if(!motors.armed() || g.rc_3.control_in <= 0) {
+            motors.set_throttle_low_comp(0.1f);
+        } else {
+            motors.set_throttle_low_comp(0.5f);
+        }
+    } else {
+        // autopilot controlled throttle
+        const Vector3f angle_target = attitude_control.angle_ef_targets();
+        if (pythagorous2(angle_target.x, angle_target.y) > 1500.0f) {
+            // if target lean angle is over 15 degrees set high
+            motors.set_throttle_low_comp(0.9f);
+        } else {
+            motors.set_throttle_low_comp(0.1f);
+        }
+    }
+}