diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_roll_controller.cpp')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 25 |
1 files changed, 12 insertions, 13 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 658a0a501..0772f88bc 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -54,8 +54,7 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _bodyrate_setpoint(0.0f) { } @@ -92,10 +91,11 @@ 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 */ +// warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (airspeed_min + airspeed_max); @@ -113,18 +113,17 @@ 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) { + 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); } @@ -134,13 +133,13 @@ float ECL_RollController::control_bodyrate(float pitch, /* integrator limit */ //xxx: until start detection is available: integral part in control signal is limited here - float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); + float integrator_constrained = math::constrain(_integrator * _k_i, -_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_constrained * scaler + (_rate_setpoint * k_ff)) * scaler; + /* 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 - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_RollController::reset_integrator() |