diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 84 |
1 files changed, 40 insertions, 44 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index fe03b8065..82ba2c6c7 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,45 +47,43 @@ #include <systemlib/err.h> ECL_YawController::ECL_YawController() : - _last_run(0), - _k_p(0.0f), - _k_i(0.0f), - _k_ff(0.0f), - _integrator_max(0.0f), - _max_rate(0.0f), - _last_output(0.0f), - _integrator(0.0f), - _rate_error(0.0f), - _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _coordinated_min_speed(1.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + ECL_Controller("yaw"), + _coordinated_min_speed(1.0f) { } ECL_YawController::~ECL_YawController() { - perf_free(_nonfinite_input_perf); } -float ECL_YawController::control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_v, float speed_body_w, - float roll_rate_setpoint, float pitch_rate_setpoint) +float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && - isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && - isfinite(pitch_rate_setpoint))) { + if (!(isfinite(ctl_data.roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.speed_body_u) && + isfinite(ctl_data.speed_body_v) && + isfinite(ctl_data.speed_body_w) && + isfinite(ctl_data.roll_rate_setpoint) && + isfinite(ctl_data.pitch_rate_setpoint))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } + // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; - if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { - float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v + + ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) { + float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) + + ctl_data.speed_body_w * sinf(ctl_data.pitch)); + if(fabsf(denumerator) > FLT_EPSILON) { - _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + _rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint + + 9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) + + ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) / + denumerator; + // warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } @@ -111,46 +109,49 @@ float ECL_YawController::control_attitude(float roll, float pitch, return _rate_setpoint; } -float ECL_YawController::control_bodyrate(float roll, float pitch, - float pitch_rate, float yaw_rate, - float pitch_rate_setpoint, - float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && - isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && - isfinite(airspeed_max) && isfinite(scaler))) { + if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && + isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; if (dt_micros > 500000) lock_integrator = true; /* input conditioning */ + float airspeed = ctl_data.airspeed; if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (airspeed_min + airspeed_max); - } else if (airspeed < airspeed_min) { - airspeed = airspeed_min; + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; } - /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint; - /* Transform estimation to body angular rates */ - float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + /* Transform estimation to body angular rates (jacobian) */ + float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate + + cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate; /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { float id = _rate_error * dt; @@ -173,14 +174,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * ctl_data.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); } - -void ECL_YawController::reset_integrator() -{ - _integrator = 0.0f; -} |