Diff: Accelerometer.cpp

Revision:

5:f59b51ac4b40

Parent:

4:303fb83498fd

Child:

6:3facf0329142

diff -r 303fb83498fd -r f59b51ac4b40 Accelerometer.cpp
--- a/Accelerometer.cpp	Sat Jan 14 23:32:29 2017 +0000
+++ b/Accelerometer.cpp	Sun Jan 15 01:34:25 2017 +0000
@@ -28,6 +28,36 @@
     return (difference <= tolerance);
 }
 
+//Return angle between 0 and 360 degree
+double wrap_angle(double angle)
+{
+    return angle - 360 * floor( angle / 360 );
+} 
+    
+double g_force_from_int_axis_data(const int axis_data)
+{
+    return (double)axis_data / 64.0;
+}
+
+double degree_from_radian(const double angle_radian)
+{
+    const double angle_degree = angle_radian * 180.0 / PI;
+    return wrap_angle(angle_degree);
+}
+
+//Z axis is perpendicular to the horizontal plane, towards the floor when the accelerometer is flat
+//Therefore, 
+// - 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
+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));
+    return degree_from_radian(angle_radian);
+}
+
 Accelerometer::Accelerometer(
     PinName sda_pin, 
     PinName scl_pin, 
@@ -125,4 +155,10 @@
     const char axis_register = get_axis_register(axis);
     const char register_value = read_register(axis_register);
     return raw_axis_data_to_int(register_value);
+}
+
+double Accelerometer::get_angle_from_horizontal()
+{
+    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