AP_Math: Replace safe_sqrt() by template function

libraries/AP_Math/AP_Math.cpp

@@ -21,20 +21,21 @@ 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
+template <class T>
-// valid value as output. If a negative number is given then 0 is
+float safe_sqrt(const T v)
-// returned. The reasoning is that a negative number for sqrt() in our
-// code is usually caused by small numerical rounding errors, so the
-// real input should have been zero
-float safe_sqrt(float v)
 {
-    float ret = sqrtf(v);
+    float ret = sqrtf(static_cast<float>(v));
     if (isnan(ret)) {
         return 0;
     }
     return ret;
 }
 
+template float safe_sqrt<int>(const int v);
+template float safe_sqrt<short>(const short v);
+template float safe_sqrt<float>(const float v);
+template float safe_sqrt<double>(const double v);
+
 /*
   linear interpolation based on a variable in a range
  */

libraries/AP_Math/AP_Math.h

@@ -38,8 +38,14 @@ static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILO
 template <class T>
 float safe_asin(const T v);
 
-// a varient of sqrt() that always gives a valid answer.
+/*
-float           safe_sqrt(float v);
+ * A variant 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 code is usually caused by small
+ * numerical rounding errors, so the real input should have been zero
+ */
+template <class T>
+float safe_sqrt(const T v);
 
 // return determinant of square matrix
 float                   detnxn(const float C[], const uint8_t n);