Taylor Andrews / Mbed 2 deprecated LSM9DS0

Allows for reading accelerometer, gyroscope, and magnetometer data from an LSM9DS0 IMU device

Dependencies:   mbed

Dependents:   uVGA_4180 uLCD_4180_mini ECE4781_Project

LSM9DS0.cpp@4:bf8f4e7c9905, 2014-12-03 (annotated)

Committer:
randrews33
Date:
Wed Dec 03 23:08:09 2014 +0000
Revision:
4:bf8f4e7c9905
Parent:
0:1b975a6ae539 
Issues with the previous versions caused very strange values to be read back. This was due to the wrong addresses being read. Fixed here

Who changed what in which revision?

UserRevisionLine numberNew contents of line
randrews33 0:1b975a6ae539 1 #include "LSM9DS0.h"
randrews33 0:1b975a6ae539 2
randrews33 0:1b975a6ae539 3 LSM9DS0::LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr)
randrews33 0:1b975a6ae539 4 {
randrews33 0:1b975a6ae539 5 // xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
randrews33 0:1b975a6ae539 6 xmAddress = xmAddr;
randrews33 0:1b975a6ae539 7 gAddress = gAddr;
randrews33 0:1b975a6ae539 8
randrews33 0:1b975a6ae539 9 i2c_ = new I2Cdev(sda, scl);
randrews33 0:1b975a6ae539 10 }
randrews33 0:1b975a6ae539 11
randrews33 0:1b975a6ae539 12 uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
randrews33 0:1b975a6ae539 13 gyro_odr gODR, accel_odr aODR, mag_odr mODR)
randrews33 0:1b975a6ae539 14 {
randrews33 0:1b975a6ae539 15 // Store the given scales in class variables. These scale variables
randrews33 0:1b975a6ae539 16 // are used throughout to calculate the actual g's, DPS,and Gs's.
randrews33 0:1b975a6ae539 17 gScale = gScl;
randrews33 0:1b975a6ae539 18 aScale = aScl;
randrews33 0:1b975a6ae539 19 mScale = mScl;
randrews33 0:1b975a6ae539 20
randrews33 0:1b975a6ae539 21 // Once we have the scale values, we can calculate the resolution
randrews33 0:1b975a6ae539 22 // of each sensor. That's what these functions are for. One for each sensor
randrews33 0:1b975a6ae539 23 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
randrews33 0:1b975a6ae539 24 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
randrews33 0:1b975a6ae539 25 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
randrews33 0:1b975a6ae539 26
randrews33 0:1b975a6ae539 27
randrews33 0:1b975a6ae539 28 // To verify communication, we can read from the WHO_AM_I register of
randrews33 0:1b975a6ae539 29 // each device. Store those in a variable so we can return them.
randrews33 0:1b975a6ae539 30 uint8_t gTest = gReadByte(WHO_AM_I_G); // Read the gyro WHO_AM_I
randrews33 0:1b975a6ae539 31 uint8_t xmTest = xmReadByte(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
randrews33 0:1b975a6ae539 32
randrews33 0:1b975a6ae539 33 // Gyro initialization stuff:
randrews33 0:1b975a6ae539 34 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
randrews33 0:1b975a6ae539 35 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
randrews33 0:1b975a6ae539 36 setGyroScale(gScale); // Set the gyro range
randrews33 0:1b975a6ae539 37
randrews33 0:1b975a6ae539 38 // Accelerometer initialization stuff:
randrews33 0:1b975a6ae539 39 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
randrews33 4:bf8f4e7c9905 40 setAccelODR(aODR); // Set the accel data rate.
randrews33 4:bf8f4e7c9905 41 setAccelScale(aScale); // Set the accel range.
randrews33 0:1b975a6ae539 42
randrews33 0:1b975a6ae539 43 // Magnetometer initialization stuff:
randrews33 0:1b975a6ae539 44 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
randrews33 0:1b975a6ae539 45 setMagODR(mODR); // Set the magnetometer output data rate.
randrews33 0:1b975a6ae539 46 setMagScale(mScale); // Set the magnetometer's range.
randrews33 0:1b975a6ae539 47
randrews33 0:1b975a6ae539 48 // Once everything is initialized, return the WHO_AM_I registers we read:
randrews33 0:1b975a6ae539 49 return (xmTest << 8) | gTest;
randrews33 0:1b975a6ae539 50 }
randrews33 0:1b975a6ae539 51
randrews33 0:1b975a6ae539 52 void LSM9DS0::initGyro()
randrews33 0:1b975a6ae539 53 {
randrews33 4:bf8f4e7c9905 54
randrews33 0:1b975a6ae539 55 gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
randrews33 0:1b975a6ae539 56 gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
randrews33 4:bf8f4e7c9905 57 gWriteByte(CTRL_REG3_G, 0x88); //Interrupt enabled on both INT_G and I2_DRDY
randrews33 4:bf8f4e7c9905 58 gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
randrews33 4:bf8f4e7c9905 59 gWriteByte(CTRL_REG5_G, 0x00); //Init default values
randrews33 0:1b975a6ae539 60
randrews33 0:1b975a6ae539 61 }
randrews33 0:1b975a6ae539 62
randrews33 0:1b975a6ae539 63 void LSM9DS0::initAccel()
randrews33 0:1b975a6ae539 64 {
randrews33 4:bf8f4e7c9905 65 xmWriteByte(CTRL_REG0_XM, 0x00);
randrews33 4:bf8f4e7c9905 66 xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz data rate, x/y/z all enabled
randrews33 0:1b975a6ae539 67 xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
randrews33 4:bf8f4e7c9905 68 xmWriteByte(CTRL_REG3_XM, 0x04); // Accelerometer data ready on INT1_XM (0x04)
randrews33 4:bf8f4e7c9905 69
randrews33 0:1b975a6ae539 70 }
randrews33 0:1b975a6ae539 71
randrews33 0:1b975a6ae539 72 void LSM9DS0::initMag()
randrews33 0:1b975a6ae539 73 {
randrews33 4:bf8f4e7c9905 74 xmWriteByte(CTRL_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor
randrews33 0:1b975a6ae539 75 xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
randrews33 0:1b975a6ae539 76 xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
randrews33 0:1b975a6ae539 77 xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08)
randrews33 0:1b975a6ae539 78 xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
randrews33 0:1b975a6ae539 79 }
randrews33 0:1b975a6ae539 80
randrews33 4:bf8f4e7c9905 81 void LSM9DS0::calLSM9DS0(float * gbias, float * abias)
randrews33 4:bf8f4e7c9905 82 {
randrews33 4:bf8f4e7c9905 83 uint8_t data[6] = {0, 0, 0, 0, 0, 0};
randrews33 4:bf8f4e7c9905 84 int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
randrews33 4:bf8f4e7c9905 85 int samples, ii;
randrews33 4:bf8f4e7c9905 86
randrews33 4:bf8f4e7c9905 87 // First get gyro bias
randrews33 4:bf8f4e7c9905 88 uint8_t c = gReadByte(CTRL_REG5_G);
randrews33 4:bf8f4e7c9905 89 gWriteByte(CTRL_REG5_G, c | 0x40); // Enable gyro FIFO
randrews33 4:bf8f4e7c9905 90 wait_ms(20); // Wait for change to take effect
randrews33 4:bf8f4e7c9905 91 gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples
randrews33 4:bf8f4e7c9905 92 wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples
randrews33 4:bf8f4e7c9905 93
randrews33 4:bf8f4e7c9905 94 samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
randrews33 4:bf8f4e7c9905 95
randrews33 4:bf8f4e7c9905 96 for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO
randrews33 4:bf8f4e7c9905 97
randrews33 4:bf8f4e7c9905 98 data[0] = gReadByte(OUT_X_L_G);
randrews33 4:bf8f4e7c9905 99 data[1] = gReadByte(OUT_X_H_G);
randrews33 4:bf8f4e7c9905 100 data[2] = gReadByte(OUT_Y_L_G);
randrews33 4:bf8f4e7c9905 101 data[3] = gReadByte(OUT_Y_H_G);
randrews33 4:bf8f4e7c9905 102 data[4] = gReadByte(OUT_Z_L_G);
randrews33 4:bf8f4e7c9905 103 data[5] = gReadByte(OUT_Z_H_G);
randrews33 4:bf8f4e7c9905 104
randrews33 4:bf8f4e7c9905 105 gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
randrews33 4:bf8f4e7c9905 106 gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
randrews33 4:bf8f4e7c9905 107 gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
randrews33 4:bf8f4e7c9905 108 }
randrews33 4:bf8f4e7c9905 109
randrews33 4:bf8f4e7c9905 110 gyro_bias[0] /= samples; // average the data
randrews33 4:bf8f4e7c9905 111 gyro_bias[1] /= samples;
randrews33 4:bf8f4e7c9905 112 gyro_bias[2] /= samples;
randrews33 4:bf8f4e7c9905 113
randrews33 4:bf8f4e7c9905 114 gbias[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s
randrews33 4:bf8f4e7c9905 115 gbias[1] = (float)gyro_bias[1]*gRes;
randrews33 4:bf8f4e7c9905 116 gbias[2] = (float)gyro_bias[2]*gRes;
randrews33 4:bf8f4e7c9905 117
randrews33 4:bf8f4e7c9905 118 c = gReadByte(CTRL_REG5_G);
randrews33 4:bf8f4e7c9905 119 gWriteByte(CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO
randrews33 4:bf8f4e7c9905 120 wait_ms(20);
randrews33 4:bf8f4e7c9905 121 gWriteByte(FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode
randrews33 4:bf8f4e7c9905 122
randrews33 4:bf8f4e7c9905 123 // Now get the accelerometer biases
randrews33 4:bf8f4e7c9905 124 c = xmReadByte(CTRL_REG0_XM);
randrews33 4:bf8f4e7c9905 125 xmWriteByte(CTRL_REG0_XM, c | 0x40); // Enable accelerometer FIFO
randrews33 4:bf8f4e7c9905 126 wait_ms(20); // Wait for change to take effect
randrews33 4:bf8f4e7c9905 127 xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples
randrews33 4:bf8f4e7c9905 128 wait_ms(1000); // delay 1000 milliseconds to collect FIFO samples
randrews33 4:bf8f4e7c9905 129
randrews33 4:bf8f4e7c9905 130 samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
randrews33 4:bf8f4e7c9905 131
randrews33 4:bf8f4e7c9905 132 for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO
randrews33 4:bf8f4e7c9905 133
randrews33 4:bf8f4e7c9905 134 data[0] = xmReadByte(OUT_X_L_A);
randrews33 4:bf8f4e7c9905 135 data[1] = xmReadByte(OUT_X_H_A);
randrews33 4:bf8f4e7c9905 136 data[2] = xmReadByte(OUT_Y_L_A);
randrews33 4:bf8f4e7c9905 137 data[3] = xmReadByte(OUT_Y_H_A);
randrews33 4:bf8f4e7c9905 138 data[4] = xmReadByte(OUT_Z_L_A);
randrews33 4:bf8f4e7c9905 139 data[5] = xmReadByte(OUT_Z_H_A);
randrews33 4:bf8f4e7c9905 140 accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
randrews33 4:bf8f4e7c9905 141 accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
randrews33 4:bf8f4e7c9905 142 accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1./aRes); // Assumes sensor facing up!
randrews33 4:bf8f4e7c9905 143 }
randrews33 4:bf8f4e7c9905 144
randrews33 4:bf8f4e7c9905 145 accel_bias[0] /= samples; // average the data
randrews33 4:bf8f4e7c9905 146 accel_bias[1] /= samples;
randrews33 4:bf8f4e7c9905 147 accel_bias[2] /= samples;
randrews33 4:bf8f4e7c9905 148
randrews33 4:bf8f4e7c9905 149 abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
randrews33 4:bf8f4e7c9905 150 abias[1] = (float)accel_bias[1]*aRes;
randrews33 4:bf8f4e7c9905 151 abias[2] = (float)accel_bias[2]*aRes;
randrews33 4:bf8f4e7c9905 152
randrews33 4:bf8f4e7c9905 153 c = xmReadByte(CTRL_REG0_XM);
randrews33 4:bf8f4e7c9905 154 xmWriteByte(CTRL_REG0_XM, c & ~0x40); // Disable accelerometer FIFO
randrews33 4:bf8f4e7c9905 155 wait_ms(20);
randrews33 4:bf8f4e7c9905 156 xmWriteByte(FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode
randrews33 4:bf8f4e7c9905 157
randrews33 4:bf8f4e7c9905 158 }
randrews33 0:1b975a6ae539 159 void LSM9DS0::readAccel()
randrews33 0:1b975a6ae539 160 {
randrews33 0:1b975a6ae539 161 uint16_t Temp = 0;
randrews33 0:1b975a6ae539 162
randrews33 0:1b975a6ae539 163 //Get x
randrews33 0:1b975a6ae539 164 Temp = xmReadByte(OUT_X_H_A);
randrews33 0:1b975a6ae539 165 Temp = Temp<<8;
randrews33 0:1b975a6ae539 166 Temp |= xmReadByte(OUT_X_L_A);
randrews33 0:1b975a6ae539 167 ax = Temp;
randrews33 0:1b975a6ae539 168
randrews33 0:1b975a6ae539 169
randrews33 0:1b975a6ae539 170 //Get y
randrews33 0:1b975a6ae539 171 Temp=0;
randrews33 0:1b975a6ae539 172 Temp = xmReadByte(OUT_Y_H_A);
randrews33 0:1b975a6ae539 173 Temp = Temp<<8;
randrews33 0:1b975a6ae539 174 Temp |= xmReadByte(OUT_Y_L_A);
randrews33 0:1b975a6ae539 175 ay = Temp;
randrews33 0:1b975a6ae539 176
randrews33 0:1b975a6ae539 177 //Get z
randrews33 0:1b975a6ae539 178 Temp=0;
randrews33 0:1b975a6ae539 179 Temp = xmReadByte(OUT_Z_H_A);
randrews33 0:1b975a6ae539 180 Temp = Temp<<8;
randrews33 0:1b975a6ae539 181 Temp |= xmReadByte(OUT_Z_L_A);
randrews33 0:1b975a6ae539 182 az = Temp;
randrews33 0:1b975a6ae539 183
randrews33 0:1b975a6ae539 184 }
randrews33 0:1b975a6ae539 185
randrews33 0:1b975a6ae539 186 void LSM9DS0::readMag()
randrews33 0:1b975a6ae539 187 {
randrews33 4:bf8f4e7c9905 188 uint16_t Temp = 0;
randrews33 0:1b975a6ae539 189
randrews33 0:1b975a6ae539 190 //Get x
randrews33 0:1b975a6ae539 191 Temp = xmReadByte(OUT_X_H_M);
randrews33 0:1b975a6ae539 192 Temp = Temp<<8;
randrews33 0:1b975a6ae539 193 Temp |= xmReadByte(OUT_X_L_M);
randrews33 0:1b975a6ae539 194 mx = Temp;
randrews33 0:1b975a6ae539 195
randrews33 0:1b975a6ae539 196
randrews33 0:1b975a6ae539 197 //Get y
randrews33 0:1b975a6ae539 198 Temp=0;
randrews33 0:1b975a6ae539 199 Temp = xmReadByte(OUT_Y_H_M);
randrews33 0:1b975a6ae539 200 Temp = Temp<<8;
randrews33 0:1b975a6ae539 201 Temp |= xmReadByte(OUT_Y_L_M);
randrews33 0:1b975a6ae539 202 my = Temp;
randrews33 0:1b975a6ae539 203
randrews33 0:1b975a6ae539 204 //Get z
randrews33 0:1b975a6ae539 205 Temp=0;
randrews33 0:1b975a6ae539 206 Temp = xmReadByte(OUT_Z_H_M);
randrews33 0:1b975a6ae539 207 Temp = Temp<<8;
randrews33 0:1b975a6ae539 208 Temp |= xmReadByte(OUT_Z_L_M);
randrews33 0:1b975a6ae539 209 mz = Temp;
randrews33 0:1b975a6ae539 210 }
randrews33 0:1b975a6ae539 211
randrews33 4:bf8f4e7c9905 212 void LSM9DS0::readTemp()
randrews33 0:1b975a6ae539 213 {
randrews33 4:bf8f4e7c9905 214 uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp
randrews33 4:bf8f4e7c9905 215
randrews33 4:bf8f4e7c9905 216 temp[0] = xmReadByte(OUT_TEMP_L_XM);
randrews33 4:bf8f4e7c9905 217 temp[1] = xmReadByte(OUT_TEMP_H_XM);
randrews33 0:1b975a6ae539 218
randrews33 4:bf8f4e7c9905 219 temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
randrews33 4:bf8f4e7c9905 220 }
randrews33 4:bf8f4e7c9905 221
randrews33 4:bf8f4e7c9905 222
randrews33 4:bf8f4e7c9905 223 void LSM9DS0::readGyro()
randrews33 4:bf8f4e7c9905 224 {
randrews33 0:1b975a6ae539 225 uint16_t Temp = 0;
randrews33 0:1b975a6ae539 226
randrews33 0:1b975a6ae539 227 //Get x
randrews33 4:bf8f4e7c9905 228 Temp = gReadByte(OUT_X_H_G);
randrews33 0:1b975a6ae539 229 Temp = Temp<<8;
randrews33 4:bf8f4e7c9905 230 Temp |= gReadByte(OUT_X_L_G);
randrews33 0:1b975a6ae539 231 gx = Temp;
randrews33 0:1b975a6ae539 232
randrews33 0:1b975a6ae539 233
randrews33 0:1b975a6ae539 234 //Get y
randrews33 0:1b975a6ae539 235 Temp=0;
randrews33 4:bf8f4e7c9905 236 Temp = gReadByte(OUT_Y_H_G);
randrews33 0:1b975a6ae539 237 Temp = Temp<<8;
randrews33 4:bf8f4e7c9905 238 Temp |= gReadByte(OUT_Y_L_G);
randrews33 0:1b975a6ae539 239 gy = Temp;
randrews33 0:1b975a6ae539 240
randrews33 0:1b975a6ae539 241 //Get z
randrews33 0:1b975a6ae539 242 Temp=0;
randrews33 4:bf8f4e7c9905 243 Temp = gReadByte(OUT_Z_H_G);
randrews33 0:1b975a6ae539 244 Temp = Temp<<8;
randrews33 4:bf8f4e7c9905 245 Temp |= gReadByte(OUT_Z_L_G);
randrews33 0:1b975a6ae539 246 gz = Temp;
randrews33 0:1b975a6ae539 247 }
randrews33 0:1b975a6ae539 248
randrews33 0:1b975a6ae539 249 float LSM9DS0::calcGyro(int16_t gyro)
randrews33 0:1b975a6ae539 250 {
randrews33 0:1b975a6ae539 251 // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
randrews33 0:1b975a6ae539 252 return gRes * gyro;
randrews33 0:1b975a6ae539 253 }
randrews33 0:1b975a6ae539 254
randrews33 0:1b975a6ae539 255 float LSM9DS0::calcAccel(int16_t accel)
randrews33 0:1b975a6ae539 256 {
randrews33 0:1b975a6ae539 257 // Return the accel raw reading times our pre-calculated g's / (ADC tick):
randrews33 0:1b975a6ae539 258 return aRes * accel;
randrews33 0:1b975a6ae539 259 }
randrews33 0:1b975a6ae539 260
randrews33 0:1b975a6ae539 261 float LSM9DS0::calcMag(int16_t mag)
randrews33 0:1b975a6ae539 262 {
randrews33 0:1b975a6ae539 263 // Return the mag raw reading times our pre-calculated Gs / (ADC tick):
randrews33 0:1b975a6ae539 264 return mRes * mag;
randrews33 0:1b975a6ae539 265 }
randrews33 0:1b975a6ae539 266
randrews33 0:1b975a6ae539 267 void LSM9DS0::setGyroScale(gyro_scale gScl)
randrews33 0:1b975a6ae539 268 {
randrews33 0:1b975a6ae539 269 // We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
randrews33 0:1b975a6ae539 270 uint8_t temp = gReadByte(CTRL_REG4_G);
randrews33 0:1b975a6ae539 271 // Then mask out the gyro scale bits:
randrews33 0:1b975a6ae539 272 temp &= 0xFF^(0x3 << 4);
randrews33 0:1b975a6ae539 273 // Then shift in our new scale bits:
randrews33 0:1b975a6ae539 274 temp |= gScl << 4;
randrews33 0:1b975a6ae539 275 // And write the new register value back into CTRL_REG4_G:
randrews33 0:1b975a6ae539 276 gWriteByte(CTRL_REG4_G, temp);
randrews33 0:1b975a6ae539 277
randrews33 0:1b975a6ae539 278 // We've updated the sensor, but we also need to update our class variables
randrews33 0:1b975a6ae539 279 // First update gScale:
randrews33 0:1b975a6ae539 280 gScale = gScl;
randrews33 0:1b975a6ae539 281 // Then calculate a new gRes, which relies on gScale being set correctly:
randrews33 0:1b975a6ae539 282 calcgRes();
randrews33 0:1b975a6ae539 283 }
randrews33 0:1b975a6ae539 284
randrews33 0:1b975a6ae539 285 void LSM9DS0::setAccelScale(accel_scale aScl)
randrews33 0:1b975a6ae539 286 {
randrews33 0:1b975a6ae539 287 // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
randrews33 0:1b975a6ae539 288 uint8_t temp = xmReadByte(CTRL_REG2_XM);
randrews33 0:1b975a6ae539 289 // Then mask out the accel scale bits:
randrews33 0:1b975a6ae539 290 temp &= 0xFF^(0x3 << 3);
randrews33 0:1b975a6ae539 291 // Then shift in our new scale bits:
randrews33 0:1b975a6ae539 292 temp |= aScl << 3;
randrews33 0:1b975a6ae539 293 // And write the new register value back into CTRL_REG2_XM:
randrews33 0:1b975a6ae539 294 xmWriteByte(CTRL_REG2_XM, temp);
randrews33 0:1b975a6ae539 295
randrews33 0:1b975a6ae539 296 // We've updated the sensor, but we also need to update our class variables
randrews33 0:1b975a6ae539 297 // First update aScale:
randrews33 0:1b975a6ae539 298 aScale = aScl;
randrews33 0:1b975a6ae539 299 // Then calculate a new aRes, which relies on aScale being set correctly:
randrews33 0:1b975a6ae539 300 calcaRes();
randrews33 0:1b975a6ae539 301 }
randrews33 0:1b975a6ae539 302
randrews33 0:1b975a6ae539 303 void LSM9DS0::setMagScale(mag_scale mScl)
randrews33 0:1b975a6ae539 304 {
randrews33 0:1b975a6ae539 305 // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
randrews33 0:1b975a6ae539 306 uint8_t temp = xmReadByte(CTRL_REG6_XM);
randrews33 0:1b975a6ae539 307 // Then mask out the mag scale bits:
randrews33 0:1b975a6ae539 308 temp &= 0xFF^(0x3 << 5);
randrews33 0:1b975a6ae539 309 // Then shift in our new scale bits:
randrews33 0:1b975a6ae539 310 temp |= mScl << 5;
randrews33 0:1b975a6ae539 311 // And write the new register value back into CTRL_REG6_XM:
randrews33 0:1b975a6ae539 312 xmWriteByte(CTRL_REG6_XM, temp);
randrews33 0:1b975a6ae539 313
randrews33 0:1b975a6ae539 314 // We've updated the sensor, but we also need to update our class variables
randrews33 0:1b975a6ae539 315 // First update mScale:
randrews33 0:1b975a6ae539 316 mScale = mScl;
randrews33 0:1b975a6ae539 317 // Then calculate a new mRes, which relies on mScale being set correctly:
randrews33 0:1b975a6ae539 318 calcmRes();
randrews33 0:1b975a6ae539 319 }
randrews33 0:1b975a6ae539 320
randrews33 0:1b975a6ae539 321 void LSM9DS0::setGyroODR(gyro_odr gRate)
randrews33 0:1b975a6ae539 322 {
randrews33 0:1b975a6ae539 323 // We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
randrews33 0:1b975a6ae539 324 uint8_t temp = gReadByte(CTRL_REG1_G);
randrews33 0:1b975a6ae539 325 // Then mask out the gyro ODR bits:
randrews33 0:1b975a6ae539 326 temp &= 0xFF^(0xF << 4);
randrews33 0:1b975a6ae539 327 // Then shift in our new ODR bits:
randrews33 0:1b975a6ae539 328 temp |= (gRate << 4);
randrews33 0:1b975a6ae539 329 // And write the new register value back into CTRL_REG1_G:
randrews33 0:1b975a6ae539 330 gWriteByte(CTRL_REG1_G, temp);
randrews33 0:1b975a6ae539 331 }
randrews33 0:1b975a6ae539 332 void LSM9DS0::setAccelODR(accel_odr aRate)
randrews33 0:1b975a6ae539 333 {
randrews33 0:1b975a6ae539 334 // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
randrews33 0:1b975a6ae539 335 uint8_t temp = xmReadByte(CTRL_REG1_XM);
randrews33 0:1b975a6ae539 336 // Then mask out the accel ODR bits:
randrews33 0:1b975a6ae539 337 temp &= 0xFF^(0xF << 4);
randrews33 0:1b975a6ae539 338 // Then shift in our new ODR bits:
randrews33 0:1b975a6ae539 339 temp |= (aRate << 4);
randrews33 0:1b975a6ae539 340 // And write the new register value back into CTRL_REG1_XM:
randrews33 0:1b975a6ae539 341 xmWriteByte(CTRL_REG1_XM, temp);
randrews33 0:1b975a6ae539 342 }
randrews33 0:1b975a6ae539 343 void LSM9DS0::setMagODR(mag_odr mRate)
randrews33 0:1b975a6ae539 344 {
randrews33 0:1b975a6ae539 345 // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
randrews33 0:1b975a6ae539 346 uint8_t temp = xmReadByte(CTRL_REG5_XM);
randrews33 0:1b975a6ae539 347 // Then mask out the mag ODR bits:
randrews33 0:1b975a6ae539 348 temp &= 0xFF^(0x7 << 2);
randrews33 0:1b975a6ae539 349 // Then shift in our new ODR bits:
randrews33 0:1b975a6ae539 350 temp |= (mRate << 2);
randrews33 0:1b975a6ae539 351 // And write the new register value back into CTRL_REG5_XM:
randrews33 0:1b975a6ae539 352 xmWriteByte(CTRL_REG5_XM, temp);
randrews33 0:1b975a6ae539 353 }
randrews33 0:1b975a6ae539 354
randrews33 0:1b975a6ae539 355 void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
randrews33 0:1b975a6ae539 356 {
randrews33 0:1b975a6ae539 357 gWriteByte(INT1_CFG_G, int1Cfg);
randrews33 0:1b975a6ae539 358 gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
randrews33 0:1b975a6ae539 359 gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF));
randrews33 0:1b975a6ae539 360 gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
randrews33 0:1b975a6ae539 361 gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF));
randrews33 0:1b975a6ae539 362 gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
randrews33 0:1b975a6ae539 363 gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF));
randrews33 0:1b975a6ae539 364 if (duration)
randrews33 0:1b975a6ae539 365 gWriteByte(INT1_DURATION_G, 0x80 | duration);
randrews33 0:1b975a6ae539 366 else
randrews33 0:1b975a6ae539 367 gWriteByte(INT1_DURATION_G, 0x00);
randrews33 0:1b975a6ae539 368 }
randrews33 0:1b975a6ae539 369
randrews33 0:1b975a6ae539 370 void LSM9DS0::calcgRes()
randrews33 0:1b975a6ae539 371 {
randrews33 0:1b975a6ae539 372 // Possible gyro scales (and their register bit settings) are:
randrews33 0:1b975a6ae539 373 // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
randrews33 0:1b975a6ae539 374 // to calculate DPS/(ADC tick) based on that 2-bit value:
randrews33 0:1b975a6ae539 375 switch (gScale)
randrews33 0:1b975a6ae539 376 {
randrews33 0:1b975a6ae539 377 case G_SCALE_245DPS:
randrews33 0:1b975a6ae539 378 gRes = 245.0 / 32768.0;
randrews33 0:1b975a6ae539 379 break;
randrews33 0:1b975a6ae539 380 case G_SCALE_500DPS:
randrews33 0:1b975a6ae539 381 gRes = 500.0 / 32768.0;
randrews33 0:1b975a6ae539 382 break;
randrews33 0:1b975a6ae539 383 case G_SCALE_2000DPS:
randrews33 0:1b975a6ae539 384 gRes = 2000.0 / 32768.0;
randrews33 0:1b975a6ae539 385 break;
randrews33 0:1b975a6ae539 386 }
randrews33 0:1b975a6ae539 387 }
randrews33 0:1b975a6ae539 388
randrews33 0:1b975a6ae539 389 void LSM9DS0::calcaRes()
randrews33 0:1b975a6ae539 390 {
randrews33 0:1b975a6ae539 391 // Possible accelerometer scales (and their register bit settings) are:
randrews33 0:1b975a6ae539 392 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an
randrews33 0:1b975a6ae539 393 // algorithm to calculate g/(ADC tick) based on that 3-bit value:
randrews33 0:1b975a6ae539 394 aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 :
randrews33 0:1b975a6ae539 395 (((float) aScale + 1.0) * 2.0) / 32768.0;
randrews33 0:1b975a6ae539 396 }
randrews33 0:1b975a6ae539 397
randrews33 0:1b975a6ae539 398 void LSM9DS0::calcmRes()
randrews33 0:1b975a6ae539 399 {
randrews33 0:1b975a6ae539 400 // Possible magnetometer scales (and their register bit settings) are:
randrews33 0:1b975a6ae539 401 // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
randrews33 0:1b975a6ae539 402 // to calculate Gs/(ADC tick) based on that 2-bit value:
randrews33 0:1b975a6ae539 403 mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 :
randrews33 0:1b975a6ae539 404 (float) (mScale << 2) / 32768.0;
randrews33 0:1b975a6ae539 405 }
randrews33 0:1b975a6ae539 406
randrews33 0:1b975a6ae539 407 void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
randrews33 0:1b975a6ae539 408 {
randrews33 0:1b975a6ae539 409 // Whether we're using I2C or SPI, write a byte using the
randrews33 0:1b975a6ae539 410 // gyro-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 411 I2CwriteByte(gAddress, subAddress, data);
randrews33 0:1b975a6ae539 412 }
randrews33 0:1b975a6ae539 413
randrews33 0:1b975a6ae539 414 void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
randrews33 0:1b975a6ae539 415 {
randrews33 0:1b975a6ae539 416 // Whether we're using I2C or SPI, write a byte using the
randrews33 0:1b975a6ae539 417 // accelerometer-specific I2C address or SPI CS pin.
randrews33 0:1b975a6ae539 418 return I2CwriteByte(xmAddress, subAddress, data);
randrews33 0:1b975a6ae539 419 }
randrews33 0:1b975a6ae539 420
randrews33 0:1b975a6ae539 421 uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
randrews33 0:1b975a6ae539 422 {
randrews33 0:1b975a6ae539 423 return I2CreadByte(gAddress, subAddress);
randrews33 0:1b975a6ae539 424 }
randrews33 0:1b975a6ae539 425
randrews33 0:1b975a6ae539 426 void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
randrews33 0:1b975a6ae539 427 {
randrews33 0:1b975a6ae539 428 // Whether we're using I2C or SPI, read multiple bytes using the
randrews33 4:bf8f4e7c9905 429 // gyro-specific I2C address.
randrews33 0:1b975a6ae539 430 I2CreadBytes(gAddress, subAddress, dest, count);
randrews33 0:1b975a6ae539 431 }
randrews33 0:1b975a6ae539 432
randrews33 0:1b975a6ae539 433 uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
randrews33 0:1b975a6ae539 434 {
randrews33 0:1b975a6ae539 435 // Whether we're using I2C or SPI, read a byte using the
randrews33 4:bf8f4e7c9905 436 // accelerometer-specific I2C address.
randrews33 0:1b975a6ae539 437 return I2CreadByte(xmAddress, subAddress);
randrews33 0:1b975a6ae539 438 }
randrews33 0:1b975a6ae539 439
randrews33 0:1b975a6ae539 440 void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
randrews33 0:1b975a6ae539 441 {
randrews33 4:bf8f4e7c9905 442 // read multiple bytes using the
randrews33 4:bf8f4e7c9905 443 // accelerometer-specific I2C address.
randrews33 4:bf8f4e7c9905 444 I2CreadBytes(xmAddress, subAddress, dest, count);
randrews33 0:1b975a6ae539 445 }
randrews33 0:1b975a6ae539 446
randrews33 0:1b975a6ae539 447
randrews33 0:1b975a6ae539 448 void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
randrews33 4:bf8f4e7c9905 449 {
randrews33 0:1b975a6ae539 450 i2c_->writeByte(address,subAddress,data);
randrews33 0:1b975a6ae539 451 }
randrews33 0:1b975a6ae539 452
randrews33 0:1b975a6ae539 453 uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
randrews33 0:1b975a6ae539 454 {
randrews33 0:1b975a6ae539 455 char data[1]; // `data` will store the register data
randrews33 0:1b975a6ae539 456
randrews33 0:1b975a6ae539 457 I2CreadBytes(address, subAddress,(uint8_t*)data, 1);
randrews33 0:1b975a6ae539 458 return (uint8_t)data[0];
randrews33 0:1b975a6ae539 459
randrews33 0:1b975a6ae539 460 }
randrews33 0:1b975a6ae539 461
randrews33 0:1b975a6ae539 462 void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest,
randrews33 0:1b975a6ae539 463 uint8_t count)
randrews33 4:bf8f4e7c9905 464 {
randrews33 0:1b975a6ae539 465 i2c_->readBytes(address, subAddress, count, dest);
randrews33 0:1b975a6ae539 466 }