This my big project for PYRN Board
Dependencies: CAN HTTPClient MODSERIAL MyThings Pyrn3GModem Socket TinyGPS MyUSBHost lwip-sys lwip mbed-rtos mbed-src
LSM303DLH.cpp
- Committer:
- clemounet
- Date:
- 2015-04-14
- Revision:
- 0:efe6085327fd
File content as of revision 0:efe6085327fd:
#define __DEBUG__ 0 #ifndef __MODULE__ #define __MODULE__ "LSM303DLH.cpp" #endif #include "MyDebug.h" #include "LSM303DLH.h" LSM303DLH::LSM303DLH(PinName sda, PinName scl): dev(sda,scl) { uint8_t d[16]; INFO("LSM303DLH Init (400kHz)"); dev.frequency(400000); // Check accelerometer presence this->devRead(WHO_AM_I,d,1); if(d[0] == LSM303DH_ID) { INFO("LSM303DH(0x%02X) well detected!!",d[0]); // Config accelerometer this->basicConfig(); } else ERR("Unknow ID 0x%02X ...",d[0]); } uint8_t LSM303DLH::devRead(const uint8_t reg, uint8_t *data, uint8_t size) { uint8_t *pReg = (uint8_t*)® *pReg |= 0x80; __disable_irq(); dev.write(LSM303D_ADDR_W,(char*)pReg,1); dev.read(LSM303D_ADDR_R,(char*)data,size); __enable_irq(); return size; } uint8_t LSM303DLH::devReadSingle(const uint8_t reg, uint8_t byte) { return this->devRead(reg,&byte,1); } uint8_t LSM303DLH::devWrite(const uint8_t reg, uint8_t *data, uint8_t size) { // Check size if((size+1)>WRITE_BUFF_MAX) { ERR("BAD SIZE WRITE"); return 0; } else { // Put the reg addr in first byte place dataBuff[0] = reg | 0x80; for(int i = 0; i< size; i++) dataBuff[i+1] = data[i]; // Do the Writing dev.write(LSM303D_ADDR_W,(char*)dataBuff,size+1); return size; } } uint8_t LSM303DLH::devWriteSingle(const uint8_t reg, uint8_t byte) { return this->devWrite(reg,&byte,1); } // Hi Level API void LSM303DLH::basicConfig() { // Accelerometer // 0x00 = 0b00000000 // AFS = 0 (+/- 2 g full scale) - > 0.061 g/LSB this->devWriteSingle(CTRL2, 0x00); accLSB = 61.0; // 0x57 = 0b01010111 // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) this->devWriteSingle(CTRL1, 0x57); // Magnetometer // 0x64 = 0b01100100 // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) this->devWriteSingle(CTRL5, 0x64); // 0x20 = 0b00100000 // MFS = 01 (+/- 4 gauss full scale) this->devWriteSingle(CTRL6, 0x20); magLSB = 80.0; // 0x00 = 0b00000000 // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode) this->devWriteSingle(CTRL7, 0x00); DBG("LSM303DH configuration done\r\n"); } void LSM303DLH::readRawAcc(int16_t *x, int16_t *y, int16_t *z) { uint8_t rawData[6] = {0,0,0,0,0,0}; if(this->devRead(OUT_X_L_A,rawData,6)){ *x = (int16_t)(rawData[0] | (rawData[1] << 8)); *y = (int16_t)(rawData[2] | (rawData[3] << 8)); *z = (int16_t)(rawData[4] | (rawData[5] << 8)); } } void LSM303DLH::readAcc(float *x, float *y, float *z) { int16_t rawX; int16_t rawY; int16_t rawZ; this->readRawAcc(&rawX,&rawY,&rawZ); *x = (((float) rawX) * accLSB) /1000000.0; *y = (((float) rawY) * accLSB) /1000000.0; *z = (((float) rawZ) * accLSB) /1000000.0; } void LSM303DLH::readRawMag(int16_t *x, int16_t *y, int16_t *z) { uint8_t rawData[6] = {0,0,0,0,0,0}; if(this->devRead(OUT_X_L_M,rawData,6)){ *x = (int16_t)(rawData[0] | (rawData[1] << 8)); *y = (int16_t)(rawData[2] | (rawData[3] << 8)); *z = (int16_t)(rawData[4] | (rawData[5] << 8)); } } void LSM303DLH::readMag(float *x, float *y, float *z) { int16_t rawX; int16_t rawY; int16_t rawZ; this->readRawMag(&rawX,&rawY,&rawZ); *x = (((float) rawX) * magLSB) / 1000000.0; *y = (((float) rawY) * magLSB) / 1000000.0; *z = (((float) rawZ) * magLSB) / 1000000.0; }