Diff: KXR94_2050.cpp

Revision:

0:2700ada3a245

Child:

2:1e4a75b43643

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KXR94_2050.cpp	Sat Jan 03 15:06:55 2015 +0000
@@ -0,0 +1,28 @@
+#include "KXR94_2050.h"
+
+KXR94_2050::KXR94_2050(PinName OutX, 
+                       PinName OutY,
+                       PinName OutZ) : _OutX(OutX), _OutY(OutY), _OutZ(OutZ) {    
+    X_ZERO_VAL = 2097;
+    Y_ZERO_VAL = 2050;
+    Z_ZERO_VAL = 2052;
+}
+
+void KXR94_2050::gsetZeroVal(uint16_t x_zero, uint16_t y_zero, uint16_t z_zero) {
+    X_ZERO_VAL = x_zero;
+    Y_ZERO_VAL = y_zero;
+    Z_ZERO_VAL = z_zero;
+}
+
+void KXR94_2050::getRawData(float &pitch, float &roll) {
+    uint16_t Xacc = X_value.read_u16();
+    uint16_t Yacc = Y_value.read_u16();
+    uint16_t Zacc = Z_value.read_u16();
+    
+    int xval = Xacc - X_ZERO_VAL;
+    int yval = Yacc - Y_ZERO_VAL;
+    int zval = Zacc - Z_ZERO_VAL;
+    
+    roll = atan2l(yval, zval) * 180.0 / PI;
+    pitch = atan2l(xval, zval) * 180.0 / PI;
+}
\ No newline at end of file