Diff: Accelerometer.cpp

Revision:

8:862e28c7f6f6

Parent:

7:1e00dfecc92d

Child:

9:12519f9dd3cd

diff -r 1e00dfecc92d -r 862e28c7f6f6 Accelerometer.cpp
--- a/Accelerometer.cpp	Sun Jan 15 02:08:32 2017 +0000
+++ b/Accelerometer.cpp	Sun Jan 15 03:32:36 2017 +0000
@@ -1,6 +1,8 @@
 #include "Accelerometer.hpp"
 #include "Utility.hpp"
 
+#include <cmath>
+
 namespace accelerometer
 {
     //Compute inverse two's complement to obtain a signed value
@@ -24,7 +26,7 @@
             default: return AXIS_INVALID;
         }
     }
-        
+     
     double g_force_from_int_axis_data(const int axis_data)
     {
         return (double)axis_data / 64.0;
@@ -35,11 +37,17 @@
     // - if the Z force is +1g, the accelerometer is flat
     // - if the Z force is -1g, the accelerometer is upside down
     // - if the Z force is 0g, the accelerometer 90 degree from the horizontal
-    //sin(theta) = Z force in g
+    //cos(theta) = Z force in g
     double angle_from_int_axis_data(const int axis_data)
     {
         const double z_g_force = g_force_from_int_axis_data(axis_data);
-        const double angle_radian = acos(fabs(z_g_force));
+        const double absolute_z_g_force = std::fabs(z_g_force);
+        if(absolute_z_g_force > 1.0)
+        {
+            return 0.0;
+        }
+        
+        const double angle_radian = std::acos(absolute_z_g_force);
         return utility::degree_from_radian(angle_radian);
     }
     
@@ -144,7 +152,7 @@
     
     double Accelerometer::get_angle_from_horizontal()
     {
-        const int z_axis_data = read_axis_data_8_bits(AXIS_Z);
+        const int z_axis_data = read_axis_data_8_bits(AXIS_Z);        
         return angle_from_int_axis_data(z_axis_data);
     }
 };
\ No newline at end of file