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;
+}