LSM9DS1.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 
00004 //spi8,はmbedのSPIクラスのインスタンス
00005 LSM9DS1::LSM9DS1(PinName _mosi, PinName _miso, PinName _sclk, PinName _cs_AG, PinName _cs_M) : spi8(_mosi, _miso, _sclk),cs_AG(_cs_AG),cs_M(_cs_M) {
00006     initSPI();
00007 }
00008 
00009 void LSM9DS1::initSPI(void){
00010     cs_AG = 1; //deselect
00011     cs_M =1;
00012     spi8.format(8,3);
00013     spi8.frequency(10000000);  
00014 }
00015 
00016 void LSM9DS1::initAccel(lsm9ds1AccelRange_t range){ 
00017     // Enable the accelerometer continous
00018     write8(LSM9DS1_cs_AG, LSM9DS1_REGISTER_CTRL_REG5_XL, 0x38); // enable X Y and Z axis    
00019     write8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_CTRL_REG6_XL,0xC0 | range); // 1 KHz out data rate, BW set by ODR, 408Hz anti-aliasing 感度設定
00020 
00021     switch (range)
00022     {
00023         case LSM9DS1_ACCELRANGE_2G:
00024             _accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_2G;
00025             break;
00026         case LSM9DS1_ACCELRANGE_4G:
00027             _accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_4G;
00028             break;
00029         case LSM9DS1_ACCELRANGE_8G:
00030             _accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_8G;
00031             break;    
00032         case LSM9DS1_ACCELRANGE_16G:
00033             _accel_mg_lsb =LSM9DS1_ACCEL_MG_LSB_16G;
00034             break;
00035     }
00036 }
00037 
00038 
00039 void LSM9DS1::initGyro(lsm9ds1GyroScale_t scale){
00040     write8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_CTRL_REG1_G,0xC0 | scale); //感度設定
00041     
00042     switch(scale)
00043     {
00044         case LSM9DS1_GYROSCALE_245DPS:
00045             _gyro_dps_digit = LSM9DS1_GYRO_DPS_DIGIT_245DPS;
00046             break;
00047         case LSM9DS1_GYROSCALE_500DPS:
00048             _gyro_dps_digit = LSM9DS1_GYRO_DPS_DIGIT_500DPS;
00049             break;
00050         case LSM9DS1_GYROSCALE_2000DPS:
00051             _gyro_dps_digit = LSM9DS1_GYRO_DPS_DIGIT_2000DPS;
00052             break;
00053     }
00054 }
00055 
00056 void LSM9DS1::initMag(lsm9ds1MagGain_t gain){
00057     write8(LSM9DS1_cs_M,LSM9DS1_REGISTER_CTRL_REG1_M,0xFC);
00058     write8(LSM9DS1_cs_M, LSM9DS1_REGISTER_CTRL_REG2_M, gain ); //感度設定
00059     write8(LSM9DS1_cs_M,LSM9DS1_REGISTER_CTRL_REG3_M,0x00);
00060     write8(LSM9DS1_cs_M,LSM9DS1_REGISTER_CTRL_REG4_M,0x0C);
00061 
00062     switch(gain)
00063     {
00064         case LSM9DS1_MAGGAIN_4GAUSS:
00065             _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_4GAUSS;
00066             break;
00067         case LSM9DS1_MAGGAIN_8GAUSS:
00068             _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_8GAUSS;
00069             break;
00070         case LSM9DS1_MAGGAIN_12GAUSS:
00071             _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_12GAUSS;
00072             break;
00073         case LSM9DS1_MAGGAIN_16GAUSS:
00074             _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_16GAUSS;
00075             break;
00076     }
00077 }
00078 
00079 void LSM9DS1::readAccel() {
00080     // Read the accelerometer
00081 
00082     uint8_t xlo = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_X_L_XL);
00083     int16_t xhi = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_X_H_XL);
00084     uint8_t ylo = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Y_L_XL);
00085     int16_t yhi = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Y_H_XL);
00086     uint8_t zlo = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Z_L_XL);
00087     int16_t zhi = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Z_H_XL);
00088 
00089     // Shift values to create properly formed integer (low byte first)
00090     xhi <<= 8; xhi |= xlo;
00091     yhi <<= 8; yhi |= ylo;
00092     zhi <<= 8; zhi |= zlo;
00093     accel_x = (xhi * _accel_mg_lsb)/100;
00094     accel_y = (yhi * _accel_mg_lsb)/100;
00095     accel_z = (zhi * _accel_mg_lsb)/100;
00096 }
00097 
00098 void LSM9DS1::readGyro(){
00099 
00100     uint8_t xlo = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_X_L_G);
00101     int16_t xhi = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_X_H_G);
00102     uint8_t ylo = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Y_L_G);
00103     int16_t yhi = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Y_H_G);
00104     uint8_t zlo = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Z_L_G);
00105     int16_t zhi = read8(LSM9DS1_cs_AG,LSM9DS1_REGISTER_OUT_Z_H_G);
00106 
00107     // Shift values to create properly formed integer (low byte first)
00108     xhi <<= 8; xhi |= xlo;
00109     yhi <<= 8; yhi |= ylo;
00110     zhi <<= 8; zhi |= zlo;
00111 
00112     gyro_x = xhi * _gyro_dps_digit;
00113     gyro_y = yhi * _gyro_dps_digit;
00114     gyro_z = zhi * _gyro_dps_digit;
00115 }
00116 
00117 void LSM9DS1::readMag(){
00118 
00119 
00120     uint8_t xlo = read8(LSM9DS1_cs_M,LSM9DS1_REGISTER_OUT_X_L_M);
00121     int16_t xhi = read8(LSM9DS1_cs_M,LSM9DS1_REGISTER_OUT_X_H_M);
00122     uint8_t ylo = read8(LSM9DS1_cs_M,LSM9DS1_REGISTER_OUT_Y_L_M);
00123     int16_t yhi = read8(LSM9DS1_cs_M,LSM9DS1_REGISTER_OUT_Y_H_M);
00124     uint8_t zlo = read8(LSM9DS1_cs_M,LSM9DS1_REGISTER_OUT_Z_L_M);
00125     int16_t zhi = read8(LSM9DS1_cs_M,LSM9DS1_REGISTER_OUT_Z_H_M);
00126 
00127     // Shift values to create properly formed integer (low byte first)
00128     xhi <<= 8; xhi |= xlo;
00129     yhi <<= 8; yhi |= ylo;
00130     zhi <<= 8; zhi |= zlo;
00131 
00132     mag_x = xhi * _mag_mgauss_lsb;
00133     mag_y = yhi * _mag_mgauss_lsb;
00134     mag_z = zhi * _mag_mgauss_lsb;
00135 }
00136 
00137 uint8_t LSM9DS1::whoami(void){
00138     return read8(LSM9DS1_cs_AG,0x0F);
00139 }
00140 
00141 uint8_t LSM9DS1::read8(lsm9ds1ChipSelect_t cs,uint8_t address){
00142     if(cs == LSM9DS1_cs_AG) cs_AG = 0;
00143     else if(cs == LSM9DS1_cs_M)  cs_M  = 0;
00144     spi8.write(address | 0x80); //先頭ビットを1にするとreadモードになる
00145     uint8_t x = spi8.write(0x00);   //ダミーリードして読み込み
00146     if(cs == LSM9DS1_cs_AG) cs_AG = 1;
00147     else if(cs == LSM9DS1_cs_M)  cs_M  = 1;
00148     return x;
00149 }
00150 
00151 void LSM9DS1::write8(lsm9ds1ChipSelect_t cs,uint8_t address,uint8_t data){
00152     if(cs == LSM9DS1_cs_AG) cs_AG = 0;
00153     else if(cs == LSM9DS1_cs_M)  cs_M  = 0;
00154     spi8.write(address);
00155     spi8.write(data);
00156     if(cs == LSM9DS1_cs_AG) cs_AG = 1;
00157     else if(cs == LSM9DS1_cs_M)  cs_M  = 1;
00158 }