diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-20 15:03:11 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-24 18:08:23 +0200 |
| commit | 146279b20fc65705e31e03298a1d3eea8911d0f8 (patch) | |
| tree | 846eaf2859548eabc100a08393f9bad6e9480de5 /src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | |
| parent | 3c14483df247fb842cabdbb02206912f7f4350cf (diff) | |
| download | px4-firmware-146279b20fc65705e31e03298a1d3eea8911d0f8.tar.gz px4-firmware-146279b20fc65705e31e03298a1d3eea8911d0f8.tar.bz2 px4-firmware-146279b20fc65705e31e03298a1d3eea8911d0f8.zip | |
wip fw ctrl, several small bugfixes, set limit to 1
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 9 |
1 files changed, 4 insertions, 5 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 84d9ebd88..bc036e082 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -54,7 +54,6 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)), _coordinated(1.0f) { @@ -135,10 +134,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -150,9 +149,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_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^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_YawController::reset_integrator() |