From fc67f519011e75f6f490ecb2ab7d46abd55aff71 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 11 Jul 2013 08:05:25 +1000
Subject: [PATCH] Plane: added ACRO_ROLL_RATE and ACRO_PITCH_RATE parameters

default to 180 degrees/second, which seems reasonable
---
 ArduPlane/Attitude.pde   | 41 ++++++++++++++++++++++++++++++++++++----
 ArduPlane/Parameters.h   |  4 ++++
 ArduPlane/Parameters.pde | 18 ++++++++++++++++++
 3 files changed, 59 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index 818371f1f1..675e96fc28 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -236,22 +236,34 @@ static void stabilize_training(float speed_scaler)
  */
 static void stabilize_acro(float speed_scaler)
 {
-    float roll_rate = channel_roll->norm_input() * 180;
-    float pitch_rate = channel_pitch->norm_input() * 180;
+    float roll_rate = channel_roll->norm_input() * g.acro_roll_rate;
+    float pitch_rate = channel_pitch->norm_input() * g.acro_pitch_rate;
 
+    /*
+      check for special roll handling near the pitch poles
+     */
     if (roll_rate == 0 && 
         acro_state.locked_roll && 
         (ahrs.pitch_sensor > 7000 ||
          ahrs.pitch_sensor < -7000)) {
         // when near the poles do rate holding, but don't unlock the
-        // desired roll
+        // desired roll.
         channel_roll->servo_out  = g.rollController.get_rate_out(roll_rate,  speed_scaler);
     } else if (roll_rate == 0) {
+        /*
+          we have no roll stick input, so we will enter "roll locked"
+          mode, and hold the roll we had when the stick was released
+         */
         if (!acro_state.locked_roll) {
             acro_state.locked_roll = true;
             acro_state.locked_roll_cd = ahrs.roll_sensor;
         }
-        // try to cope with wrap while looping.
+        /* 
+           special handling for the roll inversion while
+           looping. Using euler angles for this is ugly, but it fits
+           with the rest of the APM code, and actually works
+           surprisingly well
+         */
         int32_t roll_error_cd = ahrs.roll_sensor - acro_state.locked_roll_cd;
         if (roll_error_cd > 13500 && roll_error_cd < 21500) {
             acro_state.locked_roll_cd += 18000;
@@ -261,6 +273,11 @@ static void stabilize_acro(float speed_scaler)
         }
         acro_state.locked_roll_cd = wrap_180_cd(acro_state.locked_roll_cd);
         nav_roll_cd = acro_state.locked_roll_cd;
+
+        /* 
+           now we need to prevent the roll controller from seeing a
+           jump as roll passes through the -180 -> 180 point 
+        */
         roll_error_cd = ahrs.roll_sensor - nav_roll_cd;
         if (roll_error_cd > 31500) {
             nav_roll_cd += 36000;
@@ -269,11 +286,19 @@ static void stabilize_acro(float speed_scaler)
         }
         stabilize_roll(speed_scaler);
     } else {
+        /*
+          aileron stick is non-zero, use pure rate control until the
+          user releases the stick
+         */
         acro_state.locked_roll = false;
         channel_roll->servo_out  = g.rollController.get_rate_out(roll_rate,  speed_scaler);
     }
 
     if (pitch_rate == 0) {
+        /*
+          user has zero pitch stick input, so we lock pitch at the
+          point they release the stick
+         */
         if (!acro_state.locked_pitch) {
             acro_state.locked_pitch = true;
             acro_state.locked_pitch_cd = ahrs.pitch_sensor;
@@ -281,10 +306,18 @@ static void stabilize_acro(float speed_scaler)
         nav_pitch_cd = acro_state.locked_pitch_cd;
         stabilize_pitch(speed_scaler);
     } else {
+        /*
+          user has non-zero pitch input, use a pure rate controller
+         */
         acro_state.locked_pitch = false;
         channel_pitch->servo_out = g.pitchController.get_rate_out(pitch_rate, speed_scaler);
     }
 
+    /*
+      call the normal yaw stabilize for now. This allows for manual
+      rudder input, plus automatic coordinated turn handling. For
+      knife-edge we'll need to do something quite different
+     */
     stabilize_yaw(speed_scaler);
 }
 
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index bdd5698067..2cfd62f926 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -110,6 +110,8 @@ public:
         k_param_flybywire_elev_reverse,
         k_param_alt_control_algorithm,
         k_param_flybywire_climb_rate,
+        k_param_acro_roll_rate,
+        k_param_acro_pitch_rate,
 
         //
         // 130: Sensor parameters
@@ -329,6 +331,8 @@ public:
     //
     AP_Int16 roll_limit_cd;
     AP_Int16 alt_offset;
+    AP_Int16 acro_roll_rate;
+    AP_Int16 acro_pitch_rate;
 
     // Misc
     //
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 1bc613af38..daaf9a389c 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -465,6 +465,24 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     ASCALAR(pitch_limit_min_cd,     "LIM_PITCH_MIN",  PITCH_MIN_CENTIDEGREE),
 
+    // @Param: ACRO_ROLL_RATE
+    // @DisplayName: ACRO mode roll rate
+    // @Description: The maximum roll rate at full stick deflection in ACRO mode
+    // @Units: degrees/second
+    // @Range: 10 500
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(acro_roll_rate,          "ACRO_ROLL_RATE",    180),
+
+    // @Param: ACRO_PITCH_RATE
+    // @DisplayName: ACRO mode pitch rate
+    // @Description: The maximum pitch rate at full stick deflection in ACRO mode
+    // @Units: degrees/second
+    // @Range: 10 500
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(acro_pitch_rate,          "ACRO_PITCH_RATE",  180),
+
     // @Param: AUTO_TRIM
     // @DisplayName: Auto trim
     // @Description: Set RC trim PWM levels to current levels when switching away from manual mode