AP_Math: Replace is_equal with a type safe template function

It makes sense to consider also other floating point types.

libraries/AC_WPNav/AC_Circle.cpp

@@ -98,7 +98,7 @@ void AC_Circle::init()
 /// set_circle_rate - set circle rate in degrees per second
 void AC_Circle::set_rate(float deg_per_sec)
 {
-    if (!is_equal(deg_per_sec,_rate)) {
+    if (!is_equal(deg_per_sec, _rate.get())) {
         _rate = deg_per_sec;
         calc_velocities(false);
     }

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -792,7 +792,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // flat, but still allow for yaw correction using the
     // accelerometers at high roll angles as long as we have a GPS
     if (AP_AHRS_DCM::use_compass()) {
-        if (have_gps() && is_equal(gps_gain,1.0f)) {
+        if (have_gps() && is_equal(gps_gain.get(), 1.0f)) {
             error[besti].z *= sinf(fabsf(roll));
         } else {
             error[besti].z = 0;

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -227,7 +227,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
         // compute integral error component to converge to a crosstrack of zero when traveling
 		// straight but reset it when disabled or if it changes. That allows for much easier
 		// tuning by having it re-converge each time it changes.
-		if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain, _L1_xtrack_i_gain_prev)) {
+        if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
 		    _L1_xtrack_i = 0;
 		    _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
 		} else if (fabsf(Nu1) < radians(5)) {

libraries/AP_Math/AP_Math.cpp

@@ -1,6 +1,19 @@
 #include "AP_Math.h"
 #include <float.h>
 
+template <class FloatOne, class FloatTwo>
+bool is_equal(const FloatOne v_1, const FloatTwo v_2)
+{
+    static_assert(std::is_arithmetic<FloatOne>::value, "template parameter not of type float or int");
+    static_assert(std::is_arithmetic<FloatTwo>::value, "template parameter not of type float or int");
+    return fabsf(v_1 - v_2) < std::numeric_limits<decltype(v_1 - v_2)>::epsilon();
+}
+
+template bool is_equal<int>(const int v_1, const int v_2);
+template bool is_equal<short>(const short v_1, const short v_2);
+template bool is_equal<float>(const float v_1, const float v_2);
+template bool is_equal<double>(const double v_1, const double v_2);
+
 template <class T>
 float safe_asin(const T v)
 {

libraries/AP_Math/AP_Math.h

@@ -24,8 +24,11 @@
 // define AP_Param types AP_Vector3f and Ap_Matrix3f
 AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
 
-// are two floats equal
+/*
-static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf(fVal1 - fVal2) < FLT_EPSILON ? true : false; }
+ * Check whether two floats are equal
+ */
+template <class FloatOne, class FloatTwo>
+bool is_equal(const FloatOne, const FloatTwo);
 
 // is a float is zero
 static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILON ? true : false; }

libraries/AP_Mount/SoloGimbal_Parameters.cpp

@@ -190,7 +190,7 @@ void SoloGimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavli
                     break;
                 case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
                     if (i == GMB_PARAM_GMB_FLASH) {
-                        if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && is_equal(packet.param_value,1)) {
+                        if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && (int)packet.param_value == 1) {
                             _flashing_step = GMB_PARAM_NOT_FLASHING;
                         }
                         _params[i].value = 0;