From da49149d1910ccd57808f3600f32c87065f01c55 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Sat, 17 Dec 2016 19:16:39 +0100
Subject: [PATCH] AP_Math: is_equal correct comparison for integer as epsilon
 doesn't exist. Credit to Kwikius for the right solution

---
 libraries/AP_Math/AP_Math.cpp | 15 +++++++++++----
 libraries/AP_Math/AP_Math.h   |  9 +++++++--
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 9f6c85ad99..c68c7491f4 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -1,11 +1,18 @@
 #include "AP_Math.h"
 #include <float.h>
 
-template <class FloatOne, class FloatTwo>
-bool is_equal(const FloatOne v_1, const FloatTwo v_2)
+template <class Arithmetic1, class Arithmetic2>
+typename std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type
+is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
+{
+    typedef typename std::common_type<Arithmetic1, Arithmetic2>::type common_type;
+    return static_cast<common_type>(v_1) == static_cast<common_type>(v_2);
+}
+
+template <class Arithmetic1, class Arithmetic2>
+typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
+is_equal(const Arithmetic1 v_1, const Arithmetic2 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();
 }
 
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 4e1399f548..2969e10577 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -24,8 +24,13 @@ AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
 /*
  * Check whether two floats are equal
  */
-template <class FloatOne, class FloatTwo>
-bool is_equal(const FloatOne, const FloatTwo);
+template <class Arithmetic1, class Arithmetic2>
+typename std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type
+is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
+
+template <class Arithmetic1, class Arithmetic2>
+typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
+is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
 
 /* 
  * @brief: Check whether a float is zero