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

fw att control: change control surface deflection scaling

This commit is contained in:
Thomas Gubler 2014-08-15 18:32:51 +02:00
parent e9b0ee7501
commit da0e3169f2
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt; float id = _rate_error * dt * scaler;
/* /*
* anti-windup: do not allow integrator to increase if actuator is at limit * anti-windup: do not allow integrator to increase if actuator is at limit
@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */ /* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed _last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
+ integrator_constrained; //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("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); // warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);

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

@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt; float id = _rate_error * dt * scaler;
/* /*
* anti-windup: do not allow integrator to increase if actuator is at limit * anti-windup: do not allow integrator to increase if actuator is at limit
@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */ /* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed _last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }