APM_Control: Added integrator limiting adjustable by an advanced user parameter

this is compatible with the old IMAX settings

libraries/APM_Control/AP_PitchController.cpp

@@ -75,6 +75,14 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
 	// @User: User
 	AP_GROUPINFO("RLL",      6, AP_PitchController, _roll_ff,        1.0f),
 
+	// @Param: IMAX
+	// @DisplayName: Integrator limit
+	// @Description: This limits the number of centi-degrees of elevator over which the integrator will operate. At the default setting of 1500 centi-degrees, the integrator will be limited to +- 15 degrees of servo travel. The maximum servo deflection is +- 45 degrees, so the default value represents a 1/3rd of the total control throw which is adequate for most aircraft unless they are severely out of trim or have very limited elevator control effectiveness.
+	// @Range: 0 4500
+	// @Increment: 1
+	// @User: Advanced
+	AP_GROUPINFO("IMAX",      7, AP_PitchController, _imax,        1500),
+
 	AP_GROUPEND
 };
 
@@ -185,6 +193,12 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
 	} else {
 		_integrator = 0;
 	}
+
+    // Scale the integration limit
+    float intLimScaled = float(_imax) / scaler;
+
+    // Constrain the integrator state
+    _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
 	
 	// Calculate the demanded control surface deflection
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward

libraries/APM_Control/AP_PitchController.h

@@ -29,6 +29,7 @@ private:
 	AP_Int16 _max_rate_pos;
 	AP_Int16 _max_rate_neg;
 	AP_Float _roll_ff;
+    AP_Int16  _imax;
 	uint32_t _last_t;
 	float _last_out;
 	

libraries/APM_Control/AP_RollController.cpp

@@ -57,6 +57,14 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
 	// @User: Advanced
 	AP_GROUPINFO("RMAX",   4, AP_RollController, _max_rate,       0),
 
+	// @Param: IMAX
+	// @DisplayName: Integrator limit
+	// @Description: This limits the number of degrees of aileron in centi-dgrees over which the integrator will operate. At the default setting of 1500 centi-degrees, the integrator will be limited to +- 15 degrees of servo travel. The maximum servo deflection is +- 45 centi-degrees, so the default value represents a 1/3rd of the total control throw which is adequate unless the aircraft is severely out of trim.
+	// @Range: 0 4500
+	// @Increment: 1
+	// @User: Advanced
+	AP_GROUPINFO("IMAX",      5, AP_RollController, _imax,        1500),
+
 	AP_GROUPEND
 };
 
@@ -136,6 +144,12 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
 		_integrator = 0;
 	}
 	
+    // Scale the integration limit
+    float intLimScaled = float(_imax) / scaler;
+
+    // Constrain the integrator state
+    _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
+	
 	// Calculate the demanded control surface deflection
 	// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
 	// path, but want a 1/speed^2 scaler applied to the rate error path. 

libraries/APM_Control/AP_RollController.h

@@ -27,6 +27,7 @@ private:
 	AP_Float _K_I;
 	AP_Float _K_D;
 	AP_Int16 _max_rate;
+    AP_Int16  _imax;
 	uint32_t _last_t;
 	float _last_out;
 

libraries/APM_Control/AP_YawController.cpp

@@ -45,6 +45,14 @@ const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = {
 	// @Increment: 0.05
 	AP_GROUPINFO("RLL",   3, AP_YawController, _K_FF,   1),
 
+	// @Param: IMAX
+	// @DisplayName: Integrator limit
+	// @Description: This limits the number of degrees of rudder over which the integrator will operate. At the default setting of 15 degrees, the integrator will be limited to +- 15 degrees of servo travel. The maximum servo deflection is +- 45 degrees, so the default value represents a 1/3rd of the total control throw which is adequate for most aircraft unless they are severely out of trim or have very limited rudder control effectiveness.
+	// @Range: 0 45
+	// @Increment: 1
+	// @User: Advanced
+	AP_GROUPINFO("IMAX",      4, AP_YawController, _imax,        15),
+
 	AP_GROUPEND
 };
 
@@ -119,6 +127,12 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stabilize, int16_t as
 		_integrator = 0;
 	}
 	
+    // Scale the integration limit
+    float intLimScaled = float(_imax) / (_K_D * scaler * scaler);
+
+    // Constrain the integrator state
+    _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
+	
 	// Protect against increases to _K_D during in-flight tuning from creating large control transients
 	// due to stored integrator values
 	if (_K_D > _K_D_last && _K_D > 0) {

libraries/APM_Control/AP_YawController.h

@@ -30,6 +30,7 @@ private:
 	AP_Float _K_I;
 	AP_Float _K_D;
 	AP_Float _K_FF;
+    AP_Int8  _imax;
 	uint32_t _last_t;
 	float _last_error;
 	float _last_out;