dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

manual merge fw_integrator_fix_ouputlimit into fw_staging_ouputlimit

This commit is contained in:
Thomas Gubler 2013-10-24 18:34:22 +02:00
parent bff3bf8d87 1e89f30120
commit 01d8ca1fba
4 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -160,10 +160,11 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
}
/* integrator limit */
_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);

8
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -132,12 +132,12 @@ float ECL_RollController::control_bodyrate(float pitch,
}
/* integrator limit */
_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
// warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i);
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
// warnx("roll: _last_output %.4f", (double)_last_output);
_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}

5
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -146,10 +146,11 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
}
/* integrator limit */
_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);

6
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 1000.0f);
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
// @DisplayName Roll feedforward gain.
// @Description This compensates during turns and ensures the nose stays level.
@ -119,7 +119,7 @@ PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 1000.0f);
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
// @DisplayName Maximum Roll Rate
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@ -130,7 +130,7 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 1000.0f);
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);