AP_Math: Replace the constrain_* functions by a single template

Besides being simpler this reduces ~4k in the binary size for PX4.

libraries/AP_Math/AP_Math.cpp

@@ -131,3 +131,20 @@ template float wrap_2PI<int>(const int radian);
 template float wrap_2PI<short>(const short radian);
 template float wrap_2PI<float>(const float radian);
 template float wrap_2PI<double>(const double radian);
+
+template <class T>
+T constrain_value(const T amt, const T low, const T high)
+{
+    // the check for NaN as a float prevents propogation of floating point
+    // errors through any function that uses constrain_float(). The normal
+    // float semantics already handle -Inf and +Inf
+    if (isnan(amt)) {
+        return (low + high) * 0.5f;
+    }
+    return amt < low ? low : (amt > high ? high : amt);
+}
+
+template int constrain_value<int>(const int amt, const int low, const int high);
+template short constrain_value<short>(const short amt, const short low, const short high);
+template float constrain_value<float>(const float amt, const float low, const float high);
+template double constrain_value<double>(const double amt, const double low, const double high);

libraries/AP_Math/AP_Math.h

@@ -91,28 +91,16 @@ float wrap_PI(const T radian);
 template <class T>
 float wrap_2PI(const T radian);
 
-// constrain a value
+/*
-static inline float constrain_float(float amt, float low, float high)
+ * Constrain a value to be within the range: low and high
-{
+ */
-	// the check for NaN as a float prevents propogation of
+template <class T>
-	// floating point errors through any function that uses
+T constrain_value(const T amt, const T low, const T high);
-	// constrain_float(). The normal float semantics already handle -Inf
-	// and +Inf
-	if (isnan(amt)) {
-		return (low+high)*0.5f;
-	}
-	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
-}
 
-// constrain a int16_t value
+auto const constrain_float = &constrain_value<float>;
-static inline int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) {
+auto const constrain_int16 = &constrain_value<int16_t>;
-	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
+auto const constrain_int32 = &constrain_value<int32_t>;
-}
 
-// constrain a int32_t value
-static inline int32_t constrain_int32(int32_t amt, int32_t low, int32_t high) {
-	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
-}
 
 //matrix algebra
 bool inverse(float x[], float y[], uint16_t dim);