AP_Math: Replace safe_asin() by template function

libraries/AP_Math/AP_Math.cpp

@@ -1,23 +1,26 @@
 #include "AP_Math.h"
 #include <float.h>
 
-// a varient of asin() that checks the input ranges and ensures a
-// valid angle as output. If nan is given as input then zero is
-// returned.
-float safe_asin(float v)
+template <class T>
+float safe_asin(const T v)
 {
-    if (isnan(v)) {
+    if (isnan(static_cast<float>(v))) {
         return 0.0f;
     }
     if (v >= 1.0f) {
-        return M_PI/2;
+        return static_cast<float>(M_PI_2);
     }
     if (v <= -1.0f) {
-        return -M_PI/2;
+        return static_cast<float>(-M_PI_2);
     }
-    return asinf(v);
+    return asinf(static_cast<float>(v));
 }
 
+template float safe_asin<int>(const int v);
+template float safe_asin<short>(const short v);
+template float safe_asin<float>(const float v);
+template float safe_asin<double>(const double v);
+
 // a varient of sqrt() that checks the input ranges and ensures a
 // valid value as output. If a negative number is given then 0 is
 // returned. The reasoning is that a negative number for sqrt() in our

libraries/AP_Math/AP_Math.h

@@ -30,8 +30,13 @@ static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf
 // is a float is zero
 static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILON ? true : false; }
 
-// a varient of asin() that always gives a valid answer.
-float           safe_asin(float v);
+
+/*
+ * A variant of asin() that checks the input ranges and ensures a valid angle
+ * as output. If nan is given as input then zero is returned.
+ */
+template <class T>
+float safe_asin(const T v);
 
 // a varient of sqrt() that always gives a valid answer.
 float           safe_sqrt(float v);