diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 07:41:06 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-15 10:13:49 +0100 |
commit | e5e42650c446e3d75dd9c23a8fc4e9eab6b65135 (patch) | |
tree | ef244e6d796f5edc1e86b997b459559555417f75 /src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | |
parent | edc5f8a05723b91bff7975dd4a5f60c62b275809 (diff) | |
download | px4-firmware-e5e42650c446e3d75dd9c23a8fc4e9eab6b65135.tar.gz px4-firmware-e5e42650c446e3d75dd9c23a8fc4e9eab6b65135.tar.bz2 px4-firmware-e5e42650c446e3d75dd9c23a8fc4e9eab6b65135.zip |
run code style fixer tool on ecl attitude lib
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 46 |
1 files changed, 29 insertions, 17 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index add884146..ca454fa62 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -61,11 +61,12 @@ ECL_PitchController::~ECL_PitchController() float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) { float roll = ctl_data.roll; + /* Do not calculate control signal with bad inputs */ if (!(isfinite(ctl_data.pitch_setpoint) && - isfinite(roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.airspeed))) { + isfinite(roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; @@ -78,6 +79,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da if (fabsf(roll) < math::radians(90.0f)) { /* not inverted, but numerically still potentially close to infinity */ roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); + } else { /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ @@ -85,6 +87,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da if (roll > 0.0f) { /* right hemisphere */ roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); + } else { /* left hemisphere */ roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); @@ -94,9 +97,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da /* calculate the offset in the rate resulting from rolling */ //xxx needs explanation and conversion to body angular rates or should be removed float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) * - tanf(roll) * sinf(roll)) * _roll_ff; - if (inverted) + tanf(roll) * sinf(roll)) * _roll_ff; + + if (inverted) { turn_offset = -turn_offset; + } /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; @@ -108,9 +113,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da _rate_setpoint += turn_offset; /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { if (_rate_setpoint > 0.0f) { _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + } else { _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; } @@ -124,13 +131,13 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + isfinite(ctl_data.pitch) && + isfinite(ctl_data.pitch_rate) && + isfinite(ctl_data.yaw_rate) && + isfinite(ctl_data.yaw_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); } @@ -142,25 +149,29 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da /* lock integral for long intervals */ bool lock_integrator = ctl_data.lock_integrator; - if (dt_micros > 500000) + + 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 * (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 (jacobian) */ _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; /* Transform estimation to body angular rates (jacobian) */ float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate; + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate; _rate_error = _bodyrate_setpoint - pitch_bodyrate; @@ -174,6 +185,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); @@ -188,8 +200,8 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da /* Apply PI rate controller and store non-limited output */ _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + - _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler - + integrator_constrained; //scaler is proportional to 1/airspeed + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); |