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@0:efe6085327fd, 2015-04-14 (annotated)
- Committer:
- clemounet
- Date:
- Tue Apr 14 13:30:02 2015 +0000
- Revision:
- 0:efe6085327fd
All the projet PYRN 3G-CAN-ACC...
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
clemounet | 0:efe6085327fd | 1 | |
clemounet | 0:efe6085327fd | 2 | #define __DEBUG__ 0 |
clemounet | 0:efe6085327fd | 3 | #ifndef __MODULE__ |
clemounet | 0:efe6085327fd | 4 | #define __MODULE__ "LSM303DLH.cpp" |
clemounet | 0:efe6085327fd | 5 | #endif |
clemounet | 0:efe6085327fd | 6 | #include "MyDebug.h" |
clemounet | 0:efe6085327fd | 7 | |
clemounet | 0:efe6085327fd | 8 | #include "LSM303DLH.h" |
clemounet | 0:efe6085327fd | 9 | |
clemounet | 0:efe6085327fd | 10 | LSM303DLH::LSM303DLH(PinName sda, PinName scl): dev(sda,scl) { |
clemounet | 0:efe6085327fd | 11 | uint8_t d[16]; |
clemounet | 0:efe6085327fd | 12 | INFO("LSM303DLH Init (400kHz)"); |
clemounet | 0:efe6085327fd | 13 | dev.frequency(400000); |
clemounet | 0:efe6085327fd | 14 | // Check accelerometer presence |
clemounet | 0:efe6085327fd | 15 | this->devRead(WHO_AM_I,d,1); |
clemounet | 0:efe6085327fd | 16 | if(d[0] == LSM303DH_ID) { |
clemounet | 0:efe6085327fd | 17 | INFO("LSM303DH(0x%02X) well detected!!",d[0]); |
clemounet | 0:efe6085327fd | 18 | // Config accelerometer |
clemounet | 0:efe6085327fd | 19 | this->basicConfig(); |
clemounet | 0:efe6085327fd | 20 | } else |
clemounet | 0:efe6085327fd | 21 | ERR("Unknow ID 0x%02X ...",d[0]); |
clemounet | 0:efe6085327fd | 22 | } |
clemounet | 0:efe6085327fd | 23 | |
clemounet | 0:efe6085327fd | 24 | uint8_t LSM303DLH::devRead(const uint8_t reg, uint8_t *data, uint8_t size) { |
clemounet | 0:efe6085327fd | 25 | uint8_t *pReg = (uint8_t*)® |
clemounet | 0:efe6085327fd | 26 | *pReg |= 0x80; |
clemounet | 0:efe6085327fd | 27 | __disable_irq(); |
clemounet | 0:efe6085327fd | 28 | dev.write(LSM303D_ADDR_W,(char*)pReg,1); |
clemounet | 0:efe6085327fd | 29 | dev.read(LSM303D_ADDR_R,(char*)data,size); |
clemounet | 0:efe6085327fd | 30 | __enable_irq(); |
clemounet | 0:efe6085327fd | 31 | return size; |
clemounet | 0:efe6085327fd | 32 | } |
clemounet | 0:efe6085327fd | 33 | |
clemounet | 0:efe6085327fd | 34 | uint8_t LSM303DLH::devReadSingle(const uint8_t reg, uint8_t byte) { |
clemounet | 0:efe6085327fd | 35 | return this->devRead(reg,&byte,1); |
clemounet | 0:efe6085327fd | 36 | } |
clemounet | 0:efe6085327fd | 37 | |
clemounet | 0:efe6085327fd | 38 | uint8_t LSM303DLH::devWrite(const uint8_t reg, uint8_t *data, uint8_t size) { |
clemounet | 0:efe6085327fd | 39 | // Check size |
clemounet | 0:efe6085327fd | 40 | if((size+1)>WRITE_BUFF_MAX) { |
clemounet | 0:efe6085327fd | 41 | ERR("BAD SIZE WRITE"); |
clemounet | 0:efe6085327fd | 42 | return 0; |
clemounet | 0:efe6085327fd | 43 | } else { |
clemounet | 0:efe6085327fd | 44 | // Put the reg addr in first byte place |
clemounet | 0:efe6085327fd | 45 | dataBuff[0] = reg | 0x80; |
clemounet | 0:efe6085327fd | 46 | for(int i = 0; i< size; i++) |
clemounet | 0:efe6085327fd | 47 | dataBuff[i+1] = data[i]; |
clemounet | 0:efe6085327fd | 48 | // Do the Writing |
clemounet | 0:efe6085327fd | 49 | dev.write(LSM303D_ADDR_W,(char*)dataBuff,size+1); |
clemounet | 0:efe6085327fd | 50 | return size; |
clemounet | 0:efe6085327fd | 51 | } |
clemounet | 0:efe6085327fd | 52 | } |
clemounet | 0:efe6085327fd | 53 | |
clemounet | 0:efe6085327fd | 54 | uint8_t LSM303DLH::devWriteSingle(const uint8_t reg, uint8_t byte) { |
clemounet | 0:efe6085327fd | 55 | return this->devWrite(reg,&byte,1); |
clemounet | 0:efe6085327fd | 56 | } |
clemounet | 0:efe6085327fd | 57 | |
clemounet | 0:efe6085327fd | 58 | // Hi Level API |
clemounet | 0:efe6085327fd | 59 | |
clemounet | 0:efe6085327fd | 60 | void LSM303DLH::basicConfig() { |
clemounet | 0:efe6085327fd | 61 | // Accelerometer |
clemounet | 0:efe6085327fd | 62 | // 0x00 = 0b00000000 |
clemounet | 0:efe6085327fd | 63 | // AFS = 0 (+/- 2 g full scale) - > 0.061 g/LSB |
clemounet | 0:efe6085327fd | 64 | this->devWriteSingle(CTRL2, 0x00); |
clemounet | 0:efe6085327fd | 65 | accLSB = 61.0; |
clemounet | 0:efe6085327fd | 66 | // 0x57 = 0b01010111 |
clemounet | 0:efe6085327fd | 67 | // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) |
clemounet | 0:efe6085327fd | 68 | this->devWriteSingle(CTRL1, 0x57); |
clemounet | 0:efe6085327fd | 69 | |
clemounet | 0:efe6085327fd | 70 | // Magnetometer |
clemounet | 0:efe6085327fd | 71 | // 0x64 = 0b01100100 |
clemounet | 0:efe6085327fd | 72 | // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) |
clemounet | 0:efe6085327fd | 73 | this->devWriteSingle(CTRL5, 0x64); |
clemounet | 0:efe6085327fd | 74 | // 0x20 = 0b00100000 |
clemounet | 0:efe6085327fd | 75 | // MFS = 01 (+/- 4 gauss full scale) |
clemounet | 0:efe6085327fd | 76 | this->devWriteSingle(CTRL6, 0x20); |
clemounet | 0:efe6085327fd | 77 | magLSB = 80.0; |
clemounet | 0:efe6085327fd | 78 | // 0x00 = 0b00000000 |
clemounet | 0:efe6085327fd | 79 | // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode) |
clemounet | 0:efe6085327fd | 80 | this->devWriteSingle(CTRL7, 0x00); |
clemounet | 0:efe6085327fd | 81 | |
clemounet | 0:efe6085327fd | 82 | DBG("LSM303DH configuration done\r\n"); |
clemounet | 0:efe6085327fd | 83 | } |
clemounet | 0:efe6085327fd | 84 | |
clemounet | 0:efe6085327fd | 85 | void LSM303DLH::readRawAcc(int16_t *x, int16_t *y, int16_t *z) { |
clemounet | 0:efe6085327fd | 86 | uint8_t rawData[6] = {0,0,0,0,0,0}; |
clemounet | 0:efe6085327fd | 87 | if(this->devRead(OUT_X_L_A,rawData,6)){ |
clemounet | 0:efe6085327fd | 88 | *x = (int16_t)(rawData[0] | (rawData[1] << 8)); |
clemounet | 0:efe6085327fd | 89 | *y = (int16_t)(rawData[2] | (rawData[3] << 8)); |
clemounet | 0:efe6085327fd | 90 | *z = (int16_t)(rawData[4] | (rawData[5] << 8)); |
clemounet | 0:efe6085327fd | 91 | } |
clemounet | 0:efe6085327fd | 92 | } |
clemounet | 0:efe6085327fd | 93 | |
clemounet | 0:efe6085327fd | 94 | void LSM303DLH::readAcc(float *x, float *y, float *z) { |
clemounet | 0:efe6085327fd | 95 | int16_t rawX; |
clemounet | 0:efe6085327fd | 96 | int16_t rawY; |
clemounet | 0:efe6085327fd | 97 | int16_t rawZ; |
clemounet | 0:efe6085327fd | 98 | this->readRawAcc(&rawX,&rawY,&rawZ); |
clemounet | 0:efe6085327fd | 99 | *x = (((float) rawX) * accLSB) /1000000.0; |
clemounet | 0:efe6085327fd | 100 | *y = (((float) rawY) * accLSB) /1000000.0; |
clemounet | 0:efe6085327fd | 101 | *z = (((float) rawZ) * accLSB) /1000000.0; |
clemounet | 0:efe6085327fd | 102 | } |
clemounet | 0:efe6085327fd | 103 | |
clemounet | 0:efe6085327fd | 104 | |
clemounet | 0:efe6085327fd | 105 | void LSM303DLH::readRawMag(int16_t *x, int16_t *y, int16_t *z) { |
clemounet | 0:efe6085327fd | 106 | uint8_t rawData[6] = {0,0,0,0,0,0}; |
clemounet | 0:efe6085327fd | 107 | if(this->devRead(OUT_X_L_M,rawData,6)){ |
clemounet | 0:efe6085327fd | 108 | *x = (int16_t)(rawData[0] | (rawData[1] << 8)); |
clemounet | 0:efe6085327fd | 109 | *y = (int16_t)(rawData[2] | (rawData[3] << 8)); |
clemounet | 0:efe6085327fd | 110 | *z = (int16_t)(rawData[4] | (rawData[5] << 8)); |
clemounet | 0:efe6085327fd | 111 | } |
clemounet | 0:efe6085327fd | 112 | } |
clemounet | 0:efe6085327fd | 113 | |
clemounet | 0:efe6085327fd | 114 | void LSM303DLH::readMag(float *x, float *y, float *z) { |
clemounet | 0:efe6085327fd | 115 | int16_t rawX; |
clemounet | 0:efe6085327fd | 116 | int16_t rawY; |
clemounet | 0:efe6085327fd | 117 | int16_t rawZ; |
clemounet | 0:efe6085327fd | 118 | this->readRawMag(&rawX,&rawY,&rawZ); |
clemounet | 0:efe6085327fd | 119 | *x = (((float) rawX) * magLSB) / 1000000.0; |
clemounet | 0:efe6085327fd | 120 | *y = (((float) rawY) * magLSB) / 1000000.0; |
clemounet | 0:efe6085327fd | 121 | *z = (((float) rawZ) * magLSB) / 1000000.0; |
clemounet | 0:efe6085327fd | 122 | } |