Clément BENOIT / PYRN

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?

UserRevisionLine numberNew 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 }