Diff: MMA8452.cpp

Revision:

13:4bd8b4cd479d

Parent:

12:172540ff6b8b

Child:

14:0602b45ca70f

--- a/MMA8452.cpp	Tue Mar 04 17:50:47 2014 +0000
+++ b/MMA8452.cpp	Wed Mar 05 15:04:13 2014 +0000
@@ -36,6 +36,9 @@
    // set some defaults
    _bitDepth = BIT_DEPTH_UNKNOWN;
    setBitDepth(BIT_DEPTH_12);
+   _dynamicRange = DYNAMIC_RANGE_UNKNOWN;
+   setDynamicRange(DYNAMIC_RANGE_2G);
+   
    DBG("Done");
 }
 
@@ -90,6 +93,7 @@
 }
 
 int MMA8452::setDynamicRange(DynamicRange range, int toggleActivation) {
+   _dynamicRange = range;
    return maskAndApplyRegister(
       MMA8452_XYZ_DATA_CFG,
       MMA8452_DYNAMIC_RANGE_MASK,
@@ -155,7 +159,7 @@
 }
 
 // Reads xyz
-int MMA8452::readRawXYZ(char *dst) {
+int MMA8452::readXYZRaw(char *dst) {
    if(_bitDepth==BIT_DEPTH_UNKNOWN) {
       return 1;
    }
@@ -166,6 +170,54 @@
    return readRegister(MMA8452_OUT_X_MSB,dst,readLen);
 }
 
+int MMA8452::readXYZCounts(int *x, int *y, int *z) {
+   char buf[6];
+   if(readXYZRaw((char*)&buf)) {
+       return 1;
+   }
+   if(_bitDepth==BIT_DEPTH_12) {
+     *x = twelveBitToSigned(&buf[0]);
+     *y = twelveBitToSigned(&buf[2]);
+     *z = twelveBitToSigned(&buf[4]);
+   } else {
+     *x = eightBitToSigned(&buf[0]);
+     *y = eightBitToSigned(&buf[1]);
+     *z = eightBitToSigned(&buf[2]);
+   }
+   
+   return 0;
+}
+
+double MMA8452::convertCountToGravity(int count, int countsPerG) {
+   return (double)count/(double)countsPerG;
+}
+
+int MMA8452::readXYZGravity(double *x, double *y, double *z) {
+   int xCount = 0, yCount = 0, zCount = 0;
+   if(readXYZCounts(&xCount,&yCount,&zCount)) {
+      return 1;
+   }
+   
+   // assume starting with DYNAMIC_RANGE_2G and BIT_DEPTH_12
+   int countsPerG = 1024;
+   if(_bitDepth==BIT_DEPTH_8) {
+      countsPerG = 64;
+   }
+   switch(_dynamicRange) {
+      case DYNAMIC_RANGE_4G:
+         countsPerG /= 2;
+      break;
+      case DYNAMIC_RANGE_8G:
+         countsPerG /= 4;
+      break;
+   }
+   
+   *x = convertCountToGravity(xCount,countsPerG);
+   *y = convertCountToGravity(yCount,countsPerG);
+   *z = convertCountToGravity(zCount,countsPerG);
+   return 0;
+}
+
 // apply an AND mask to a register. read register value, apply mask, write it back
 int MMA8452::logicalANDRegister(char addr, char mask) {
    char value = 0;
@@ -220,6 +272,19 @@
     // note, could also do return writeRegister(addr,&data,1);
 }
 
+int MMA8452::eightBitToSigned(char *buf) {
+   return (int8_t)*buf;
+}
+
+int MMA8452::twelveBitToSigned(char *buf) {
+   // cheat by using the int16_t internal type
+   // all we need to do is convert to little-endian format and shift right
+   int16_t x = 0;
+   ((char*)&x)[1] = buf[0];
+   ((char*)&x)[0] = buf[1];
+   // note this only works because the below is an arithmetic right shift
+   return x>>4; 
+}
 
 int MMA8452::writeRegister(char addr, char *data, int nbytes) {
     // writing multiple bytes is a little bit annoying because