From 23c3f25654e47a4b9a367fef8d2e7fbb288a5d27 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Wed, 4 Apr 2012 22:51:39 +0900
Subject: [PATCH] ArduCopter - Attitude.pde - moved heli_get_angle_boost from
 heli.pde (which will be removed) to Attitude.pde.  In fact we should combine
 heli_get_angle_boost and the regular get_angle_boost.

---
 ArduCopter/Attitude.pde | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 7ab489e91e..a41b142e1b 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -80,7 +80,7 @@ get_stabilize_yaw(int32_t target_angle)
 
 	// do not use rate controllers for helicotpers with external gyros
 #if FRAME_CONFIG == HELI_FRAME
-	if(!g.heli_ext_gyro_enabled){
+	if(!motors.ext_gyro_enabled){
 		output = get_rate_yaw(target_rate) + i_term;
 	}else{
 		output = constrain((target_rate + i_term), -4500, 4500);
@@ -390,6 +390,20 @@ static int16_t get_angle_boost(int16_t value)
 //	return (int)(temp * value);
 }
 
+#if FRAME_CONFIG == HELI_FRAME
+// heli_angle_boost - adds a boost depending on roll/pitch values
+// equivalent of quad's angle_boost function
+// throttle value should be 0 ~ 1000
+static int16_t heli_get_angle_boost(int16_t throttle)
+{
+    float angle_boost_factor = cos_pitch_x * cos_roll_x;
+	angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
+	int throttle_above_mid = max(throttle - motors.throttle_mid,0);
+	return throttle + throttle_above_mid*angle_boost_factor;
+
+}
+#endif // HELI_FRAME
+
 #define NUM_G_SAMPLES 40
 
 #if ACCEL_ALT_HOLD == 2