Toyoshi Yoshimoto / LSM9DS1_2

jm6wud

LSM9DS1.cpp@0:67431d0a9143, 2021-01-03 (annotated)

Committer:
jm6wud
Date:
Sun Jan 03 15:48:10 2021 +0000
Revision:
0:67431d0a9143
jm6wud

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jm6wud 0:67431d0a9143 1 #include "LSM9DS1.h"
jm6wud 0:67431d0a9143 2
jm6wud 0:67431d0a9143 3 LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) : i2c(sda, scl)
jm6wud 0:67431d0a9143 4 {
jm6wud 0:67431d0a9143 5 // xgAddress and mAddress will store the 7-bit I2C address, if using I2C.
jm6wud 0:67431d0a9143 6 xgAddress = xgAddr;
jm6wud 0:67431d0a9143 7 mAddress = mAddr;
jm6wud 0:67431d0a9143 8 }
jm6wud 0:67431d0a9143 9
jm6wud 0:67431d0a9143 10 uint16_t LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
jm6wud 0:67431d0a9143 11 gyro_odr gODR, accel_odr aODR, mag_odr mODR)
jm6wud 0:67431d0a9143 12 {
jm6wud 0:67431d0a9143 13 // Store the given scales in class variables. These scale variables
jm6wud 0:67431d0a9143 14 // are used throughout to calculate the actual g's, DPS,and Gs's.
jm6wud 0:67431d0a9143 15 gScale = gScl;
jm6wud 0:67431d0a9143 16 aScale = aScl;
jm6wud 0:67431d0a9143 17 mScale = mScl;
jm6wud 0:67431d0a9143 18
jm6wud 0:67431d0a9143 19 // Once we have the scale values, we can calculate the resolution
jm6wud 0:67431d0a9143 20 // of each sensor. That's what these functions are for. One for each sensor
jm6wud 0:67431d0a9143 21 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
jm6wud 0:67431d0a9143 22 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
jm6wud 0:67431d0a9143 23 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
jm6wud 0:67431d0a9143 24
jm6wud 0:67431d0a9143 25
jm6wud 0:67431d0a9143 26 // To verify communication, we can read from the WHO_AM_I register of
jm6wud 0:67431d0a9143 27 // each device. Store those in a variable so we can return them.
jm6wud 0:67431d0a9143 28 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 29 char cmd[2] = {
jm6wud 0:67431d0a9143 30 WHO_AM_I_XG,
jm6wud 0:67431d0a9143 31 0
jm6wud 0:67431d0a9143 32 };
jm6wud 0:67431d0a9143 33
jm6wud 0:67431d0a9143 34 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 35 i2c.write(xgAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 36 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 37 i2c.read(xgAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 38 uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
jm6wud 0:67431d0a9143 39
jm6wud 0:67431d0a9143 40 // Reset to the address of the mag who am i
jm6wud 0:67431d0a9143 41 cmd[1] = WHO_AM_I_M;
jm6wud 0:67431d0a9143 42 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 43 i2c.write(mAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 44 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 45 i2c.read(mAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 46 uint8_t mTest = cmd[1]; // Read the mag WHO_AM_I
jm6wud 0:67431d0a9143 47
jm6wud 0:67431d0a9143 48 // Gyro initialization stuff:
jm6wud 0:67431d0a9143 49 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
jm6wud 0:67431d0a9143 50 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
jm6wud 0:67431d0a9143 51 setGyroScale(gScale); // Set the gyro range
jm6wud 0:67431d0a9143 52
jm6wud 0:67431d0a9143 53 // Accelerometer initialization stuff:
jm6wud 0:67431d0a9143 54 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
jm6wud 0:67431d0a9143 55 setAccelODR(aODR); // Set the accel data rate.
jm6wud 0:67431d0a9143 56 setAccelScale(aScale); // Set the accel range.
jm6wud 0:67431d0a9143 57
jm6wud 0:67431d0a9143 58 // Magnetometer initialization stuff:
jm6wud 0:67431d0a9143 59 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
jm6wud 0:67431d0a9143 60 setMagODR(mODR); // Set the magnetometer output data rate.
jm6wud 0:67431d0a9143 61 setMagScale(mScale); // Set the magnetometer's range.
jm6wud 0:67431d0a9143 62
jm6wud 0:67431d0a9143 63 // Once everything is initialized, return the WHO_AM_I registers we read:
jm6wud 0:67431d0a9143 64 return (xgTest << 8) | mTest;
jm6wud 0:67431d0a9143 65 }
jm6wud 0:67431d0a9143 66
jm6wud 0:67431d0a9143 67 void LSM9DS1::initGyro()
jm6wud 0:67431d0a9143 68 {
jm6wud 0:67431d0a9143 69 char cmd[4] = {
jm6wud 0:67431d0a9143 70 CTRL_REG1_G,
jm6wud 0:67431d0a9143 71 gScale | G_ODR_119_BW_14,
jm6wud 0:67431d0a9143 72 0, // Default data out and int out
jm6wud 0:67431d0a9143 73 0 // Default power mode and high pass settings
jm6wud 0:67431d0a9143 74 };
jm6wud 0:67431d0a9143 75
jm6wud 0:67431d0a9143 76 // Write the data to the gyro control registers
jm6wud 0:67431d0a9143 77 i2c.write(xgAddress, cmd, 4);
jm6wud 0:67431d0a9143 78 }
jm6wud 0:67431d0a9143 79
jm6wud 0:67431d0a9143 80 void LSM9DS1::initAccel()
jm6wud 0:67431d0a9143 81 {
jm6wud 0:67431d0a9143 82 char cmd[4] = {
jm6wud 0:67431d0a9143 83 CTRL_REG5_XL,
jm6wud 0:67431d0a9143 84 0x38, // Enable all axis and don't decimate data in out Registers
jm6wud 0:67431d0a9143 85 (A_ODR_119 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE), // 119 Hz ODR, set scale, and auto BW
jm6wud 0:67431d0a9143 86 0 // Default resolution mode and filtering settings
jm6wud 0:67431d0a9143 87 };
jm6wud 0:67431d0a9143 88
jm6wud 0:67431d0a9143 89 // Write the data to the accel control registers
jm6wud 0:67431d0a9143 90 i2c.write(xgAddress, cmd, 4);
jm6wud 0:67431d0a9143 91 }
jm6wud 0:67431d0a9143 92
jm6wud 0:67431d0a9143 93 void LSM9DS1::initMag()
jm6wud 0:67431d0a9143 94 {
jm6wud 0:67431d0a9143 95 char cmd[4] = {
jm6wud 0:67431d0a9143 96 CTRL_REG1_M,
jm6wud 0:67431d0a9143 97 0x10, // Default data rate, xy axes mode, and temp comp
jm6wud 0:67431d0a9143 98 mScale << 5, // Set mag scale
jm6wud 0:67431d0a9143 99 0 // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
jm6wud 0:67431d0a9143 100 };
jm6wud 0:67431d0a9143 101
jm6wud 0:67431d0a9143 102 // Write the data to the mag control registers
jm6wud 0:67431d0a9143 103 i2c.write(mAddress, cmd, 4);
jm6wud 0:67431d0a9143 104 }
jm6wud 0:67431d0a9143 105
jm6wud 0:67431d0a9143 106 void LSM9DS1::readAccel()
jm6wud 0:67431d0a9143 107 {
jm6wud 0:67431d0a9143 108 // The data we are going to read from the accel
jm6wud 0:67431d0a9143 109 char data[6];
jm6wud 0:67431d0a9143 110
jm6wud 0:67431d0a9143 111 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 112 char subAddress = OUT_X_L_XL;
jm6wud 0:67431d0a9143 113
jm6wud 0:67431d0a9143 114 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 115 i2c.write(xgAddress, &subAddress, 1, true);
jm6wud 0:67431d0a9143 116 // Read in all 8 bit registers containing the axes data
jm6wud 0:67431d0a9143 117 i2c.read(xgAddress, data, 6);
jm6wud 0:67431d0a9143 118
jm6wud 0:67431d0a9143 119 // Reassemble the data and convert to g
jm6wud 0:67431d0a9143 120 ax_raw = data[0] | (data[1] << 8);
jm6wud 0:67431d0a9143 121 ay_raw = data[2] | (data[3] << 8);
jm6wud 0:67431d0a9143 122 az_raw = data[4] | (data[5] << 8);
jm6wud 0:67431d0a9143 123 ax = ax_raw * aRes;
jm6wud 0:67431d0a9143 124 ay = ay_raw * aRes;
jm6wud 0:67431d0a9143 125 az = az_raw * aRes;
jm6wud 0:67431d0a9143 126 }
jm6wud 0:67431d0a9143 127
jm6wud 0:67431d0a9143 128 void LSM9DS1::readMag()
jm6wud 0:67431d0a9143 129 {
jm6wud 0:67431d0a9143 130 // The data we are going to read from the mag
jm6wud 0:67431d0a9143 131 char data[6];
jm6wud 0:67431d0a9143 132
jm6wud 0:67431d0a9143 133 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 134 char subAddress = OUT_X_L_M;
jm6wud 0:67431d0a9143 135
jm6wud 0:67431d0a9143 136 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 137 i2c.write(mAddress, &subAddress, 1, true);
jm6wud 0:67431d0a9143 138 // Read in all 8 bit registers containing the axes data
jm6wud 0:67431d0a9143 139 i2c.read(mAddress, data, 6);
jm6wud 0:67431d0a9143 140
jm6wud 0:67431d0a9143 141 // Reassemble the data and convert to degrees
jm6wud 0:67431d0a9143 142 mx_raw = data[0] | (data[1] << 8);
jm6wud 0:67431d0a9143 143 my_raw = data[2] | (data[3] << 8);
jm6wud 0:67431d0a9143 144 mz_raw = data[4] | (data[5] << 8);
jm6wud 0:67431d0a9143 145 mx = mx_raw * mRes;
jm6wud 0:67431d0a9143 146 my = my_raw * mRes;
jm6wud 0:67431d0a9143 147 mz = mz_raw * mRes;
jm6wud 0:67431d0a9143 148 }
jm6wud 0:67431d0a9143 149
jm6wud 0:67431d0a9143 150 void LSM9DS1::readTemp()
jm6wud 0:67431d0a9143 151 {
jm6wud 0:67431d0a9143 152 // The data we are going to read from the temp
jm6wud 0:67431d0a9143 153 char data[2];
jm6wud 0:67431d0a9143 154
jm6wud 0:67431d0a9143 155 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 156 char subAddress = OUT_TEMP_L;
jm6wud 0:67431d0a9143 157
jm6wud 0:67431d0a9143 158 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 159 i2c.write(xgAddress, &subAddress, 1, true);
jm6wud 0:67431d0a9143 160 // Read in all 8 bit registers containing the axes data
jm6wud 0:67431d0a9143 161 i2c.read(xgAddress, data, 2);
jm6wud 0:67431d0a9143 162
jm6wud 0:67431d0a9143 163 // Temperature is a 12-bit signed integer
jm6wud 0:67431d0a9143 164 temperature_raw = data[0] | (data[1] << 8);
jm6wud 0:67431d0a9143 165
jm6wud 0:67431d0a9143 166 temperature_c = (float)temperature_raw / 8.0 + 25;
jm6wud 0:67431d0a9143 167 temperature_f = temperature_c * 1.8 + 32;
jm6wud 0:67431d0a9143 168 }
jm6wud 0:67431d0a9143 169
jm6wud 0:67431d0a9143 170
jm6wud 0:67431d0a9143 171 void LSM9DS1::readGyro()
jm6wud 0:67431d0a9143 172 {
jm6wud 0:67431d0a9143 173 // The data we are going to read from the gyro
jm6wud 0:67431d0a9143 174 char data[6];
jm6wud 0:67431d0a9143 175
jm6wud 0:67431d0a9143 176 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 177 char subAddress = OUT_X_L_G;
jm6wud 0:67431d0a9143 178
jm6wud 0:67431d0a9143 179 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 180 i2c.write(xgAddress, &subAddress, 1, true);
jm6wud 0:67431d0a9143 181 // Read in all 8 bit registers containing the axes data
jm6wud 0:67431d0a9143 182 i2c.read(xgAddress, data, 6);
jm6wud 0:67431d0a9143 183
jm6wud 0:67431d0a9143 184 // Reassemble the data and convert to degrees/sec
jm6wud 0:67431d0a9143 185 gx_raw = data[0] | (data[1] << 8);
jm6wud 0:67431d0a9143 186 gy_raw = data[2] | (data[3] << 8);
jm6wud 0:67431d0a9143 187 gz_raw = data[4] | (data[5] << 8);
jm6wud 0:67431d0a9143 188 gx = gx_raw * gRes;
jm6wud 0:67431d0a9143 189 gy = gy_raw * gRes;
jm6wud 0:67431d0a9143 190 gz = gz_raw * gRes;
jm6wud 0:67431d0a9143 191 }
jm6wud 0:67431d0a9143 192
jm6wud 0:67431d0a9143 193 void LSM9DS1::setGyroScale(gyro_scale gScl)
jm6wud 0:67431d0a9143 194 {
jm6wud 0:67431d0a9143 195 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 196 char cmd[2] = {
jm6wud 0:67431d0a9143 197 CTRL_REG1_G,
jm6wud 0:67431d0a9143 198 0
jm6wud 0:67431d0a9143 199 };
jm6wud 0:67431d0a9143 200
jm6wud 0:67431d0a9143 201 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 202 i2c.write(xgAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 203 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 204 i2c.read(xgAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 205
jm6wud 0:67431d0a9143 206 // Then mask out the gyro scale bits:
jm6wud 0:67431d0a9143 207 cmd[1] &= 0xFF^(0x3 << 3);
jm6wud 0:67431d0a9143 208 // Then shift in our new scale bits:
jm6wud 0:67431d0a9143 209 cmd[1] |= gScl << 3;
jm6wud 0:67431d0a9143 210
jm6wud 0:67431d0a9143 211 // Write the gyroscale out to the gyro
jm6wud 0:67431d0a9143 212 i2c.write(xgAddress, cmd, 2);
jm6wud 0:67431d0a9143 213
jm6wud 0:67431d0a9143 214 // We've updated the sensor, but we also need to update our class variables
jm6wud 0:67431d0a9143 215 // First update gScale:
jm6wud 0:67431d0a9143 216 gScale = gScl;
jm6wud 0:67431d0a9143 217 // Then calculate a new gRes, which relies on gScale being set correctly:
jm6wud 0:67431d0a9143 218 calcgRes();
jm6wud 0:67431d0a9143 219 }
jm6wud 0:67431d0a9143 220
jm6wud 0:67431d0a9143 221 void LSM9DS1::setAccelScale(accel_scale aScl)
jm6wud 0:67431d0a9143 222 {
jm6wud 0:67431d0a9143 223 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 224 char cmd[2] = {
jm6wud 0:67431d0a9143 225 CTRL_REG6_XL,
jm6wud 0:67431d0a9143 226 0
jm6wud 0:67431d0a9143 227 };
jm6wud 0:67431d0a9143 228
jm6wud 0:67431d0a9143 229 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 230 i2c.write(xgAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 231 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 232 i2c.read(xgAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 233
jm6wud 0:67431d0a9143 234 // Then mask out the accel scale bits:
jm6wud 0:67431d0a9143 235 cmd[1] &= 0xFF^(0x3 << 3);
jm6wud 0:67431d0a9143 236 // Then shift in our new scale bits:
jm6wud 0:67431d0a9143 237 cmd[1] |= aScl << 3;
jm6wud 0:67431d0a9143 238
jm6wud 0:67431d0a9143 239 // Write the accelscale out to the accel
jm6wud 0:67431d0a9143 240 i2c.write(xgAddress, cmd, 2);
jm6wud 0:67431d0a9143 241
jm6wud 0:67431d0a9143 242 // We've updated the sensor, but we also need to update our class variables
jm6wud 0:67431d0a9143 243 // First update aScale:
jm6wud 0:67431d0a9143 244 aScale = aScl;
jm6wud 0:67431d0a9143 245 // Then calculate a new aRes, which relies on aScale being set correctly:
jm6wud 0:67431d0a9143 246 calcaRes();
jm6wud 0:67431d0a9143 247 }
jm6wud 0:67431d0a9143 248
jm6wud 0:67431d0a9143 249 void LSM9DS1::setMagScale(mag_scale mScl)
jm6wud 0:67431d0a9143 250 {
jm6wud 0:67431d0a9143 251 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 252 char cmd[2] = {
jm6wud 0:67431d0a9143 253 CTRL_REG2_M,
jm6wud 0:67431d0a9143 254 0
jm6wud 0:67431d0a9143 255 };
jm6wud 0:67431d0a9143 256
jm6wud 0:67431d0a9143 257 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 258 i2c.write(mAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 259 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 260 i2c.read(mAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 261
jm6wud 0:67431d0a9143 262 // Then mask out the mag scale bits:
jm6wud 0:67431d0a9143 263 cmd[1] &= 0xFF^(0x3 << 5);
jm6wud 0:67431d0a9143 264 // Then shift in our new scale bits:
jm6wud 0:67431d0a9143 265 cmd[1] |= mScl << 5;
jm6wud 0:67431d0a9143 266
jm6wud 0:67431d0a9143 267 // Write the magscale out to the mag
jm6wud 0:67431d0a9143 268 i2c.write(mAddress, cmd, 2);
jm6wud 0:67431d0a9143 269
jm6wud 0:67431d0a9143 270 // We've updated the sensor, but we also need to update our class variables
jm6wud 0:67431d0a9143 271 // First update mScale:
jm6wud 0:67431d0a9143 272 mScale = mScl;
jm6wud 0:67431d0a9143 273 // Then calculate a new mRes, which relies on mScale being set correctly:
jm6wud 0:67431d0a9143 274 calcmRes();
jm6wud 0:67431d0a9143 275 }
jm6wud 0:67431d0a9143 276
jm6wud 0:67431d0a9143 277 void LSM9DS1::setGyroODR(gyro_odr gRate)
jm6wud 0:67431d0a9143 278 {
jm6wud 0:67431d0a9143 279 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 280 char cmd[2] = {
jm6wud 0:67431d0a9143 281 CTRL_REG1_G,
jm6wud 0:67431d0a9143 282 0
jm6wud 0:67431d0a9143 283 };
jm6wud 0:67431d0a9143 284
jm6wud 0:67431d0a9143 285 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 286 i2c.write(xgAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 287 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 288 i2c.read(xgAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 289
jm6wud 0:67431d0a9143 290 // Then mask out the gyro odr bits:
jm6wud 0:67431d0a9143 291 cmd[1] &= (0x3 << 3);
jm6wud 0:67431d0a9143 292 // Then shift in our new odr bits:
jm6wud 0:67431d0a9143 293 cmd[1] |= gRate;
jm6wud 0:67431d0a9143 294
jm6wud 0:67431d0a9143 295 // Write the gyroodr out to the gyro
jm6wud 0:67431d0a9143 296 i2c.write(xgAddress, cmd, 2);
jm6wud 0:67431d0a9143 297 }
jm6wud 0:67431d0a9143 298
jm6wud 0:67431d0a9143 299 void LSM9DS1::setAccelODR(accel_odr aRate)
jm6wud 0:67431d0a9143 300 {
jm6wud 0:67431d0a9143 301 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 302 char cmd[2] = {
jm6wud 0:67431d0a9143 303 CTRL_REG6_XL,
jm6wud 0:67431d0a9143 304 0
jm6wud 0:67431d0a9143 305 };
jm6wud 0:67431d0a9143 306
jm6wud 0:67431d0a9143 307 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 308 i2c.write(xgAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 309 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 310 i2c.read(xgAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 311
jm6wud 0:67431d0a9143 312 // Then mask out the accel odr bits:
jm6wud 0:67431d0a9143 313 cmd[1] &= 0xFF^(0x7 << 5);
jm6wud 0:67431d0a9143 314 // Then shift in our new odr bits:
jm6wud 0:67431d0a9143 315 cmd[1] |= aRate << 5;
jm6wud 0:67431d0a9143 316
jm6wud 0:67431d0a9143 317 // Write the accelodr out to the accel
jm6wud 0:67431d0a9143 318 i2c.write(xgAddress, cmd, 2);
jm6wud 0:67431d0a9143 319 }
jm6wud 0:67431d0a9143 320
jm6wud 0:67431d0a9143 321 void LSM9DS1::setMagODR(mag_odr mRate)
jm6wud 0:67431d0a9143 322 {
jm6wud 0:67431d0a9143 323 // The start of the addresses we want to read from
jm6wud 0:67431d0a9143 324 char cmd[2] = {
jm6wud 0:67431d0a9143 325 CTRL_REG1_M,
jm6wud 0:67431d0a9143 326 0
jm6wud 0:67431d0a9143 327 };
jm6wud 0:67431d0a9143 328
jm6wud 0:67431d0a9143 329 // Write the address we are going to read from and don't end the transaction
jm6wud 0:67431d0a9143 330 i2c.write(mAddress, cmd, 1, true);
jm6wud 0:67431d0a9143 331 // Read in all the 8 bits of data
jm6wud 0:67431d0a9143 332 i2c.read(mAddress, cmd+1, 1);
jm6wud 0:67431d0a9143 333
jm6wud 0:67431d0a9143 334 // Then mask out the mag odr bits:
jm6wud 0:67431d0a9143 335 cmd[1] &= 0xFF^(0x7 << 2);
jm6wud 0:67431d0a9143 336 // Then shift in our new odr bits:
jm6wud 0:67431d0a9143 337 cmd[1] |= mRate << 2;
jm6wud 0:67431d0a9143 338
jm6wud 0:67431d0a9143 339 // Write the magodr out to the mag
jm6wud 0:67431d0a9143 340 i2c.write(mAddress, cmd, 2);
jm6wud 0:67431d0a9143 341 }
jm6wud 0:67431d0a9143 342
jm6wud 0:67431d0a9143 343 void LSM9DS1::calcgRes()
jm6wud 0:67431d0a9143 344 {
jm6wud 0:67431d0a9143 345 // Possible gyro scales (and their register bit settings) are:
jm6wud 0:67431d0a9143 346 // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
jm6wud 0:67431d0a9143 347 switch (gScale)
jm6wud 0:67431d0a9143 348 {
jm6wud 0:67431d0a9143 349 case G_SCALE_245DPS:
jm6wud 0:67431d0a9143 350 gRes = 245.0 / 32768.0;
jm6wud 0:67431d0a9143 351 break;
jm6wud 0:67431d0a9143 352 case G_SCALE_500DPS:
jm6wud 0:67431d0a9143 353 gRes = 500.0 / 32768.0;
jm6wud 0:67431d0a9143 354 break;
jm6wud 0:67431d0a9143 355 case G_SCALE_2000DPS:
jm6wud 0:67431d0a9143 356 gRes = 2000.0 / 32768.0;
jm6wud 0:67431d0a9143 357 break;
jm6wud 0:67431d0a9143 358 }
jm6wud 0:67431d0a9143 359 }
jm6wud 0:67431d0a9143 360
jm6wud 0:67431d0a9143 361 void LSM9DS1::calcaRes()
jm6wud 0:67431d0a9143 362 {
jm6wud 0:67431d0a9143 363 // Possible accelerometer scales (and their register bit settings) are:
jm6wud 0:67431d0a9143 364 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
jm6wud 0:67431d0a9143 365 switch (aScale)
jm6wud 0:67431d0a9143 366 {
jm6wud 0:67431d0a9143 367 case A_SCALE_2G:
jm6wud 0:67431d0a9143 368 aRes = 2.0 / 32768.0;
jm6wud 0:67431d0a9143 369 break;
jm6wud 0:67431d0a9143 370 case A_SCALE_4G:
jm6wud 0:67431d0a9143 371 aRes = 4.0 / 32768.0;
jm6wud 0:67431d0a9143 372 break;
jm6wud 0:67431d0a9143 373 case A_SCALE_8G:
jm6wud 0:67431d0a9143 374 aRes = 8.0 / 32768.0;
jm6wud 0:67431d0a9143 375 break;
jm6wud 0:67431d0a9143 376 case A_SCALE_16G:
jm6wud 0:67431d0a9143 377 aRes = 16.0 / 32768.0;
jm6wud 0:67431d0a9143 378 break;
jm6wud 0:67431d0a9143 379 }
jm6wud 0:67431d0a9143 380 }
jm6wud 0:67431d0a9143 381
jm6wud 0:67431d0a9143 382 void LSM9DS1::calcmRes()
jm6wud 0:67431d0a9143 383 {
jm6wud 0:67431d0a9143 384 // Possible magnetometer scales (and their register bit settings) are:
jm6wud 0:67431d0a9143 385 // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
jm6wud 0:67431d0a9143 386 switch (mScale)
jm6wud 0:67431d0a9143 387 {
jm6wud 0:67431d0a9143 388 case M_SCALE_4GS:
jm6wud 0:67431d0a9143 389 mRes = 4.0 / 32768.0;
jm6wud 0:67431d0a9143 390 break;
jm6wud 0:67431d0a9143 391 case M_SCALE_8GS:
jm6wud 0:67431d0a9143 392 mRes = 8.0 / 32768.0;
jm6wud 0:67431d0a9143 393 break;
jm6wud 0:67431d0a9143 394 case M_SCALE_12GS:
jm6wud 0:67431d0a9143 395 mRes = 12.0 / 32768.0;
jm6wud 0:67431d0a9143 396 break;
jm6wud 0:67431d0a9143 397 case M_SCALE_16GS:
jm6wud 0:67431d0a9143 398 mRes = 16.0 / 32768.0;
jm6wud 0:67431d0a9143 399 break;
jm6wud 0:67431d0a9143 400 }
jm6wud 0:67431d0a9143 401 }
jm6wud 0:67431d0a9143 402
jm6wud 0:67431d0a9143 403 bool LSM9DS1::whoAmI(){
jm6wud 0:67431d0a9143 404 uint8_t resultXG,resultM ,whoAmIAddressXG=WHO_AM_I_XG,whoAmIAddressM=WHO_AM_I_M;
jm6wud 0:67431d0a9143 405
jm6wud 0:67431d0a9143 406 //acc gyro
jm6wud 0:67431d0a9143 407 i2c.start();
jm6wud 0:67431d0a9143 408 i2c.write(xgAddress);
jm6wud 0:67431d0a9143 409 i2c.write(whoAmIAddressXG);
jm6wud 0:67431d0a9143 410 i2c.write(xgAddress | 1);
jm6wud 0:67431d0a9143 411 resultXG = i2c.read(whoAmIAddressXG);
jm6wud 0:67431d0a9143 412 i2c.stop();
jm6wud 0:67431d0a9143 413
jm6wud 0:67431d0a9143 414 //magn
jm6wud 0:67431d0a9143 415 i2c.start();
jm6wud 0:67431d0a9143 416 i2c.write(xgAddress);
jm6wud 0:67431d0a9143 417 i2c.write(whoAmIAddressM);
jm6wud 0:67431d0a9143 418 i2c.write(xgAddress | 1);
jm6wud 0:67431d0a9143 419 resultM = i2c.read(whoAmIAddressM);
jm6wud 0:67431d0a9143 420 i2c.stop();
jm6wud 0:67431d0a9143 421
jm6wud 0:67431d0a9143 422 uint16_t combinedResult = (resultXG << 8) | resultM;
jm6wud 0:67431d0a9143 423 if(combinedResult!=((whoAmIAddressXG << 8) | whoAmIAddressM))
jm6wud 0:67431d0a9143 424 return true;
jm6wud 0:67431d0a9143 425 else
jm6wud 0:67431d0a9143 426 return false;
jm6wud 0:67431d0a9143 427 }