Stabilization patches

removed Angle error limit for stabilization
constricted Iterm to +- 5° error and limited the implementation to when the quad is +- 5° from center
doubled the output limit for Rate controller.
increased default Rate_P gain to .18 with matching Rate_D of .004
Tested in the SIM and in backyard. dramatically increases performance and quad no longer overshoots and flips when pushed hard.

ArduCopter/Attitude.pde

@@ -18,14 +18,18 @@ get_stabilize_roll(int32_t target_angle)
 	return constrain(target_angle, -4500, 4500);
 #else
 
-	// limit the error we're feeding to the PID
-	target_angle 		= constrain(target_angle, -2500, 2500);
-
 	// convert to desired Rate:
 	int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
-	int16_t iterm 		= g.pi_stabilize_roll.get_i(target_angle, G_Dt);
 
-	return get_rate_roll(target_rate) + iterm;
+	int16_t i_stab;
+	if(abs(ahrs.roll_sensor) < 500){
+		target_angle 		= constrain(target_angle, -500, 500);
+		i_stab 				= g.pi_stabilize_roll.get_i(target_angle, G_Dt);
+	}else{
+		i_stab 				= g.pi_stabilize_roll.get_integrator();
+	}
+
+	return get_rate_roll(target_rate) + i_stab;
 #endif
 }
 
@@ -45,14 +49,19 @@ get_stabilize_pitch(int32_t target_angle)
 	// output control:
 	return constrain(target_angle, -4500, 4500);
 #else
-	// limit the error we're feeding to the PID
-	target_angle 		= constrain(target_angle, -2500, 2500);
 
-	// conver to desired Rate:
+	// convert to desired Rate:
 	int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle);
-	int16_t iterm 		= g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
 
-	return get_rate_pitch(target_rate) + iterm;
+	int16_t i_stab;
+	if(abs(ahrs.roll_sensor) < 500){
+		target_angle 		= constrain(target_angle, -500, 500);
+		i_stab 				= g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
+	}else{
+		i_stab 				= g.pi_stabilize_pitch.get_integrator();
+	}
+	return get_rate_pitch(target_rate) + i_stab;
+
 #endif
 }
 
@@ -158,7 +167,7 @@ get_rate_roll(int32_t target_rate)
 	output -= rate_d_dampener;
 
 	// constrain output
-	output = constrain(output, -2500, 2500);
+	output = constrain(output, -5000, 5000);
 
 #if LOGGING_ENABLED == ENABLED
 	static int8_t log_counter = 0;					// used to slow down logging of PID values to dataflash
@@ -209,7 +218,7 @@ get_rate_pitch(int32_t target_rate)
 	output -= rate_d_dampener;
 
 	// constrain output
-	output = constrain(output, -2500, 2500);
+	output = constrain(output, -5000, 5000);
 
 #if LOGGING_ENABLED == ENABLED
 	static int8_t log_counter = 0;					// used to slow down logging of PID values to dataflash
@@ -412,11 +421,15 @@ get_nav_yaw_offset(int yaw_input, int reset)
 
 static int16_t get_angle_boost(int16_t value)
 {
-	float temp = cos_pitch_x * cos_roll_x;
-	temp = 1.0 - constrain(temp, .5, 1.0);
-	int16_t output = temp * value;
-	return constrain(output, 0, 200);
+//	float temp = cos_pitch_x * cos_roll_x;
+//	temp = 1.0 - constrain(temp, .5, 1.0);
+//	int16_t output = temp * value;
+//	return constrain(output, 0, 200);
 //	return (int)(temp * value);
+
+	float temp = cos_pitch_x * cos_roll_x;
+	temp = constrain(temp, .5, 1.0);
+	return ((float)g.throttle_cruise / temp) - g.throttle_cruise;
 }
 
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/config.h

@@ -654,26 +654,26 @@
 // Stabilize Rate Control
 //
 #ifndef RATE_ROLL_P
-# define RATE_ROLL_P        0.14
+# define RATE_ROLL_P        0.18
 #endif
 #ifndef RATE_ROLL_I
 # define RATE_ROLL_I        0.0
 #endif
 #ifndef RATE_ROLL_D
-# define RATE_ROLL_D        0.000 //.002
+# define RATE_ROLL_D        0.004
 #endif
 #ifndef RATE_ROLL_IMAX
 # define RATE_ROLL_IMAX	 	5			// degrees
 #endif
 
 #ifndef RATE_PITCH_P
-# define RATE_PITCH_P       0.14
+# define RATE_PITCH_P       0.18
 #endif
 #ifndef RATE_PITCH_I
 # define RATE_PITCH_I		0.0
 #endif
 #ifndef RATE_PITCH_D
-# define RATE_PITCH_D       0.00
+# define RATE_PITCH_D       0.004
 #endif
 #ifndef RATE_PITCH_IMAX
 # define RATE_PITCH_IMAX   	5			// degrees
@@ -694,7 +694,7 @@
 
 
 #ifndef STABILIZE_D
-# define STABILIZE_D 		0.15
+# define STABILIZE_D 		0.00
 #endif
 
 #ifndef STABILIZE_D_SCHEDULE