From 5ddd33227774a505d965a43fc425dc859167ef5c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 7 Jan 2016 12:11:27 +0900
Subject: [PATCH] AC_AttControl: add ANGLE_BOOST param

This allows disabling angle boost
---
 libraries/AC_AttitudeControl/AC_AttitudeControl.cpp       | 7 +++++++
 libraries/AC_AttitudeControl/AC_AttitudeControl.h         | 3 +++
 libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 4 ++++
 3 files changed, 14 insertions(+)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index 430c22a15b..095c7faee1 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -59,6 +59,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
 
     // IDs 8,9,10,11 RESERVED (in use on Solo)
 
+    // @Param: ANGLE_BOOST
+    // @DisplayName: Angle Boost
+    // @Description: Angle Boost increases output throttle as the vehicle leans to reduce loss of altitude
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
index 773204767f..956fcd4547 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -293,6 +293,9 @@ protected:
     // Enable/Disable body frame rate feed forward
     AP_Int8             _rate_bf_ff_enabled;
 
+    // Enable/Disable angle boost
+    AP_Int8             _angle_boost_enabled;
+
     // Intersampling period in seconds
     float               _dt;
 
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
index 993df984a9..073d79f41b 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
@@ -16,6 +16,10 @@ float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
 // throttle value should be 0 ~ 1000
 float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in)
 {
+    if (!_angle_boost_enabled) {
+        return throttle_in;
+        _angle_boost = 0;
+    }
     // inverted_factor is 1 for tilt angles below 60 degrees
     // reduces as a function of angle beyond 60 degrees
     // becomes zero at 90 degrees