From f3fd79597f93eec6ec2b3b72f98d628d616f05d4 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 13 Aug 2014 11:25:59 +0900
Subject: [PATCH] Copter: add ACRO_EXPO parameter

---
 ArduCopter/Parameters.h     |  4 +++-
 ArduCopter/Parameters.pde   |  7 +++++++
 ArduCopter/config.h         |  4 ++++
 ArduCopter/control_acro.pde | 35 +++++++++++++++++++++--------------
 4 files changed, 35 insertions(+), 15 deletions(-)

diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 2e4db9cc2a..72c6899142 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -117,8 +117,9 @@ public:
         k_param_serial2_baud,
         k_param_land_repositioning,
         k_param_sonar, // sonar object
-        k_param_ekfcheck_thresh,        // 54
+        k_param_ekfcheck_thresh,
         k_param_terrain,
+        k_param_acro_expo,              // 56
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -434,6 +435,7 @@ public:
     AP_Float                acro_balance_roll;
     AP_Float                acro_balance_pitch;
     AP_Int8                 acro_trainer;
+    AP_Float                acro_expo;
 
     // PI/D controllers
 #if FRAME_CONFIG == HELI_FRAME
diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index ad22b2db42..136c4cc196 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -586,6 +586,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(acro_trainer,   "ACRO_TRAINER",     ACRO_TRAINER_LIMITED),
 
+    // @Param: ACRO_EXPO
+    // @DisplayName: Acro Expo
+    // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
+    // @Values: 0:Disabled,0.2:Low,0.3:Medium,0.4:High
+    // @User: Advanced
+    GSCALAR(acro_expo,  "ACRO_EXPO",    ACRO_EXPO_DEFAULT),
+
     // PID controller
     //---------------
 
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 57811c194b..7293119e8a 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -492,6 +492,10 @@
  #define ACRO_BALANCE_PITCH         1.0f
 #endif
 
+#ifndef ACRO_EXPO_DEFAULT
+ #define ACRO_EXPO_DEFAULT          0.3f
+#endif
+
 // Stabilize (angle controller) gains
 #ifndef STABILIZE_ROLL_P
  # define STABILIZE_ROLL_P          4.5f
diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde
index 4e8f2b804b..0b928adb42 100644
--- a/ArduCopter/control_acro.pde
+++ b/ArduCopter/control_acro.pde
@@ -44,10 +44,6 @@ static void acro_run()
 // returns desired angle rates in centi-degrees-per-second
 static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
 {
-    // expo variables
-    static float _expo_factor = 0.3;
-    float rp_in, rp_in3, rp_out;
-    
     float rate_limit;
     Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
 
@@ -55,17 +51,28 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
     roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
     pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
     
-    // calculate rate requests
-    rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
-    rp_in3 = rp_in*rp_in*rp_in;
-    rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in);
-    rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
+    // calculate roll, pitch rate requests
+    if (g.acro_expo <= 0) {
+        rate_bf_request.x = roll_in * g.acro_rp_p;
+        rate_bf_request.y = pitch_in * g.acro_rp_p;
+    } else {
+        // expo variables
+        float rp_in, rp_in3, rp_out;
 
-    rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
-    rp_in3 = rp_in*rp_in*rp_in;
-    rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in);
-    rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
-    
+        // roll expo
+        rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
+        rp_in3 = rp_in*rp_in*rp_in;
+        rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
+        rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
+
+        // pitch expo
+        rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
+        rp_in3 = rp_in*rp_in*rp_in;
+        rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
+        rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
+    }
+
+    // calculate yaw rate request
     rate_bf_request.z = yaw_in * g.acro_yaw_p;
 
     // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode