diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-19 08:50:57 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-19 08:50:57 +0200 |
| commit | cc98c6deff8c9977f3faf403f42b6be46322d359 (patch) | |
| tree | d8055fb7e7f82654b0d272ff560404f00a6da6e7 /src | |
| parent | de7d9c5b95a75f01f5731c79a07156a9bf37684b (diff) | |
| parent | da0e3169f2796d3223b4f7857da067e9aac2fea4 (diff) | |
| download | px4-firmware-cc98c6deff8c9977f3faf403f42b6be46322d359.tar.gz px4-firmware-cc98c6deff8c9977f3faf403f42b6be46322d359.tar.bz2 px4-firmware-cc98c6deff8c9977f3faf403f42b6be46322d359.zip | |
Merge pull request #1277 from PX4/fwattvscale
fw att control: change control surface deflection scaling
Diffstat (limited to 'src')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 | ||||
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 |
2 files changed, 8 insertions, 4 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 46db788a6..926a8db2a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -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) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * 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); /* 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("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9894a34d7..94bd26f03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch, 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 @@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* 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); } |