Diff: FXOS8700Q.cpp

Revision:

9:5553a64d0762

Parent:

8:52116f8fb3da

--- a/FXOS8700Q.cpp	Sat Apr 26 01:36:41 2014 +0000
+++ b/FXOS8700Q.cpp	Wed May 07 18:08:08 2014 +0000
@@ -19,6 +19,8 @@
 #include "FXOS8700Q.h"
 #define UINT14_MAX        16383
 
+uint8_t SensorBuffer[12];
+int     MagReadStatus;
 
 FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
     // activate the peripheral
@@ -37,6 +39,7 @@
     data[0] = FXOS8700Q_CTRL_REG1;
     data[1] = 0x1C;
     writeRegs(data, 2);
+    MagReadStatus = 0;
 }
 
 FXOS8700Q_acc::~FXOS8700Q_acc() { }
@@ -47,6 +50,7 @@
     data[1] |= 0x01;
     data[0] = FXOS8700Q_CTRL_REG1;
     writeRegs(data, 2);
+    MagReadStatus = 0;
 }
 
 void FXOS8700Q_acc::disable(void) {
@@ -55,6 +59,7 @@
     data[1] &= 0xFE;
     data[0] = FXOS8700Q_CTRL_REG1;
     writeRegs(data, 2);
+    MagReadStatus = 0;
 }
 
 
@@ -102,44 +107,45 @@
 
 void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
     int16_t acc, t[3];
-    uint8_t res[6];
-   readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
 
-    acc = (res[0] << 6) | (res[1] >> 2);
+   readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
+
+    acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
     if (acc > UINT14_MAX/2)
         acc -= UINT14_MAX;
     t[0] = acc;
-    acc = (res[2] << 6) | (res[3] >> 2);
+    acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
     if (acc > UINT14_MAX/2)
         acc -= UINT14_MAX;
     t[1] = acc;
-    acc = (res[4] << 6) | (res[5] >> 2);
+    acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
     if (acc > UINT14_MAX/2)
         acc -= UINT14_MAX;
     t[2] = acc;
     data.x = ((float) t[0]) / 4096.0f;
     data.y = ((float) t[1]) / 4096.0f;
     data.z = ((float) t[2]) / 4096.0f;
+    MagReadStatus = 1;
 }
 
 
 void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
     int16_t acc;
-    uint8_t res[6];
-    readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
+    readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
 
-    acc = (res[0] << 6) | (res[1] >> 2);
+    acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
     if (acc > UINT14_MAX/2)
         acc -= UINT14_MAX;
     data.x = acc;
-    acc = (res[2] << 6) | (res[3] >> 2);
+    acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
     if (acc > UINT14_MAX/2)
         acc -= UINT14_MAX;
     data.y = acc;
-    acc = (res[4] << 6) | (res[5] >> 2);
+    acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
     if (acc > UINT14_MAX/2)
         acc -= UINT14_MAX;
     data.z = acc;
+    MagReadStatus = 1;
 }
 
 void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
@@ -184,6 +190,7 @@
     data[0] = FXOS8700Q_CTRL_REG1;
     data[1] = 0x18;//0x1D;
     writeRegs(data, 2);
+    MagReadStatus = 0;
 }
 
 FXOS8700Q_mag::~FXOS8700Q_mag() { }
@@ -194,6 +201,7 @@
     data[1] |= 0x01;
     data[0] = FXOS8700Q_CTRL_REG1;
     writeRegs(data, 2);
+    MagReadStatus = 0;
 }
 
 void FXOS8700Q_mag::disable(void) {
@@ -202,6 +210,7 @@
     data[1] &= 0xFE;
     data[0] = FXOS8700Q_CTRL_REG1;
     writeRegs(data, 2);
+    MagReadStatus = 0;
 }
 
 
@@ -250,12 +259,17 @@
 void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
     int16_t t[3];
     uint8_t res[6];
-   readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
-
-    t[0] = (res[0] << 8) | res[1];
-    t[1] = (res[2] << 8) | res[3];
-    t[2] = (res[4] << 8) | res[5];
-
+    
+   if(MagReadStatus) {
+        t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7];
+        t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9];
+        t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11];        
+        } else {
+        readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+        t[0] = (res[0] << 8) | res[1];
+        t[1] = (res[2] << 8) | res[3];
+        t[2] = (res[4] << 8) | res[5];
+        }
     data.x = ((float) t[0]) * 0.1f;
     data.y = ((float) t[1]) * 0.1f;
     data.z = ((float) t[2]) * 0.1f;
@@ -263,7 +277,7 @@
 
 
 void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
-    int16_t acc;
+
     uint8_t res[6];
     readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);