Diff: MMA8451Q_tb.cpp

Revision:

1:c6db5b21ad79

Parent:

0:7bc6b6007ed1

--- a/MMA8451Q_tb.cpp	Tue Apr 29 22:26:20 2014 +0000
+++ b/MMA8451Q_tb.cpp	Tue Apr 29 23:16:16 2014 +0000
@@ -56,6 +56,29 @@
     return acc;
 }
 
+void MMA8451Q::fastRead(float * acc_arr) {
+    uint8_t res[6];
+    int16_t xt, yt, zt;
+    
+    readRegs(REG_OUT_X_MSB, res, 6);
+    
+    xt = (res[0]<<6)|(res[1]>>2);
+    yt = (res[2]<<6)|(res[3]>>2);
+    zt = (res[4]<<6)|(res[5]>>2);
+    
+    if (xt > UINT14_MAX/2)
+        xt -= UINT14_MAX;
+    if (yt > UINT14_MAX/2)
+        yt -= UINT14_MAX;
+    if (zt > UINT14_MAX/2)
+        zt -= UINT14_MAX;
+        
+    acc_arr[0] = float(xt/4096.0);
+    acc_arr[1] = float(yt/4096.0);
+    acc_arr[2] = float(zt/4096.0);
+}
+
+
 void MMA8451Q::readRegs(int addr, uint8_t * data, int len) {
     char t[1] = {addr};
     m_i2c.write(m_addr, t, 1, true);