From 146279b20fc65705e31e03298a1d3eea8911d0f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 15:03:11 +0200 Subject: wip fw ctrl, several small bugfixes, set limit to 1 --- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) (limited to 'src/lib/ecl/attitude_fw/ecl_roll_controller.cpp') diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index f3909593a..e5bd7b4f7 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) { } @@ -96,6 +95,7 @@ float ECL_RollController::control_bodyrate(float pitch, 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); @@ -120,10 +120,10 @@ float ECL_RollController::control_bodyrate(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); } @@ -133,12 +133,13 @@ float ECL_RollController::control_bodyrate(float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + 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^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 + warnx("roll: _last_output %.4f", (double)_last_output); - 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() -- cgit v1.2.3