Copter: autotune disables rate output from stab controller

ArduCopter/ArduCopter.pde

@@ -396,6 +396,8 @@ static union {
         uint8_t CH8_flag            : 2; // 13,14   // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high
         uint8_t usb_connected       : 1; // 15      // true if APM is powered from USB connection
         uint8_t yaw_stopped         : 1; // 16      // Used to manage the Yaw hold capabilities
+
+        uint8_t disable_stab_rate_limit : 1; // 17  // disables limits rate request from the stability controller
     };
     uint32_t value;
 } ap;

ArduCopter/Attitude.pde

@@ -31,12 +31,14 @@ get_stabilize_roll(int32_t target_angle)
     // angle error
     target_angle = wrap_180_cd(target_angle - ahrs.roll_sensor);
 
-    // limit the error we're feeding to the PID
-    target_angle = constrain_int32(target_angle, -g.angle_max, g.angle_max);
-
     // convert to desired rate
     int32_t target_rate = g.pi_stabilize_roll.kP() * target_angle;
 
+    // constrain the target rate
+    if (!ap.disable_stab_rate_limit) {
+        target_rate = constrain_int32(target_rate, -STABILIZE_RATE_LIMIT, STABILIZE_RATE_LIMIT);
+    }
+
     // set targets for rate controller
     set_roll_rate_target(target_rate, EARTH_FRAME);
 }
@@ -47,12 +49,14 @@ get_stabilize_pitch(int32_t target_angle)
     // angle error
     target_angle            = wrap_180_cd(target_angle - ahrs.pitch_sensor);
 
-    // limit the error we're feeding to the PID
-    target_angle            = constrain_int32(target_angle, -g.angle_max, g.angle_max);
-
     // convert to desired rate
     int32_t target_rate = g.pi_stabilize_pitch.kP() * target_angle;
 
+    // constrain the target rate
+    if (!ap.disable_stab_rate_limit) {
+        target_rate = constrain_int32(target_rate, -STABILIZE_RATE_LIMIT, STABILIZE_RATE_LIMIT);
+    }
+
     // set targets for rate controller
     set_pitch_rate_target(target_rate, EARTH_FRAME);
 }

ArduCopter/auto_tune.pde

@@ -108,6 +108,7 @@ void auto_tune_restore_orig_pids()
     g.pid_rate_pitch.kI(orig_pitch_ri);
     g.pid_rate_pitch.kD(orig_pitch_rd);
     g.pi_stabilize_pitch.kP(orig_pitch_sp);
+    ap.disable_stab_rate_limit = false;
 }
 
 // auto_tune_load_tuned_pids - restore pids to their original values
@@ -122,6 +123,7 @@ void auto_tune_load_tuned_pids()
         g.pid_rate_pitch.kI(tune_pitch_rp*AUTO_TUNE_RP_RATIO_FINAL);
         g.pid_rate_pitch.kD(tune_pitch_rd);
         g.pi_stabilize_pitch.kP(tune_pitch_sp);
+        ap.disable_stab_rate_limit = false;
     }
 }
 
@@ -164,6 +166,7 @@ void auto_tune_stop()
     if (auto_tune_state.enabled) {
         auto_tune_state.enabled = false;
         auto_tune_state.active = false;
+        ap.disable_stab_rate_limit = false;
         if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
             set_roll_pitch_mode(ROLL_PITCH_STABLE); // restore roll-pitch mode
             rate_targets_frame = EARTH_FRAME;   // regular stabilize mode frame
@@ -260,6 +263,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
                         g.pid_rate_pitch.kD(tune_pitch_rd);
                         g.pi_stabilize_pitch.kP(tune_pitch_sp);
                     }
+                    ap.disable_stab_rate_limit = true;  // disable rate limits
                 }
                 break;
 
@@ -376,7 +380,6 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
 
             case AUTO_TUNE_STEP_UPDATE_GAINS:
                 // restore pids to their original values
-                //auto_tune_restore_orig_pids();
                 g.pid_rate_roll.kP(orig_roll_rp);
                 g.pid_rate_roll.kI(orig_roll_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING);
                 g.pid_rate_roll.kD(orig_roll_rd);
@@ -386,6 +389,9 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
                 g.pid_rate_pitch.kD(orig_pitch_rd);
                 g.pi_stabilize_pitch.kP(orig_pitch_sp);
 
+                // re-enable the rate limits
+                ap.disable_stab_rate_limit = false;
+
                 // logging
                 if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
                     Log_Write_AutoTune(auto_tune_state.axis, auto_tune_state.tune_type, auto_tune_test_min, auto_tune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp);

ArduCopter/config.h

@@ -785,6 +785,10 @@
  # define STABILIZE_PITCH_IMAX   	0
 #endif
 
+#ifndef STABILIZE_RATE_LIMIT
+ # define STABILIZE_RATE_LIMIT      18000
+#endif
+
 #ifndef  STABILIZE_YAW_P
  # define STABILIZE_YAW_P           4.5f            // increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif