James Nagendran / Mbed 2 deprecated 4180_final_receiver

Receiver code for SLVM

Dependencies:   mbed-rtos mbed

LSM9DS0.cpp@0:fd289b2e6b74, 2014-12-09 (annotated)

Committer:
jnagendran3
Date:
Tue Dec 09 01:15:37 2014 +0000
Revision:
0:fd289b2e6b74
first

Who changed what in which revision?

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