Daniel Begasse / Mbed 2 deprecated ECE4180_lab4

This is our rendition of popular labyrinth games. We use readings from the IMU accelerometer to move a little yellow ball around the screen. We have created a maze of walls and holes that will end the game if the user runs into either of these.

Dependencies:   4DGL-uLCD-SE mbed

LSM9DS0.cpp@1:2250e33823e2, 2015-10-20 (annotated)

Committer:
dbegasse
Date:
Tue Oct 20 18:34:56 2015 +0000
Revision:
1:2250e33823e2
Parent:
0:7b4bbd744f6d 
ECE4180 Lab 4 to_publish

Who changed what in which revision?

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