diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_controller.cpp | 12 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_controller.h | 1 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 17 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 14 |
4 files changed, 19 insertions, 25 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 46140fbfd..9cd08a50d 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -49,6 +49,7 @@ #include "ecl_controller.h" #include <stdio.h> +#include <mathlib/mathlib.h> ECL_Controller::ECL_Controller(const char *name) : _last_run(0), @@ -126,3 +127,14 @@ float ECL_Controller::get_desired_bodyrate() { return _bodyrate_setpoint; } + +float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { + float airspeed_result = airspeed; + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (minspeed + maxspeed); + } else if (airspeed < minspeed) { + airspeed = minspeed; + } + return airspeed_result; +} diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index 254052964..ac1ac88d0 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -119,4 +119,5 @@ protected: perf_counter_t _nonfinite_input_perf; static const uint8_t _perf_name_max = 40; char _perf_name[_perf_name_max]; + float constrain_airspeed(float airspeed, float minspeed, float maxspeed); }; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index ca454fa62..c80eb357c 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -94,9 +94,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da } } + /* input conditioning */ + float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); /* 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) * + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; if (inverted) { @@ -154,17 +156,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da 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; @@ -175,7 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _rate_error = _bodyrate_setpoint - pitch_bodyrate; - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f) { float id = _rate_error * dt * ctl_data.scaler; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 5d0846ac3..160dc5cad 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -106,17 +106,6 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat 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 = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; @@ -126,7 +115,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f) { float id = _rate_error * dt * ctl_data.scaler; @@ -157,3 +146,4 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat return math::constrain(_last_output, -1.0f, 1.0f); } + |