Added clip for integers and updated attitude from tilt

include/sta/math/utils.hpp

@@ -9,6 +9,8 @@ namespace sta
     {
         float fast_inv_sqrt(float);
 
+        int clip(int value, int min, int max);
+
         float clip(float value, float min, float max);
     } // namespace math
 } // namespace sta

src/algorithms/attitude/tilt.cpp

@@ -24,8 +24,8 @@ namespace sta
 				az /= norm;
 			}
 
-			float theta = std::atan(ay / az);
+			float theta = std::atan2(ay, az);
-			float phi 	= std::atan(-ax / std::sqrt(ay*ay + az*az));
+			float phi 	= std::atan2(-ax, std::sqrt(ay*ay + az*az));
 
 			Quaternion tilt(
 				  std::cos(phi / 2.0f) * std::cos(theta / 2.0f),

src/utils.cpp

@@ -22,6 +22,21 @@ namespace sta
 			return y;
 		}
 
+		int clip(int value, int min, int max)
+		{
+			if (value < min)
+			{
+				return  min;
+			}
+
+			if (value > max)
+			{
+				return max;
+			}
+
+			return value;
+		}
+
 		float clip(float value, float min, float max)
 		{
 			if (value < min)