LSM9DS1.cpp

00001 #include "LSM9DS1.h"
00002 
00003 /** Class constructor */
00004 LSM9DS1::LSM9DS1(PinName sda, PinName scl) : i2c(sda, scl)
00005 {
00006 }
00007 
00008 /** Try to initialize sensor (return true if succeed and false if failed) */
00009 bool LSM9DS1::init()
00010 {
00011     // Setup I2C bus
00012     setup_i2c();
00013 
00014     // Test I2C bus
00015     if (test_i2c()) {
00016         // Setup accelerometer and gyroscope configurations
00017         setup_gyr();
00018         setup_acc();
00019         setup_mag();
00020         return true;
00021     } else {
00022         return false;
00023     }
00024 }
00025 
00026 /** Read sensor data */
00027 void LSM9DS1::read()
00028 {
00029     // Read accelerometer and gyroscope data
00030     read_acc();
00031     read_gyr();
00032     read_mag();
00033 }
00034 
00035 /** Setup I2C bus */
00036 void LSM9DS1::setup_i2c()
00037 {
00038     // Setup I2C bus frequency to 100kHz
00039     i2c.frequency(400000);
00040 }
00041 
00042 /** Test I2C bus */
00043 bool LSM9DS1::test_i2c()
00044 {   
00045     // Register addresses
00046     char reg_acc_gyr[1] = {WHO_AM_I};
00047     char reg_mag[1] = {WHO_AM_I_M};
00048     // Data that we're going to read
00049     char data_acc_gyr[1];
00050     char data_mag[1];
00051     
00052     // Point to register address
00053     i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_acc_gyr, 1);
00054     // Read data from this address
00055     i2c.read(LSM9DS1_ADDRESS_ACC_GYR, data_acc_gyr, 1);
00056     
00057     // Point to register address
00058     i2c.write(LSM9DS1_ADDRESS_MAG, reg_mag, 1);
00059     // Read data from this address
00060     i2c.read(LSM9DS1_ADDRESS_MAG, data_mag, 1);
00061     
00062     // Check if device identity is 0x68 (acc/gyr) and 0x3D (mag)
00063     if ((data_acc_gyr[0] == 0x68) && (data_mag[0] == 0x3D)) {
00064         return true;
00065     } else {
00066         return false;
00067     }
00068 }
00069 
00070 /** Setup gyroscope configurations (full-scale range) */
00071 void LSM9DS1::setup_gyr(gyr_scale g_scale)
00072 {
00073     // Register address and data that will be writed
00074     char reg_data[2] = {CTRL_REG1_G, (0b011 << 5) | (g_scale << 3) | 0b000};
00075     
00076     // Point to register address and write data
00077     i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_data, 2);
00078 
00079     // Adjust resolution [rad/s / bit] accordingly to choose scale
00080     switch (g_scale) {
00081         case GYR_SCALE_245DPS:
00082             g_res = (245.0f*(PI/180.0f))/32768.0f;
00083             break;
00084         case GYR_SCALE_500DPS:
00085             g_res = (500.0f*(PI/180.0f))/32768.0f;
00086             break;
00087         case GYR_SCALE_2000DPS:
00088             g_res = (2000.0f*(PI/180.0f))/32768.0f;
00089             break;
00090     }
00091 }
00092 
00093 /** Setup accelerometer configurations (full-scale range) */
00094 void LSM9DS1::setup_acc(acc_scale a_scale)
00095 {
00096     // Register address and data that will be writed
00097     char reg_data[2] = {CTRL_REG6_XL, (0b011 << 5) | (a_scale << 3) | 0b000};
00098     
00099     // Point to register address and write data
00100     i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_data, 2);
00101 
00102     // Adjust resolution [m/s^2 / bit] accordingly to choose scale (g)
00103     switch (a_scale) {
00104         case ACC_SCALE_2G:
00105             a_res = (2.0f*GRAVITY)/32768.0f;
00106             break;
00107         case ACC_SCALE_4G:
00108             a_res = (4.0f*GRAVITY)/32768.0f;
00109             break;
00110         case ACC_SCALE_8G:
00111             a_res = (8.0f*GRAVITY)/32768.0f;
00112             break;
00113         case ACC_SCALE_16G:
00114             a_res = (16.0f*GRAVITY)/32768.0f;
00115             break;
00116     }
00117 }
00118 
00119 /** Setup gyroscope configurations (full-scale range) */
00120 void LSM9DS1::setup_mag(mag_scale m_scale)
00121 {
00122     // Register address and data that will be writed
00123     /*char reg_data[2] = {CTRL_REG2_M, (0b0 << 7) | (m_scale << 5) | 0b00000};
00124     
00125     // Point to register address and write data
00126     i2c.write(LSM9DS1_ADDRESS_MAG, reg_data, 2);*/
00127     
00128     char cmd[4] = {
00129         CTRL_REG1_M,
00130         0x10,       // Default data rate, xy axes mode, and temp comp
00131         m_scale << 5, // Set mag scale
00132         0           // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
00133     };
00134 
00135     // Write the data to the mag control registers
00136     i2c.write(LSM9DS1_ADDRESS_MAG, cmd, 4);
00137 
00138     // Adjust resolution [gauss / bit] accordingly to choosed scale
00139     switch (m_scale) {
00140         case MAG_SCALE_4G:
00141             m_res = 400.0f/32768.0f;
00142             break;
00143         case MAG_SCALE_8G:
00144             m_res = 800.0f/32768.0f;
00145             break;
00146         case MAG_SCALE_12G:
00147             m_res = 1200.0f/32768.0f;
00148             break;
00149         case MAG_SCALE_16G:
00150             m_res = 1600.0f/32768.0f;
00151             break;
00152     }
00153 }
00154 
00155 /** Read gyroscope data */
00156 void LSM9DS1::read_gyr()
00157 {
00158     // LSM9DS1 I2C bus address
00159     char address = LSM9DS1_ADDRESS_ACC_GYR;
00160     // Register address
00161     char reg[1] = {OUT_X_L_G};
00162     // Data that we're going to read
00163     char data[6];
00164 
00165     // Point to register address
00166     i2c.write(address, reg, 1);
00167     // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
00168     i2c.read(address, data, 6);
00169 
00170     // Reassemble the data (two 8 bit data into one 16 bit data)
00171     int16_t gx_raw = data[0] | ( data[1] << 8 );
00172     int16_t gy_raw = data[2] | ( data[3] << 8 );
00173     int16_t gz_raw = data[4] | ( data[5] << 8 );
00174     // Convert to SI units [rad/s]
00175     gx = gx_raw * g_res;
00176     gy = -gy_raw * g_res;
00177     gz = gz_raw * g_res;
00178 }
00179 
00180 /** Read accelerometer output data */
00181 void LSM9DS1::read_acc()
00182 {
00183     // LSM9DS1 I2C bus address
00184     char address = LSM9DS1_ADDRESS_ACC_GYR;
00185     // Register address
00186     char reg[1] = {OUT_X_L_XL};
00187     // Data that we're going to read
00188     char data[6];
00189 
00190     // Point to register address
00191     i2c.write(address, reg, 1);
00192     // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
00193     i2c.read(address, data, 6);
00194 
00195     // Reassemble the data (two 8 bit data into one 16 bit data)
00196     int16_t ax_raw = data[0] | ( data[1] << 8 );
00197     int16_t ay_raw = data[2] | ( data[3] << 8 );
00198     int16_t az_raw = data[4] | ( data[5] << 8 );
00199     // Convert to SI units [m/s^2]
00200     ax = -ax_raw * a_res;
00201     ay = ay_raw * a_res;
00202     az = -az_raw * a_res;
00203 }
00204 
00205 /** Read magnetometer output data */
00206 void LSM9DS1::read_mag()
00207 {
00208     // LSM9DS1 I2C bus address
00209     char address = LSM9DS1_ADDRESS_MAG;
00210     // Register address
00211     char reg[1] = {OUT_X_L_M};
00212     // Data that we're going to read
00213     char data[6];
00214 
00215     // Point to register address
00216     i2c.write(address, reg, 1);
00217     // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
00218     i2c.read(address, data, 6);
00219 
00220     // Reassemble the data (two 8 bit data into one 16 bit data)
00221     int16_t mx_raw = data[0] | ( data[1] << 8 );
00222     int16_t my_raw = data[2] | ( data[3] << 8 );
00223     int16_t mz_raw = data[4] | ( data[5] << 8 );
00224     // Convert to SI units [m/s^2]
00225     mx = -mx_raw * m_res;
00226     my = -my_raw * m_res;
00227     mz = mz_raw * m_res;
00228 }