LSM303DLH.cpp

00001 
00002 #define __DEBUG__ 0
00003 #ifndef __MODULE__
00004 #define __MODULE__ "LSM303DLH.cpp"
00005 #endif
00006 #include "MyDebug.h"
00007 
00008 #include "LSM303DLH.h"
00009 
00010 LSM303DLH::LSM303DLH(PinName sda, PinName scl): dev(sda,scl) {
00011     uint8_t d[16];
00012     INFO("LSM303DLH Init (400kHz)");
00013     dev.frequency(400000);
00014     // Check accelerometer presence
00015     this->devRead(WHO_AM_I,d,1);
00016     if(d[0] == LSM303DH_ID) {
00017         INFO("LSM303DH(0x%02X) well detected!!",d[0]);
00018         // Config accelerometer
00019         this->basicConfig();
00020     } else
00021         ERR("Unknow ID 0x%02X ...",d[0]);
00022 }
00023 
00024 uint8_t LSM303DLH::devRead(const uint8_t reg, uint8_t *data, uint8_t size) {
00025     uint8_t *pReg = (uint8_t*)® 
00026     *pReg |= 0x80;
00027     __disable_irq();
00028     dev.write(LSM303D_ADDR_W,(char*)pReg,1);
00029     dev.read(LSM303D_ADDR_R,(char*)data,size);
00030     __enable_irq();
00031     return size;
00032 }
00033 
00034 uint8_t LSM303DLH::devReadSingle(const uint8_t reg, uint8_t byte) {
00035     return this->devRead(reg,&byte,1);
00036 }
00037 
00038 uint8_t LSM303DLH::devWrite(const uint8_t reg, uint8_t *data, uint8_t size) {
00039     // Check size
00040     if((size+1)>WRITE_BUFF_MAX) {
00041         ERR("BAD SIZE WRITE");
00042         return 0;
00043     } else {
00044         // Put the reg addr in first byte place
00045         dataBuff[0] = reg | 0x80;
00046         for(int i = 0; i< size; i++)
00047             dataBuff[i+1] = data[i];
00048         // Do the Writing
00049         dev.write(LSM303D_ADDR_W,(char*)dataBuff,size+1);
00050         return size;
00051     }
00052 }
00053 
00054 uint8_t LSM303DLH::devWriteSingle(const uint8_t reg, uint8_t byte) {
00055     return this->devWrite(reg,&byte,1);
00056 }
00057 
00058 // Hi Level API
00059 
00060 void LSM303DLH::basicConfig() {
00061     // Accelerometer
00062     // 0x00 = 0b00000000
00063     // AFS = 0 (+/- 2 g full scale) - > 0.061 g/LSB
00064     this->devWriteSingle(CTRL2, 0x00);
00065     accLSB = 61.0;
00066     // 0x57 = 0b01010111
00067     // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
00068     this->devWriteSingle(CTRL1, 0x57);
00069 
00070     // Magnetometer
00071     // 0x64 = 0b01100100
00072     // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
00073     this->devWriteSingle(CTRL5, 0x64);
00074     // 0x20 = 0b00100000
00075     // MFS = 01 (+/- 4 gauss full scale)
00076     this->devWriteSingle(CTRL6, 0x20);
00077     magLSB = 80.0;
00078     // 0x00 = 0b00000000
00079     // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
00080     this->devWriteSingle(CTRL7, 0x00);
00081     
00082     DBG("LSM303DH configuration done\r\n");
00083 }
00084 
00085 void LSM303DLH::readRawAcc(int16_t *x, int16_t *y, int16_t *z) {
00086     uint8_t rawData[6] = {0,0,0,0,0,0};
00087     if(this->devRead(OUT_X_L_A,rawData,6)){
00088         *x = (int16_t)(rawData[0] | (rawData[1] << 8));
00089         *y = (int16_t)(rawData[2] | (rawData[3] << 8));
00090         *z = (int16_t)(rawData[4] | (rawData[5] << 8));
00091     }
00092 }
00093 
00094 void LSM303DLH::readAcc(float *x, float *y, float *z) {
00095     int16_t rawX;
00096     int16_t rawY;
00097     int16_t rawZ;
00098     this->readRawAcc(&rawX,&rawY,&rawZ);
00099     *x = (((float) rawX) * accLSB) /1000000.0;
00100     *y = (((float) rawY) * accLSB) /1000000.0;
00101     *z = (((float) rawZ) * accLSB) /1000000.0;
00102 }
00103 
00104 
00105 void LSM303DLH::readRawMag(int16_t *x, int16_t *y, int16_t *z) {
00106     uint8_t rawData[6] = {0,0,0,0,0,0};
00107     if(this->devRead(OUT_X_L_M,rawData,6)){
00108         *x = (int16_t)(rawData[0] | (rawData[1] << 8));
00109         *y = (int16_t)(rawData[2] | (rawData[3] << 8));
00110         *z = (int16_t)(rawData[4] | (rawData[5] << 8));
00111     }
00112 }
00113 
00114 void LSM303DLH::readMag(float *x, float *y, float *z) {
00115     int16_t rawX;
00116     int16_t rawY;
00117     int16_t rawZ;
00118     this->readRawMag(&rawX,&rawY,&rawZ);
00119     *x = (((float) rawX) * magLSB) / 1000000.0;
00120     *y = (((float) rawY) * magLSB) / 1000000.0;
00121     *z = (((float) rawZ) * magLSB) / 1000000.0;
00122 }