File content as of revision 7:9b644d1c1405:

#include "mbed.h"
#include "MMA8452.h"

DigitalOut myled(LED1);

Serial pc(USBTX,USBRX);

#define LOG(...) pc.printf(__VA_ARGS__); pc.printf("\r\n");
#define LOGX(...) pc.printf(__VA_ARGS__);

void printByte(char b) {
   LOG("%d%d%d%d%d%d%d%d",
       (b&0x80)>>7,
       (b&0x40)>>6,
       (b&0x20)>>5,
       (b&0x10)>>4,
       (b&0x08)>>3,
       (b&0x04)>>2,
       (b&0x02)>>1,
       (b&0x01)
   );
}

enum SampleType {
   SAMPLE_RAW=0,
   SAMPLE_COUNT,
   SAMPLE_GRAVITY
};

void sampleTypeToString(SampleType t, char *dst) {
   switch(t) {
      case SAMPLE_RAW:
         sprintf(dst,"SAMPLE_RAW");
      break;
      case SAMPLE_COUNT:
         sprintf(dst,"SAMPLE_COUNT");
      break;
      case SAMPLE_GRAVITY:
         sprintf(dst,"SAMPLE_GRAVITY");
      break;
   };
}

int testSampleTaking(MMA8452 *acc, int nsamples, SampleType sampleType) {
   int samples = 0;
   int bufLen = 6;
   
   // buffers for multi and single raw sampling
   char bufferMulti[6];
   char bufferSingle[6];
   memset(&bufferMulti,0x00,bufLen);
   memset(&bufferSingle,0x00,bufLen);
   
   // variables for multi and single count sampling
   int xCountM = 0, yCountM = 0, zCountM = 0;
   int xCountS = 0, yCountS = 0, zCountS = 0;
   
   // variables for multi and single gravity sampling
   double xGravityM = 0, yGravityM = 0, zGravityM = 0;
   double xGravityS = 0, yGravityS = 0, zGravityS = 0;
   
   // keep track of errors
   int error = 0;
   // mismatches between multi and single read calls are inevitable
   // since the MMA8452 has an internal sampling mechanism which is
   // not synchronous to this test routine. At low internal sampling
   // rates, these mismatches should be rare, so keep track to
   // check that this is sane
   int mismatchCount = 0;
   
   // take samples
   while(samples<nsamples) {
      // wait for device to be ready
      if(!acc->isXYZReady()) {
         wait(0.01);
         continue;
      }
      
      switch(sampleType) {
         case SAMPLE_RAW:
            // read raw data via multi and single calls
            memset(&bufferMulti,0x00,bufLen);
            memset(&bufferSingle,0x00,bufLen);
            error = 0;
            error += acc->readXYZRaw((char*)&bufferMulti);
            error += acc->readXRaw((char*)&bufferSingle[0]);
            error += acc->readYRaw((char*)&bufferSingle[2]);
            error += acc->readZRaw((char*)&bufferSingle[4]);
            if(error) {
               LOG("Error reading raw accelerometer data. Fail.");
               return false;
            }
            // compare multi and single samples for equivalence
            // note that this is bound to fail for high data rates
            if(acc->getBitDepth()==MMA8452::BIT_DEPTH_12) {
               if(memcmp(bufferMulti,bufferSingle,bufLen)) {
                  LOG("Multi and single sampling mismatch");
                  LOG("Multi: %x %x %x %x %x %x",
                     bufferMulti[0],bufferMulti[1],
                     bufferMulti[2],bufferMulti[3],
                     bufferMulti[4],bufferMulti[5]
                  );
                  LOG("Single: %x %x %x %x %x %x",
                     bufferSingle[0],bufferSingle[1],
                     bufferSingle[2],bufferSingle[3],
                     bufferSingle[4],bufferSingle[5]
                  ); 
                  mismatchCount++;
               }
               LOG("12bit raw sample %d/%d: %x %x %x %x %x %x",
                  samples,nsamples,
                  bufferMulti[0],bufferMulti[1],
                  bufferMulti[2],bufferMulti[3],
                  bufferMulti[4],bufferMulti[5]
               );
            } else {
               if(bufferMulti[0]!=bufferSingle[0]||
                  bufferMulti[1]!=bufferSingle[2]||
                  bufferMulti[2]!=bufferSingle[4]) {
                  LOG("Multi and single sampling mismatch");
                  mismatchCount++;
               }
               LOG("8 bit raw sample %d/%d: %x %x %x",
                  samples,nsamples,
                  bufferMulti[0],bufferMulti[1],bufferMulti[2]
               );
            }
         break;
         case SAMPLE_COUNT:
            error = 0;
            error += acc->readXYZCounts(&xCountM,&yCountM,&zCountM);
            error += acc->readXCount(&xCountS);
            error += acc->readYCount(&yCountS);
            error += acc->readZCount(&zCountS);
            if(error) {
               LOG("Error reading signed counts. Fail.");
               break;
            }
            // check for equivlance (note this fails sometimes but this is expected)
            if(xCountS!=xCountM || yCountS!=yCountM || zCountS!=zCountM) {
               LOG("Multi and single sampling mismatch");
               mismatchCount++;
            }
            LOG("Count sample %d/%d: %d %d %d",samples,nsamples,xCountM,yCountM,zCountM);
         break;
         case SAMPLE_GRAVITY:
            error = 0;
            error += acc->readXYZGravity(&xGravityM,&yGravityM,&zGravityM);
            error += acc->readXGravity(&xGravityS);
            error += acc->readYGravity(&yGravityS);
            error += acc->readZGravity(&zGravityS);
            if(error) {
               LOG("Error reading gravities. Fail.");
               break;
            }
            if(xGravityS!=xGravityM || yGravityS!=yGravityM || zGravityS!=zGravityM) {
               LOG("Multi and single sampling mismatch");
               mismatchCount++;
            }
            LOG("Gravity sample %d/%d: %lf %lf %lf",samples,nsamples,xGravityM,yGravityM,zGravityM);
         break;
      }
      samples++;
   }
   LOG("Mismatches between single and multi-byte reads %d/%d (mismatches are to be expected)",mismatchCount,nsamples);
   return true;
}

int sampleMMA8452Raw(MMA8452 *acc, int nsamples) {
   int samples = 0;
   int bufLen = 6;
   char buffer[6];
   memset(&buffer,0x00,bufLen);
   while(samples<nsamples) {
      if(!acc->isXYZReady()) {
         wait(0.01);
         continue;
      }
      memset(&buffer,0x00,bufLen);
      acc->readXYZRaw(buffer);
      LOGX("Sample %d of %d: ",samples,nsamples);
      for(int i=0; i<bufLen; i++) {
         LOGX("%.2x ",buffer[i]);
      }
      LOG(" ");
      samples++;
   }
   return true;
}

int sampleMMA8452Counts(MMA8452 *acc, int nsamples) {
   int samples = 0;
   int bufLen = 6;
   char buffer[6];
   int x = 0, y = 0, z = 0;
   memset(&buffer,0x00,bufLen);
   while(samples<nsamples) {
      if(!acc->isXYZReady()) {
         wait(0.01);
         continue;
      }
      memset(&buffer,0x00,bufLen);
      if(acc->readXYZCounts(&x,&y,&z)) {
         LOG("Error reading sample");
         break;
      }
      LOG("Sample %d of %d: %d, %d, %d",samples,nsamples,x,y,z);
      samples++;
   }
   return true;
}

int sampleMMA8452Gravities(MMA8452 *acc, int nsamples) {
   int samples = 0;
   int bufLen = 6;
   char buffer[6];
   double x = 0, y = 0, z = 0;
   memset(&buffer,0x00,bufLen);
   while(samples<nsamples) {
      if(!acc->isXYZReady()) {
         wait(0.01);
         continue;
      }
      memset(&buffer,0x00,bufLen);
      if(acc->readXYZGravity(&x,&y,&z)) {
         LOG("Error reading sample");
         break;
      }
      LOG("Sample %d of %d: %lf, %lf, %lf",samples,nsamples,x,y,z);
      samples++;
   }
   return true;
}

void bitDepthToString(MMA8452::BitDepth d, char *dst) {
   switch(d) {
       case MMA8452::BIT_DEPTH_12:
          sprintf(dst,"BIT_DEPTH_12");
       break;
       case MMA8452::BIT_DEPTH_8:
          sprintf(dst,"BIT_DEPTH_8");
       break;
    }
}

void dynamicRangeToString(MMA8452::DynamicRange r, char *dst) {
   switch(r) {
      case MMA8452::DYNAMIC_RANGE_2G:
         sprintf(dst,"DYNAMIC_RANGE_2G");
      break;
      case MMA8452::DYNAMIC_RANGE_4G:
         sprintf(dst,"DYNAMIC_RANGE_4G");
      break;
      case MMA8452::DYNAMIC_RANGE_8G:
         sprintf(dst,"DYNAMIC_RANGE_8G");
      break;
   }
}

void dataRateToString(MMA8452::DataRateHz r, char *dst) {
   switch(r) {
       case MMA8452::RATE_800:
          sprintf(dst,"RATE_800");
       break;
       case MMA8452::RATE_400:
          sprintf(dst,"RATE_400");
       break;
       case MMA8452::RATE_200:
          sprintf(dst,"RATE_200");
       break;
       case MMA8452::RATE_100:
          sprintf(dst,"RATE_100");
       break;
       case MMA8452::RATE_50:
          sprintf(dst,"RATE_50");
       break;
       case MMA8452::RATE_12_5:
          sprintf(dst,"RATE_12_5");
       break;
       case MMA8452::RATE_6_25:
          sprintf(dst,"RATE_6_25");
       break;
       case MMA8452::RATE_1_563:
          sprintf(dst,"RATE_1_563");
       break;
    }
}

int test() {
    MMA8452 acc(p28, p27, 40000);
    
    acc.debugRegister(MMA8452_CTRL_REG_1);
    
    LOG("Entering standby");
    if(acc.standby()) {
       LOG("Error putting MMA8452 in standby");
       return false;
    }
    
    acc.debugRegister(MMA8452_CTRL_REG_1);
    
    LOG("Activating MMA8452");
    if(acc.activate()) {
       LOG("Error activating MMA8452");
       return false;
    }
    
    char devID = 0;
    if(acc.getDeviceID(&devID)) {
       LOG("Error getting device ID");
       return false;
    }
    LOG("DeviceID: 0x%x",devID);
    if(devID!=0x2a) {
       LOG("Error, fetched device ID: 0x%x does not match expected 0x2a",devID);
       return false;
    } else {
       LOG("Device ID OK");
    }
    
    // test setting dynamic range
    MMA8452::DynamicRange setRange = MMA8452::DYNAMIC_RANGE_UNKNOWN;
    MMA8452::DynamicRange readRange = MMA8452::DYNAMIC_RANGE_UNKNOWN;
    for(int i=0; i<=(int)MMA8452::DYNAMIC_RANGE_8G; i++) {
       setRange = (MMA8452::DynamicRange)i;
       if(acc.setDynamicRange(setRange)) {
          LOG("Error setting dynamic range. Failing.");
          return false;
       }
       readRange = acc.getDynamicRange();
       if(readRange!=setRange) {
          LOG("Read dynamic range: 0x%x does not match set: 0x%x",readRange,setRange);
          return false;
       }
       LOG("Success on dynamic range %d",i);
    }
    
    // test setting data rate
    for(int i=0; i<=(int)MMA8452::RATE_1_563; i++) {
       if(acc.setDataRate((MMA8452::DataRateHz)i)) {
          LOG("Error setting data rate. Failing.");
          return false;
       }
       if(acc.getDataRate()!=(MMA8452::DataRateHz)i) {
          LOG("Read data rate: 0x%x does not match set: 0x%x",acc.getDataRate(),(MMA8452::DataRateHz)i);
          return false;
       }
       LOG("Success on data rate %d",i);
    }
    
    char depthString[32], rangeString[32], rateString[32], sampleTypeString[32];
    // draw some samples at each bit depth, rate, and dynamic range
    for(int depth=0; depth<=(int)MMA8452::BIT_DEPTH_8; depth++) {
       bitDepthToString((MMA8452::BitDepth)depth,(char*)&depthString);
       LOG("Setting bit depth to %s",depthString);
       if(acc.setBitDepth((MMA8452::BitDepth)depth)) {
          LOG("Error setting bit depth to %s. Fail.",depthString);
          return false;
       }
       for(int range=0; range<=(int)MMA8452::DYNAMIC_RANGE_8G; range++) {
          dynamicRangeToString((MMA8452::DynamicRange)range,(char*)&rangeString);
          LOG("Setting dynamic range to %s",rangeString);
          if(acc.setDynamicRange((MMA8452::DynamicRange)range)) {
             LOG("Error setting dynamic range to %s. Fail.",rangeString);
             return false;
          }
          for(int rate=0; rate<=(int)MMA8452::RATE_1_563; rate++) {
             dataRateToString((MMA8452::DataRateHz)rate,(char*)&rateString);
             LOG("Setting data rate to %s",rateString);
             if(acc.setDataRate((MMA8452::DataRateHz)rate)) {
                LOG("Error setting data rate to %s",rateString);
                return false;
             }
             // take samples
             for(int sampleType=0; sampleType<=(int)SAMPLE_GRAVITY; sampleType++) {
                sampleTypeToString((SampleType)sampleType,sampleTypeString);
                LOG("Setting sample type to %s",sampleTypeString);
                if(testSampleTaking(&acc, 10, (SampleType)sampleType)!=true) {
                  LOG("Sample taking failed for %s, %s, %s, %s",sampleTypeString,depthString,rangeString,rateString);
                  return false;
                }
             }
          }
       }
    }

    LOG("Samping gravities for interactive examination");
    if(acc.setBitDepth(MMA8452::BIT_DEPTH_8)) {
       LOG("Error setting bit depth. Fail.");
       return false;
    }
    if(acc.setDynamicRange(MMA8452::DYNAMIC_RANGE_4G)) {
       LOG("Error setting dynamic range. Fail.");
       return false;
    }
    if(acc.setDataRate(MMA8452::RATE_100)) {
       LOG("Error setting data rate. Fail");
       return false;
    }
    if(sampleMMA8452Gravities(&acc,1000)!=true) {
       LOG("Sampling gravities failed");
       return false;
    }

    return true;
}

void loop() {
   while(1) {
      wait(1);
   }
}

void u16d(uint16_t n) {
   int shift = 16;
   uint16_t mask = 0x8000;
   while(--shift>=0) {
      LOGX("%d",(n&mask)>>shift);
      mask >>= 1;
   }
   LOG(" ");
}

int eightBitToSigned(char *buf) {
   return (int8_t)*buf;
}

int twelveBitToSigned(char *buf) {
   //LOG("Doing twos complement conversion for 0x%x 0x%x",buf[0],buf[1]);
   
   // 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; 

   // for reference, here is the full conversion, in case you port this somewhere where the above won't work
   /*
   uint16_t number = 0x0000;
   //u16d(number);
   int negative = false;
   
   // bit depth 12, is spread over two bytes
   // put it into a uint16_t for easy manipulation
   number |= (buf[0]<<8);
   number |= buf[1];

   // if this is a negative number take the twos complement
   if(number&0x8000) {
       negative = true;
       // flip all bits (doesn't matter about lower 4 bits)
       number ^= 0xFFFF;

       // add 1 (but do so in a way that deals with overflow and respects our current leftwise shift)
       number += 0x0010;
   }
   
   // shifting down the result by 4 bits gives us the absolute number
   number >>= 4;

   int result = number;
   if(negative) {
      result *= -1;
   }
   return result;
   */
}

int twosCompTest() {
    // 12 bits of number gives 2048 steps
    int16_t i = -2047;
    while(1) {
       //LOG("number: %d",i);
       //u16d(number);
       uint16_t shiftedNumber = i<<4;
       //LOG("shifted:");
       //u16d(shiftedNumber);
       // ARM is little endian whereas 12 bit 2's comp rep is big endian
       uint16_t flippedNumber = 0x0000;
       //LOG("switching bytes");
       //u16d(flippedNumber);
       ((char*)&flippedNumber)[0] = ((char*)&shiftedNumber)[1];
       
       //u16d(flippedNumber);
       ((char*)&flippedNumber)[1] = ((char*)&shiftedNumber)[0]; 
       
       //u16d(flippedNumber);
       int value = twelveBitToSigned((char*)&flippedNumber);
       //LOG("%d converts to %d",i,value);
       if(i!=value) {
          return false;
       }
       if(i==2047) {
          break;
       }
       i++;
    }
    
    int8_t n = -127;
    while(1) {
       int value = eightBitToSigned((char*)&n);
       //LOG("%d converts to %d",n,value);
       if(n!=value) {
          return false;
       }
       if(n==127) {
          break;
       }
       n++;
    }
 
    return true;
}

int main() {
    pc.baud(115200);
    LOG("Begin");
    LOG("Executing twos complement test");
    if(!twosCompTest()) {
       LOG("Twos complement test failed");
       loop();
    }
    LOG("Twos complement test passed");

    LOG("Executing MMA8452 tests");
    if(!test()) {
       LOG("MMA8452 test failed.");
       loop();
    }
    LOG("MMA8452 test passed");
    LOG("All tests passed");
    loop();
}