From feb75f08cba0d18267f9d463db49f7c1db310596 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 22:07:27 +0200 Subject: wip, clean up pid in fw att --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 ++++++++------- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 14 +++++++------- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 16 +++++++++------- 3 files changed, 24 insertions(+), 21 deletions(-) (limited to 'src/lib/ecl/attitude_fw') diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index eb1031cd3..5b8897eaa 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -122,8 +122,8 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; /* input conditioning */ if (!isfinite(airspeed)) { @@ -141,13 +141,12 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _rate_error = _bodyrate_setpoint - pitch_bodyrate; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -162,8 +161,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; + + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables (k_d <--> k_p) + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index ab3ac0a9d..01a114d59 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -92,8 +92,8 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; //xxx: param /* input conditioning */ if (!isfinite(airspeed)) { @@ -113,13 +113,12 @@ float ECL_RollController::control_bodyrate(float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -136,8 +135,9 @@ float ECL_RollController::control_bodyrate(float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e56a8d08d..5c9cc820f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -106,8 +106,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; + /* input conditioning */ if (!isfinite(airspeed)) { @@ -127,13 +128,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -148,8 +148,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables (k_d <--> k_p) + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } -- cgit v1.2.3