From f9be23933b2b3dcffa2fdcc07feecde25c783567 Mon Sep 17 00:00:00 2001
From: mcsauder <mcsauder@greypointcorp.com>
Date: Sat, 5 Mar 2016 01:58:58 -0700
Subject: [PATCH] Correct deg to rad conversion inversion.  Add additional
 functionality to mathlib to allow standalone compile without Limits.cpp and
 Limits.hpp files from PX4.

---
 EKF/geo.h       |  4 ++--
 EKF/mathlib.cpp | 25 ++++++++++++++++++++++---
 EKF/mathlib.h   | 11 ++++++++---
 3 files changed, 32 insertions(+), 8 deletions(-)

diff --git a/EKF/geo.h b/EKF/geo.h
index bc33d23b94..a4f6ca2c51 100644
--- a/EKF/geo.h
+++ b/EKF/geo.h
@@ -55,8 +55,8 @@
 #define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
 #define M_TWOPI_F 6.28318530717958647692f
 #define M_PI_2_F  1.57079632679489661923f
-#define M_RAD_TO_DEG 0.01745329251994329576f
-#define M_DEG_TO_RAD 57.29577951308232087679f
+#define M_RAD_TO_DEG 57.29577951308232087679f
+#define M_DEG_TO_RAD 0.01745329251994329576f
 #define OK 0
 #define ERROR -1
 // XXX remove
diff --git a/EKF/mathlib.cpp b/EKF/mathlib.cpp
index 7db1730553..2cd1190cf0 100644
--- a/EKF/mathlib.cpp
+++ b/EKF/mathlib.cpp
@@ -42,16 +42,35 @@
 
 #ifdef POSIX_SHARED
 
-float math::constrain(float &val, float min, float max)
+
+namespace math
+{
+
+float min(float val1, float val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+float max(float val1, float val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+float constrain(float &val, float min, float max)
 {
 	return (val < min) ? min : ((val > max) ? max : val);
 }
-float math::radians(float degrees)
+
+float radians(float degrees)
 {
 	return (degrees / 180.0f) * M_PI_F;
 }
-float math::degrees(float radians)
+
+float degrees(float radians)
 {
 	return (radians * 180.0f) / M_PI_F;
 }
+
+}
+
 #endif
\ No newline at end of file
diff --git a/EKF/mathlib.h b/EKF/mathlib.h
index d4e002dd58..aebcd6d5ce 100644
--- a/EKF/mathlib.h
+++ b/EKF/mathlib.h
@@ -41,18 +41,23 @@
 #ifndef MATHLIB_H
 #define MATHLIB_H
 #ifdef POSIX_SHARED
-#include <Eigen/Dense>
-#include <algorithm>
+// #include <Eigen/Dense>
+// #include <algorithm>
+// #include "Limits.hpp"
 #define M_PI_F 3.14159265358979323846f
+#define constexpr
 
 namespace math
 {
-using namespace Eigen;
+// using namespace Eigen;
 using namespace std;
 
+float min(float val1, float val2);
+float max(float val1, float val2);
 float constrain(float &val, float min, float max);
 float radians(float degrees);
 float degrees(float radians);
+
 }
 #else
 #include <mathlib/mathlib.h>