From 93fa3fbc6942798e603203d2b3fca8cdfea070bf Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Mon, 10 Dec 2012 22:27:46 +0900
Subject: [PATCH] ArduCopter: added ACRO_TRAINER parameter to allow
 enabling/disabling the acro training function which will bring the roll back
 to within +- 45 degrees

---
 ArduCopter/Attitude.pde   | 4 ++--
 ArduCopter/Parameters.h   | 6 ++++--
 ArduCopter/Parameters.pde | 7 +++++++
 ArduCopter/config.h       | 4 ++++
 4 files changed, 17 insertions(+), 4 deletions(-)

diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 75e84e6c88..6ff75bb87f 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -130,7 +130,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
     roll_axis = wrap_180(roll_axis);
 
     // ensure that we don't reach gimbal lock
-    if (roll_axis > 4500 || roll_axis < -4500) {
+    if (labs(roll_axis > 4500) && g.acro_trainer_enabled) {
         roll_axis	= constrain(roll_axis, -4500, 4500);
         angle_error = wrap_180(roll_axis - ahrs.roll_sensor);
     } else {
@@ -166,7 +166,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
     pitch_axis = wrap_180(pitch_axis);
 
     // ensure that we don't reach gimbal lock
-    if (pitch_axis > 4500 || pitch_axis < -4500) {
+    if (labs(pitch_axis) > 4500) {
         pitch_axis	= constrain(pitch_axis, -4500, 4500);
         angle_error = wrap_180(pitch_axis - ahrs.pitch_sensor);
     } else {
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 0cc6fdc768..2fc815b2ba 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -73,6 +73,7 @@ public:
         k_param_rssi_pin,
         k_param_throttle_accel_enabled,
         k_param_yaw_override_behaviour,
+        k_param_acro_trainer_enabled,   // 27
 
         // 65: AP_Limits Library
         k_param_limits = 65,
@@ -226,8 +227,8 @@ public:
         k_param_pid_throttle,
         k_param_pid_optflow_roll,
         k_param_pid_optflow_pitch,
-        k_param_acro_balance_roll,
-        k_param_acro_balance_pitch,
+        k_param_acro_balance_roll,      // scalar (not PID)
+        k_param_acro_balance_pitch,     // scalar (not PID)
         k_param_pid_throttle_accel, // 241
 
         // 254,255: reserved
@@ -354,6 +355,7 @@ public:
     AP_Float                acro_p;
     AP_Int16                acro_balance_roll;
     AP_Int16                acro_balance_pitch;
+    AP_Int8                 acro_trainer_enabled;
 
     // PI/D controllers
     AC_PID                  pid_rate_roll;
diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index 6feab241e4..6325a6eb5b 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -499,6 +499,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(acro_balance_pitch,     "ACRO_BAL_PITCH",   ACRO_BALANCE_PITCH),
 
+    // @Param: ACRO_TRAINER
+    // @DisplayName: Acro Trainer Enabled
+    // @Description: Set to 1 (Enabled) to make roll return to within 45 degrees of level automatically
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    GSCALAR(acro_trainer_enabled,   "ACRO_TRAINER",     ACRO_TRAINER_ENABLED),
+
     // @Param: LED_MODE
     // @DisplayName: Copter LED Mode
     // @Description: bitmap to control the copter led mode
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 3a0fc33990..d9733e6a55 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -781,6 +781,10 @@
  #define ACRO_BALANCE_PITCH			200
 #endif
 
+#ifndef ACRO_TRAINER_ENABLED
+ #define ACRO_TRAINER_ENABLED       ENABLED
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Loiter control gains
 //