Fabio Bobrow / Cubli

Cubli library

LSM9DS1/LSM9DS1.cpp@2:dc7840a67f77, 2019-02-25 (annotated)

Committer:
fbob
Date:
Mon Feb 25 16:39:52 2019 +0000
Revision:
2:dc7840a67f77
Parent:
0:431ee55036ca 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 0:431ee55036ca 1 #include "LSM9DS1.h"
fbob 0:431ee55036ca 2
fbob 0:431ee55036ca 3 /** Class constructor */
fbob 0:431ee55036ca 4 LSM9DS1::LSM9DS1(PinName sda, PinName scl) : i2c(sda, scl)
fbob 0:431ee55036ca 5 {
fbob 0:431ee55036ca 6 }
fbob 0:431ee55036ca 7
fbob 0:431ee55036ca 8 /** Try to initialize sensor (return true if succeed and false if failed) */
fbob 0:431ee55036ca 9 bool LSM9DS1::init()
fbob 0:431ee55036ca 10 {
fbob 0:431ee55036ca 11 // Setup I2C bus
fbob 0:431ee55036ca 12 setup_i2c();
fbob 0:431ee55036ca 13
fbob 0:431ee55036ca 14 // Test I2C bus
fbob 0:431ee55036ca 15 if (test_i2c()) {
fbob 0:431ee55036ca 16 // Setup accelerometer and gyroscope configurations
fbob 2:dc7840a67f77 17 setup_gyr();
fbob 2:dc7840a67f77 18 setup_acc();
fbob 2:dc7840a67f77 19 setup_mag();
fbob 0:431ee55036ca 20 return true;
fbob 0:431ee55036ca 21 } else {
fbob 0:431ee55036ca 22 return false;
fbob 0:431ee55036ca 23 }
fbob 0:431ee55036ca 24 }
fbob 0:431ee55036ca 25
fbob 0:431ee55036ca 26 /** Read sensor data */
fbob 0:431ee55036ca 27 void LSM9DS1::read()
fbob 0:431ee55036ca 28 {
fbob 0:431ee55036ca 29 // Read accelerometer and gyroscope data
fbob 2:dc7840a67f77 30 read_acc();
fbob 2:dc7840a67f77 31 read_gyr();
fbob 2:dc7840a67f77 32 read_mag();
fbob 0:431ee55036ca 33 }
fbob 0:431ee55036ca 34
fbob 0:431ee55036ca 35 /** Setup I2C bus */
fbob 0:431ee55036ca 36 void LSM9DS1::setup_i2c()
fbob 0:431ee55036ca 37 {
fbob 0:431ee55036ca 38 // Setup I2C bus frequency to 100kHz
fbob 0:431ee55036ca 39 i2c.frequency(400000);
fbob 0:431ee55036ca 40 }
fbob 0:431ee55036ca 41
fbob 0:431ee55036ca 42 /** Test I2C bus */
fbob 0:431ee55036ca 43 bool LSM9DS1::test_i2c()
fbob 2:dc7840a67f77 44 {
fbob 2:dc7840a67f77 45 // Register addresses
fbob 2:dc7840a67f77 46 char reg_acc_gyr[1] = {WHO_AM_I};
fbob 2:dc7840a67f77 47 char reg_mag[1] = {WHO_AM_I_M};
fbob 2:dc7840a67f77 48 // Data that we're going to read
fbob 2:dc7840a67f77 49 char data_acc_gyr[1];
fbob 2:dc7840a67f77 50 char data_mag[1];
fbob 2:dc7840a67f77 51
fbob 2:dc7840a67f77 52 // Point to register address
fbob 2:dc7840a67f77 53 i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_acc_gyr, 1);
fbob 2:dc7840a67f77 54 // Read data from this address
fbob 2:dc7840a67f77 55 i2c.read(LSM9DS1_ADDRESS_ACC_GYR, data_acc_gyr, 1);
fbob 2:dc7840a67f77 56
fbob 2:dc7840a67f77 57 // Point to register address
fbob 2:dc7840a67f77 58 i2c.write(LSM9DS1_ADDRESS_MAG, reg_mag, 1);
fbob 2:dc7840a67f77 59 // Read data from this address
fbob 2:dc7840a67f77 60 i2c.read(LSM9DS1_ADDRESS_MAG, data_mag, 1);
fbob 2:dc7840a67f77 61
fbob 2:dc7840a67f77 62 // Check if device identity is 0x68 (acc/gyr) and 0x3D (mag)
fbob 2:dc7840a67f77 63 if ((data_acc_gyr[0] == 0x68) && (data_mag[0] == 0x3D)) {
fbob 0:431ee55036ca 64 return true;
fbob 0:431ee55036ca 65 } else {
fbob 0:431ee55036ca 66 return false;
fbob 0:431ee55036ca 67 }
fbob 0:431ee55036ca 68 }
fbob 0:431ee55036ca 69
fbob 2:dc7840a67f77 70 /** Setup gyroscope configurations (full-scale range) */
fbob 2:dc7840a67f77 71 void LSM9DS1::setup_gyr(gyr_scale g_scale)
fbob 0:431ee55036ca 72 {
fbob 2:dc7840a67f77 73 // Register address and data that will be writed
fbob 2:dc7840a67f77 74 char reg_data[2] = {CTRL_REG1_G, (0b011 << 5) | (g_scale << 3) | 0b000};
fbob 2:dc7840a67f77 75
fbob 2:dc7840a67f77 76 // Point to register address and write data
fbob 2:dc7840a67f77 77 i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_data, 2);
fbob 2:dc7840a67f77 78
fbob 2:dc7840a67f77 79 // Adjust resolution [rad/s / bit] accordingly to choose scale
fbob 2:dc7840a67f77 80 switch (g_scale) {
fbob 2:dc7840a67f77 81 case GYR_SCALE_245DPS:
fbob 2:dc7840a67f77 82 g_res = (245.0f*(PI/180.0f))/32768.0f;
fbob 2:dc7840a67f77 83 break;
fbob 2:dc7840a67f77 84 case GYR_SCALE_500DPS:
fbob 2:dc7840a67f77 85 g_res = (500.0f*(PI/180.0f))/32768.0f;
fbob 2:dc7840a67f77 86 break;
fbob 2:dc7840a67f77 87 case GYR_SCALE_2000DPS:
fbob 2:dc7840a67f77 88 g_res = (2000.0f*(PI/180.0f))/32768.0f;
fbob 2:dc7840a67f77 89 break;
fbob 2:dc7840a67f77 90 }
fbob 2:dc7840a67f77 91 }
fbob 2:dc7840a67f77 92
fbob 2:dc7840a67f77 93 /** Setup accelerometer configurations (full-scale range) */
fbob 2:dc7840a67f77 94 void LSM9DS1::setup_acc(acc_scale a_scale)
fbob 2:dc7840a67f77 95 {
fbob 2:dc7840a67f77 96 // Register address and data that will be writed
fbob 2:dc7840a67f77 97 char reg_data[2] = {CTRL_REG6_XL, (0b011 << 5) | (a_scale << 3) | 0b000};
fbob 2:dc7840a67f77 98
fbob 2:dc7840a67f77 99 // Point to register address and write data
fbob 2:dc7840a67f77 100 i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_data, 2);
fbob 0:431ee55036ca 101
fbob 0:431ee55036ca 102 // Adjust resolution [m/s^2 / bit] accordingly to choose scale (g)
fbob 0:431ee55036ca 103 switch (a_scale) {
fbob 2:dc7840a67f77 104 case ACC_SCALE_2G:
fbob 0:431ee55036ca 105 a_res = (2.0f*GRAVITY)/32768.0f;
fbob 0:431ee55036ca 106 break;
fbob 2:dc7840a67f77 107 case ACC_SCALE_4G:
fbob 0:431ee55036ca 108 a_res = (4.0f*GRAVITY)/32768.0f;
fbob 0:431ee55036ca 109 break;
fbob 2:dc7840a67f77 110 case ACC_SCALE_8G:
fbob 0:431ee55036ca 111 a_res = (8.0f*GRAVITY)/32768.0f;
fbob 0:431ee55036ca 112 break;
fbob 2:dc7840a67f77 113 case ACC_SCALE_16G:
fbob 0:431ee55036ca 114 a_res = (16.0f*GRAVITY)/32768.0f;
fbob 0:431ee55036ca 115 break;
fbob 0:431ee55036ca 116 }
fbob 0:431ee55036ca 117 }
fbob 0:431ee55036ca 118
fbob 0:431ee55036ca 119 /** Setup gyroscope configurations (full-scale range) */
fbob 2:dc7840a67f77 120 void LSM9DS1::setup_mag(mag_scale m_scale)
fbob 0:431ee55036ca 121 {
fbob 2:dc7840a67f77 122 // Register address and data that will be writed
fbob 2:dc7840a67f77 123 /*char reg_data[2] = {CTRL_REG2_M, (0b0 << 7) | (m_scale << 5) | 0b00000};
fbob 2:dc7840a67f77 124
fbob 2:dc7840a67f77 125 // Point to register address and write data
fbob 2:dc7840a67f77 126 i2c.write(LSM9DS1_ADDRESS_MAG, reg_data, 2);*/
fbob 2:dc7840a67f77 127
fbob 2:dc7840a67f77 128 char cmd[4] = {
fbob 2:dc7840a67f77 129 CTRL_REG1_M,
fbob 2:dc7840a67f77 130 0x10, // Default data rate, xy axes mode, and temp comp
fbob 2:dc7840a67f77 131 m_scale << 5, // Set mag scale
fbob 2:dc7840a67f77 132 0 // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
fbob 2:dc7840a67f77 133 };
fbob 0:431ee55036ca 134
fbob 2:dc7840a67f77 135 // Write the data to the mag control registers
fbob 2:dc7840a67f77 136 i2c.write(LSM9DS1_ADDRESS_MAG, cmd, 4);
fbob 2:dc7840a67f77 137
fbob 2:dc7840a67f77 138 // Adjust resolution [gauss / bit] accordingly to choosed scale
fbob 2:dc7840a67f77 139 switch (m_scale) {
fbob 2:dc7840a67f77 140 case MAG_SCALE_4G:
fbob 2:dc7840a67f77 141 m_res = 400.0f/32768.0f;
fbob 0:431ee55036ca 142 break;
fbob 2:dc7840a67f77 143 case MAG_SCALE_8G:
fbob 2:dc7840a67f77 144 m_res = 800.0f/32768.0f;
fbob 0:431ee55036ca 145 break;
fbob 2:dc7840a67f77 146 case MAG_SCALE_12G:
fbob 2:dc7840a67f77 147 m_res = 1200.0f/32768.0f;
fbob 2:dc7840a67f77 148 break;
fbob 2:dc7840a67f77 149 case MAG_SCALE_16G:
fbob 2:dc7840a67f77 150 m_res = 1600.0f/32768.0f;
fbob 0:431ee55036ca 151 break;
fbob 0:431ee55036ca 152 }
fbob 0:431ee55036ca 153 }
fbob 0:431ee55036ca 154
fbob 2:dc7840a67f77 155 /** Read gyroscope data */
fbob 2:dc7840a67f77 156 void LSM9DS1::read_gyr()
fbob 0:431ee55036ca 157 {
fbob 0:431ee55036ca 158 // LSM9DS1 I2C bus address
fbob 2:dc7840a67f77 159 char address = LSM9DS1_ADDRESS_ACC_GYR;
fbob 0:431ee55036ca 160 // Register address
fbob 2:dc7840a67f77 161 char reg[1] = {OUT_X_L_G};
fbob 2:dc7840a67f77 162 // Data that we're going to read
fbob 2:dc7840a67f77 163 char data[6];
fbob 2:dc7840a67f77 164
fbob 2:dc7840a67f77 165 // Point to register address
fbob 2:dc7840a67f77 166 i2c.write(address, reg, 1);
fbob 2:dc7840a67f77 167 // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
fbob 2:dc7840a67f77 168 i2c.read(address, data, 6);
fbob 2:dc7840a67f77 169
fbob 2:dc7840a67f77 170 // Reassemble the data (two 8 bit data into one 16 bit data)
fbob 2:dc7840a67f77 171 int16_t gx_raw = data[0] | ( data[1] << 8 );
fbob 2:dc7840a67f77 172 int16_t gy_raw = data[2] | ( data[3] << 8 );
fbob 2:dc7840a67f77 173 int16_t gz_raw = data[4] | ( data[5] << 8 );
fbob 2:dc7840a67f77 174 // Convert to SI units [rad/s]
fbob 2:dc7840a67f77 175 gx = gx_raw * g_res;
fbob 2:dc7840a67f77 176 gy = -gy_raw * g_res;
fbob 2:dc7840a67f77 177 gz = gz_raw * g_res;
fbob 2:dc7840a67f77 178 }
fbob 2:dc7840a67f77 179
fbob 2:dc7840a67f77 180 /** Read accelerometer output data */
fbob 2:dc7840a67f77 181 void LSM9DS1::read_acc()
fbob 2:dc7840a67f77 182 {
fbob 2:dc7840a67f77 183 // LSM9DS1 I2C bus address
fbob 2:dc7840a67f77 184 char address = LSM9DS1_ADDRESS_ACC_GYR;
fbob 2:dc7840a67f77 185 // Register address
fbob 2:dc7840a67f77 186 char reg[1] = {OUT_X_L_XL};
fbob 0:431ee55036ca 187 // Data that we're going to read
fbob 0:431ee55036ca 188 char data[6];
fbob 0:431ee55036ca 189
fbob 0:431ee55036ca 190 // Point to register address
fbob 0:431ee55036ca 191 i2c.write(address, reg, 1);
fbob 0:431ee55036ca 192 // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
fbob 0:431ee55036ca 193 i2c.read(address, data, 6);
fbob 0:431ee55036ca 194
fbob 0:431ee55036ca 195 // Reassemble the data (two 8 bit data into one 16 bit data)
fbob 0:431ee55036ca 196 int16_t ax_raw = data[0] | ( data[1] << 8 );
fbob 0:431ee55036ca 197 int16_t ay_raw = data[2] | ( data[3] << 8 );
fbob 0:431ee55036ca 198 int16_t az_raw = data[4] | ( data[5] << 8 );
fbob 0:431ee55036ca 199 // Convert to SI units [m/s^2]
fbob 2:dc7840a67f77 200 ax = -ax_raw * a_res;
fbob 2:dc7840a67f77 201 ay = ay_raw * a_res;
fbob 0:431ee55036ca 202 az = -az_raw * a_res;
fbob 0:431ee55036ca 203 }
fbob 0:431ee55036ca 204
fbob 2:dc7840a67f77 205 /** Read magnetometer output data */
fbob 2:dc7840a67f77 206 void LSM9DS1::read_mag()
fbob 0:431ee55036ca 207 {
fbob 0:431ee55036ca 208 // LSM9DS1 I2C bus address
fbob 2:dc7840a67f77 209 char address = LSM9DS1_ADDRESS_MAG;
fbob 0:431ee55036ca 210 // Register address
fbob 2:dc7840a67f77 211 char reg[1] = {OUT_X_L_M};
fbob 0:431ee55036ca 212 // Data that we're going to read
fbob 0:431ee55036ca 213 char data[6];
fbob 0:431ee55036ca 214
fbob 0:431ee55036ca 215 // Point to register address
fbob 0:431ee55036ca 216 i2c.write(address, reg, 1);
fbob 0:431ee55036ca 217 // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
fbob 0:431ee55036ca 218 i2c.read(address, data, 6);
fbob 0:431ee55036ca 219
fbob 0:431ee55036ca 220 // Reassemble the data (two 8 bit data into one 16 bit data)
fbob 2:dc7840a67f77 221 int16_t mx_raw = data[0] | ( data[1] << 8 );
fbob 2:dc7840a67f77 222 int16_t my_raw = data[2] | ( data[3] << 8 );
fbob 2:dc7840a67f77 223 int16_t mz_raw = data[4] | ( data[5] << 8 );
fbob 2:dc7840a67f77 224 // Convert to SI units [m/s^2]
fbob 2:dc7840a67f77 225 mx = -mx_raw * m_res;
fbob 2:dc7840a67f77 226 my = -my_raw * m_res;
fbob 2:dc7840a67f77 227 mz = mz_raw * m_res;
fbob 0:431ee55036ca 228 }