LSM9DS1_SPI.cpp

00001 // Written by: Erik van de Coevering 
00002 
00003 #include "mbed.h"
00004 #include "LSM9DS1_SPI.h"
00005 #include "LSM9DS1_SPI_Registers.h"
00006 
00007 lsm9ds1_spi::lsm9ds1_spi(SPI& _spi, PinName _csAG, PinName _csM) : spi(_spi), csAG(_csAG), csM(_csM) {}
00008 
00009 unsigned int lsm9ds1_spi::WriteRegAG( uint8_t WriteAddr, uint8_t WriteData)
00010 {
00011       unsigned int temp_val;
00012     selectAG();
00013     spi.write(WriteAddr);
00014     temp_val=spi.write(WriteData);
00015     deselectAG();
00016     wait_us(5);
00017     return temp_val;
00018 }
00019 
00020 unsigned int lsm9ds1_spi::WriteRegM( uint8_t WriteAddr, uint8_t WriteData)
00021 {
00022       unsigned int temp_val;
00023     selectM();
00024     spi.write(WriteAddr);
00025     temp_val=spi.write(WriteData);
00026     deselectM();
00027     wait_us(5);
00028     return temp_val;
00029 }
00030 
00031 unsigned int lsm9ds1_spi::ReadRegAG( uint8_t WriteAddr, uint8_t WriteData)
00032 {
00033         return WriteRegAG(WriteAddr | READ_FLAG, WriteData);
00034 }
00035 
00036 unsigned int lsm9ds1_spi::ReadRegM( uint8_t WriteAddr, uint8_t WriteData)
00037 {
00038         return WriteRegM(WriteAddr | READ_FLAG, WriteData);
00039 }
00040 
00041 void lsm9ds1_spi::ReadRegsAG( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
00042 {
00043     unsigned int  i = 0;
00044 
00045     selectAG();
00046     spi.write(ReadAddr | READ_FLAG);
00047     for(i=0; i<Bytes; i++)
00048         ReadBuf[i] = spi.write(0x00);
00049     deselectAG();
00050     //wait_us(50);
00051 }
00052 
00053 void lsm9ds1_spi::ReadRegsM( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
00054 {
00055     unsigned int  i = 0;
00056 
00057     selectM();
00058     spi.write(ReadAddr | READ_FLAG);
00059     for(i=0; i<Bytes; i++)
00060         ReadBuf[i] = spi.write(0x00);
00061     deselectM();
00062 }
00063 
00064 #define LSM_InitRegNumAG 10
00065 #define LSM_InitRegNumM 6
00066 
00067 /*--------------------------------------INIT-----------------------------------------------
00068 Possible values for BW / FS / ODR:
00069 
00070 Gyroscope full-scale setting:
00071 FS_G_245DPS         
00072 FS_G_500DPS         
00073 FS_G_2000DPS
00074 
00075 Gyroscope output data rate setting:
00076 ODR_G_15HZ          
00077 ODR_G_60HZ          
00078 ODR_G_119HZ         
00079 ODR_G_238HZ         
00080 ODR_G_476HZ         
00081 ODR_G_952HZ 
00082 
00083 Gyro bandwidth dependant on ODR, these freq's are correct for ODR=476Hz or 952Hz. Check datasheet for other ODR's
00084 BW_G_33HZ           
00085 BW_G_40HZ           
00086 BW_G_58HZ
00087 BW_G_100HZ
00088 
00089 Accelerometer full-scale setting:
00090 FS_A_2G
00091 FS_A_4G
00092 FS_A_8G
00093 FS_A_16G
00094 
00095 Magnetometer full-scale setting:
00096 FS_M_4GAUSS
00097 FS_M_8GAUSS 
00098 FS_M_12GAUSS
00099 FS_M_16GAUSS
00100 
00101 
00102 Set sensitivity according to full-scale settings. Possible values are:
00103 ACC_SENS_2G         
00104 ACC_SENS_4G         
00105 ACC_SENS_8G         
00106 ACC_SENS_16G        
00107 GYR_SENS_245DPS     
00108 GYR_SENS_500DPS     
00109 GYR_SENS_2000DPS    
00110 MAG_SENS_4GAUSS     
00111 MAG_SENS_8GAUSS     
00112 MAG_SENS_12GAUSS    
00113 MAG_SENS_16GAUSS    
00114 */
00115 
00116 
00117 void lsm9ds1_spi::init() {
00118     
00119     deselectAG();
00120     deselectM();
00121     
00122     uint8_t LSM_Init_Data[LSM_InitRegNumAG][2] = {
00123         {ODR_G_952HZ | FS_G_500DPS | BW_G_100HZ, CTRL_REG1_G},
00124         {0x00, CTRL_REG2_G}, // enable FIFO, disable HPF/LPF2 <-- something seems to be wrong with the filters, check datasheet if you want to use them
00125         {0x09, CTRL_REG3_G}, // Disable HPF
00126         {0x00, ORIENT_CFG_G},
00127         {0x38, CTRL_REG4},      //Enable gyro outputs, no latched interrupt
00128         {0x38, CTRL_REG5_XL}, //No decimation of accel data, Enable accelerometer outputs
00129         {0xC0 | FS_A_2G, CTRL_REG6_XL}, // Accel ODR 952Hz, BW 408 Hz, +-2g
00130         {0xC0, CTRL_REG7_XL}, //LP cutoff ODR/9, LPF enabled, HPF bypassed, bypass LPF = 0xC0
00131         {0x16, CTRL_REG9},      //Temperature FIFO enabled, I2C disabled, FIFO enabled
00132         {0xC0, FIFO_CTRL},      //Continuous mode, overwrite if FIFO is full
00133     };
00134     
00135     uint8_t LSM_Init_DataM[LSM_InitRegNumM][2] = {
00136         {0x7C, CTRL_REG1_M}, // Ultra-high performance mode (x & y axis), ODR 80Hz, no temp comp
00137         {FS_M_4GAUSS, CTRL_REG2_M}, // FS +- 4 gauss
00138         {0x84, CTRL_REG3_M}, // Disable I2C, enable SPI read/write, continuous-conversion mode
00139         {0x0C, CTRL_REG4_M}, // Ultra-high performance mode (z-axis)
00140         {0x00, CTRL_REG5_M}, // Fast read disabled, continuous update
00141         {0x02, INT_CFG_M}, 
00142     };
00143     spi.format(8,3);
00144     spi.frequency(10000000);
00145     
00146     for(int i=0; i<LSM_InitRegNumAG; i++) {
00147         WriteRegAG(LSM_Init_Data[i][1], LSM_Init_Data[i][0]);
00148     }
00149     
00150     for(int i=0; i<LSM_InitRegNumM; i++) {
00151         WriteRegM(LSM_Init_DataM[i][1], LSM_Init_DataM[i][0]);
00152     }
00153     
00154     // Sensitivity settings
00155     acc_multiplier = ACC_SENS_2G;
00156     gyro_multiplier = GYR_SENS_500DPS;
00157     mag_multiplier = MAG_SENS_4GAUSS;
00158 }
00159 
00160 void lsm9ds1_spi::read_acc()
00161 {
00162     uint8_t response[6];
00163     int16_t bit_data;
00164     float data;
00165     int i;
00166     ReadRegsAG(OUT_X_L_XL,response,6);
00167     for(i=0; i<3; i++) {
00168         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00169         data=(float)bit_data;
00170         accelerometer_data[i]=data*acc_multiplier;
00171     }
00172     
00173 }
00174 
00175 void lsm9ds1_spi::read_gyr()
00176 {
00177     uint8_t response[6];
00178     int16_t bit_data;
00179     float data;
00180     int i;
00181     ReadRegsAG(OUT_X_L_G,response,6);
00182     for(i=0; i<3; i++) {
00183         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00184         data=(float)bit_data;
00185         gyroscope_data[i]=data*gyro_multiplier;
00186     }
00187     
00188 }
00189 
00190 float lsm9ds1_spi::read_temp(){
00191     uint8_t response[2];
00192     int16_t bit_data;
00193     float data;
00194     ReadRegsAG(OUT_TEMP_L,response,2);
00195 
00196     bit_data=((int16_t)response[1]<<8)|response[0];
00197     data=(float)bit_data;
00198     data = data/16;
00199         return (data+25);
00200 }
00201 
00202 void lsm9ds1_spi::read_mag()
00203 {
00204     uint8_t response[6];
00205     int16_t bit_data;
00206     float data;
00207     int i;
00208     ReadRegsM(OUT_X_L_M,response,6);
00209     for(i=0; i<3; i++) {
00210         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00211         data=(float)bit_data;
00212         magnetometer_data[i]=data*mag_multiplier;
00213     }
00214     
00215 }
00216 
00217 void lsm9ds1_spi::read_all()
00218 {
00219         read_acc();
00220         read_gyr();
00221         read_mag();
00222 }
00223 
00224 unsigned int lsm9ds1_spi::whoami() 
00225 {
00226         return ReadRegAG(WHO_AM_I_XG, 0x00);
00227 }
00228 
00229 unsigned int lsm9ds1_spi::whoamiM() 
00230 {
00231         return ReadRegM(WHO_AM_I_M, 0x00);
00232 }
00233 
00234 void lsm9ds1_spi::selectAG() {
00235         csAG = 0;
00236 }
00237 
00238 void lsm9ds1_spi::selectM() {
00239         csM = 0;
00240 }
00241 
00242 void lsm9ds1_spi::deselectAG() {
00243         csAG = 1;
00244 }
00245 
00246 void lsm9ds1_spi::deselectM() {
00247         csM = 1;
00248 }
00249