diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-13 11:05:22 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-13 11:05:22 +0100 |
| commit | 38172497c116548320c74696d795f9198e0bf4e4 (patch) | |
| tree | a3b446c3c503c18d4d4a25be71a90553bd8b737d /src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | |
| parent | 8e92d47de1d29ca3461911483361809697bc236a (diff) | |
| download | px4-firmware-38172497c116548320c74696d795f9198e0bf4e4.tar.gz px4-firmware-38172497c116548320c74696d795f9198e0bf4e4.tar.bz2 px4-firmware-38172497c116548320c74696d795f9198e0bf4e4.zip | |
reintroduce feedforward
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 |
1 files changed, 2 insertions, 1 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a4ecc48a2..7c366aaf2 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -51,6 +51,7 @@ ECL_YawController::ECL_YawController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _roll_ff(0.0f), @@ -159,7 +160,7 @@ float ECL_YawController::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 = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _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); |