From 147856e73cb537e94ca2698dc08b5db6425aa832 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 11 Jun 2013 18:35:31 +1000 Subject: [PATCH] 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 | 14 ++++++++++++++ libraries/APM_Control/AP_PitchController.h | 1 + libraries/APM_Control/AP_RollController.cpp | 14 ++++++++++++++ libraries/APM_Control/AP_RollController.h | 1 + libraries/APM_Control/AP_YawController.cpp | 14 ++++++++++++++ libraries/APM_Control/AP_YawController.h | 1 + 6 files changed, 45 insertions(+) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index c2f520b268..8df8cabc4a 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/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 diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 197b09bd1a..f6952bc9e4 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/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; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index d8871ee183..c4f19029be 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/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. diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index f03b7fd04d..58af6dba84 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/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; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index b5cc4f6c71..696e09a66f 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/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) { diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 0a8cf1d620..31f75eb368 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/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;