AP_Math: add is_equal to compare floats

libraries/AP_Math/AP_Math.cpp

@@ -1,4 +1,11 @@
 #include "AP_Math.h"
+#include <float.h>
+
+// are two floats equal
+bool is_equal(const float &fVal1, const float &fVal2)
+{
+  return fabs(fVal1 - fVal2) < FLT_EPSILON ? true : false;
+}
 
 // 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

libraries/AP_Math/AP_Math.h

@@ -23,6 +23,9 @@
 #ifndef M_PI_F
  #define M_PI_F 3.141592653589793f
 #endif
+#ifndef M_2PI_F
+ #define M_2PI_F 2*M_PI_F
+#endif
 #ifndef PI
  # define PI M_PI_F
 #endif
@@ -71,6 +74,9 @@
 AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
 AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
 
+// are two floats equal
+bool            is_equal(const float &fVal1, const float &fVal2);
+
 // a varient of asin() that always gives a valid answer.
 float           safe_asin(float v);