Raynaud Gilles / LSM9DS1

VGR

Dependents:   VITI_motor_angle_1 VITI_motor_angle_2 VITI_motor_angle_3

LSM9DS1.cpp@4:7ffcb378cfd4, 2016-06-22 (annotated)

Committer:
5hel2l2y
Date:
Wed Jun 22 22:11:37 2016 +0000
Revision:
4:7ffcb378cfd4
Parent:
3:f96b287c0bf7
Child:
6:28c4b3c8b43d
Modified interrupt initialization.

Who changed what in which revision?

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