diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_controller.cpp')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_controller.cpp | 12 |
1 files changed, 12 insertions, 0 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; +} |