aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: Added integrator limiting adjustable by an advanced user parameter

Browse Source 
this is compatible with the old IMAX settings
This commit is contained in:
Paul Riseborough 2013-06-11 18:35:31 +10:00 committed by Andrew Tridgell
parent d8dab7b5c7
commit 147856e73c
6 changed files with 45 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/APM_Control/AP_PitchController.cpp
View File

@ -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

1
libraries/APM_Control/AP_PitchController.h
View File

@ -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;

14
libraries/APM_Control/AP_RollController.cpp
View File

@ -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.

1
libraries/APM_Control/AP_RollController.h
View File

@ -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;

14
libraries/APM_Control/AP_YawController.cpp
View File

@ -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) {

1
libraries/APM_Control/AP_YawController.h
View File

@ -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;