diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-20 20:10:31 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-24 18:14:54 +0200 |
commit | a5046fdfa02999f153502168682ade9bd5c15842 (patch) | |
tree | 6c71a9523eff9de5d33bd97e709e013d1c6a9732 /src | |
parent | 19ae09dbad3fb5d2cfa73cf8476cab654e53581f (diff) | |
download | px4-firmware-a5046fdfa02999f153502168682ade9bd5c15842.tar.gz px4-firmware-a5046fdfa02999f153502168682ade9bd5c15842.tar.bz2 px4-firmware-a5046fdfa02999f153502168682ade9bd5c15842.zip |
fix wrong operation in yaw controller
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 |
3 files changed, 9 insertions, 5 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 531512493..626812288 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,6 +45,7 @@ #include <geo/geo.h> #include <ecl/ecl.h> #include <mathlib/mathlib.h> +#include <systemlib/err.h> ECL_PitchController::ECL_PitchController() : _last_run(0), @@ -163,7 +164,8 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* 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("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 e5bd7b4f7..9e2007131 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -133,11 +133,11 @@ 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); +// warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); /* 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); +// 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_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index bc036e082..bd1f318a7 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -119,10 +119,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint * cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian /* Transform estimation to body angular rates */ - float yaw_bodyrate = -sinf(roll) * pitch_rate * cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error @@ -150,6 +150,8 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* 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("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); + return math::constrain(_last_output, -1.0f, 1.0f); } |