Xiaofei Qiu / Mbed 2 deprecated LSM9DS0

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of LSM9DS0 by Taylor Andrews

LSM9DS0.cpp@7:8d8f4c6c511d, 2015-11-28 (annotated)

Committer:
Xiaofei
Date:
Sat Nov 28 02:34:59 2015 +0000
Revision:
7:8d8f4c6c511d
Parent:
4:bf8f4e7c9905 
C

Who changed what in which revision?

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